From b03dc4d73357854c9c2fac4c617b7dae16d128d8 Mon Sep 17 00:00:00 2001 From: dominiho Date: Thu, 14 Aug 2014 17:14:29 +0200 Subject: [PATCH 01/14] -added micro second counter to increase precision -enabled low light mode without HDR but increased desired brightness -extended I2C frame with accumulated flow/gyro values between readouts --- Makefile | 2 +- inc/i2c_frame.h | 28 ++++++++- inc/main.h | 2 + src/communication.c | 4 +- src/dcmi.c | 6 +- src/flow.c | 4 +- src/i2c.c | 135 +++++++++++++++++++++++++++++++++----------- src/main.c | 67 ++++++++++++++++------ src/mt9v034.c | 12 ++-- src/sonar.c | 8 +-- 10 files changed, 197 insertions(+), 71 deletions(-) diff --git a/Makefile b/Makefile index fb66122..f82ad6c 100644 --- a/Makefile +++ b/Makefile @@ -57,7 +57,7 @@ SRCS += lib/STM32F4xx_StdPeriph_Driver/src/misc.c \ .PHONY: clean upload-usb CFLAGS = -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 \ - -O1 -g \ + -O3 \ -std=gnu99 \ -Wall \ -MD \ diff --git a/inc/i2c_frame.h b/inc/i2c_frame.h index dff9d84..14ddbf2 100644 --- a/inc/i2c_frame.h +++ b/inc/i2c_frame.h @@ -32,8 +32,11 @@ #define I2C_FRAME_H_ #include -#define I2C_FRAME_SIZE (sizeof(i2c_frame)) -typedef struct i2c_frame +// ***Take care***: struct alignment isn't necessarily what you expect, +// so unaligned (i.e., single uint8_t values) should go at the end. +// Otherwise nothing will work. + +typedef struct i2c_frame { uint16_t frame_count; int16_t pixel_flow_x_sum; @@ -47,6 +50,25 @@ typedef struct i2c_frame uint8_t gyro_range; uint8_t sonar_timestamp; int16_t ground_distance; -} i2c_frame; +} __attribute__((packed)) i2c_frame; + +#define I2C_FRAME_SIZE (sizeof(i2c_frame)) + + +typedef struct i2c_integral_frame +{ + uint16_t frame_count_since_last_readout; + int16_t pixel_flow_x_integral; + int16_t pixel_flow_y_integral; + int16_t gyro_x_rate_integral; + int16_t gyro_y_rate_integral; + int16_t gyro_z_rate_integral; + uint32_t time_since_last_readout; + uint32_t sonar_timestamp; + int16_t ground_distance; + uint8_t gyro_range; +} __attribute__((packed)) i2c_integral_frame; + +#define I2C_INTEGRAL_FRAME_SIZE (sizeof(i2c_integral_frame)) #endif /* I2C_FRAME_H_ */ diff --git a/inc/main.h b/inc/main.h index d124c65..2abc9e7 100644 --- a/inc/main.h +++ b/inc/main.h @@ -37,6 +37,8 @@ extern uint32_t get_time_between_images(void); void timer_update(void); +void timer_update_ms(void); uint32_t get_boot_time_ms(void); +uint32_t get_boot_time_us(void); #endif /* __PX4_FLOWBOARD_H */ diff --git a/src/communication.c b/src/communication.c index 1e39fe6..97d45fe 100644 --- a/src/communication.c +++ b/src/communication.c @@ -47,7 +47,7 @@ #include "debug.h" #include "communication.h" -extern uint32_t get_boot_time_ms(); +extern uint32_t get_boot_time_us(); extern void buffer_reset(); extern void systemreset(bool to_bootloader); @@ -308,7 +308,7 @@ void handle_mavlink_message(mavlink_channel_t chan, if (ping.target_system == 0 && ping.target_component == 0) { /* Respond to ping */ - uint64_t r_timestamp = get_boot_time_ms() * 1000; + uint64_t r_timestamp = get_boot_time_us(); mavlink_msg_ping_send(chan, ping.seq, msg->sysid, msg->compid, r_timestamp); } } diff --git a/src/dcmi.c b/src/dcmi.c index 63a83d1..53868e6 100644 --- a/src/dcmi.c +++ b/src/dcmi.c @@ -73,7 +73,7 @@ uint8_t dcmi_image_buffer_8bit_3[FULL_IMAGE_SIZE]; uint32_t time_between_images; /* extern functions */ -extern uint32_t get_boot_time_ms(void); +extern uint32_t get_boot_time_us(void); extern void delay(unsigned msec); /** @@ -202,8 +202,8 @@ void dma_swap_buffers(void) } /* set next time_between_images */ - cycle_time = get_boot_time_ms() - time_last_frame; - time_last_frame = get_boot_time_ms(); + cycle_time = get_boot_time_us() - time_last_frame; + time_last_frame = get_boot_time_us(); if(image_counter) // image was not fetched jet { diff --git a/src/flow.c b/src/flow.c index 78c0611..b485850 100644 --- a/src/flow.c +++ b/src/flow.c @@ -676,7 +676,7 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat if(fabsf(y_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) { /* calc pixel of gyro */ - float y_rate_pixel = y_rate * (get_time_between_images() / 1000.0f) * focal_length_px; + float y_rate_pixel = y_rate * (get_time_between_images() / 1000000.0f) * focal_length_px; float comp_x = histflowx + y_rate_pixel; /* clamp value to maximum search window size plus half pixel from subpixel search */ @@ -695,7 +695,7 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat if(fabsf(x_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) { /* calc pixel of gyro */ - float x_rate_pixel = x_rate * (get_time_between_images() / 1000.0f) * focal_length_px; + float x_rate_pixel = x_rate * (get_time_between_images() / 1000000.0f) * focal_length_px; float comp_y = histflowy - x_rate_pixel; /* clamp value to maximum search window size plus/minus half pixel from subpixel search */ diff --git a/src/i2c.c b/src/i2c.c index d3b06bd..8fec2fb 100644 --- a/src/i2c.c +++ b/src/i2c.c @@ -42,6 +42,12 @@ #include "main.h" static char offset = 0; +uint8_t dataRX=0; +uint8_t txData[2][I2C_FRAME_SIZE+I2C_INTEGRAL_FRAME_SIZE]; +uint8_t publishedIndex = 0; +uint8_t readout_done = 0; +uint8_t stop_accumulation = 0; + void i2c_init() { @@ -107,13 +113,10 @@ void i2c_init() I2C_Cmd(I2C1, ENABLE); } -uint8_t txData[2][I2C_FRAME_SIZE]; -uint8_t txBufferIndex = 0; - void I2C1_EV_IRQHandler(void) { - uint8_t dataRX; + //uint8_t dataRX; static uint8_t txDataIndex = 0x00; static uint8_t rxDataIndex = 0x00; switch (I2C_GetLastEvent(I2C1 )) @@ -130,20 +133,30 @@ void I2C1_EV_IRQHandler(void) { I2C1 ->SR1; I2C1 ->SR2; - txDataIndex = 0; break; } case I2C_EVENT_SLAVE_BYTE_RECEIVED : { + //receive address offset dataRX = I2C_ReceiveData(I2C1 ); rxDataIndex++; + //reset Index and indicate sending + txDataIndex = 0; + readout_done= 0; break; } case I2C_EVENT_SLAVE_BYTE_TRANSMITTING : case I2C_EVENT_SLAVE_BYTE_TRANSMITTED : { - I2C_SendData(I2C1, txData[txBufferIndex][txDataIndex]); + //todo check overrun condition + I2C_SendData(I2C1, txData[publishedIndex][dataRX+txDataIndex]); txDataIndex++; + + //check whether last byte is read what indicates readout of integral frame and force reset of accumulation + if(dataRX+txDataIndex>=(I2C_FRAME_SIZE+I2C_INTEGRAL_FRAME_SIZE)){ + readout_done=1; + } + break; } @@ -182,43 +195,101 @@ void update_TX_buffer(float pixel_flow_x_sum, float pixel_flow_y_sum, float flow { i2c_frame f; char c[I2C_FRAME_SIZE]; - } u[2]; + } u; + + u.f.frame_count = frame_count; + u.f.pixel_flow_x_sum = pixel_flow_x_sum; + u.f.pixel_flow_y_sum = pixel_flow_y_sum; + u.f.flow_comp_m_x = flow_comp_m_x * 1000; + u.f.flow_comp_m_y = flow_comp_m_y * 1000; + u.f.qual = qual; + u.f.ground_distance = ground_distance * 1000; + + u.f.gyro_x_rate = gyro_x_rate * getGyroScalingFactor(); + u.f.gyro_y_rate = gyro_y_rate * getGyroScalingFactor(); + u.f.gyro_z_rate = gyro_z_rate * getGyroScalingFactor(); + u.f.gyro_range = getGyroRange(); + + + uint32_t time_since_last_sonar_update; - int nrxBufferIndex = 1 - txBufferIndex; + time_since_last_sonar_update = (get_boot_time_us() - get_sonar_measure_time()); - u[nrxBufferIndex].f.frame_count = frame_count; - u[nrxBufferIndex].f.pixel_flow_x_sum = pixel_flow_x_sum; - u[nrxBufferIndex].f.pixel_flow_y_sum = pixel_flow_y_sum; - u[nrxBufferIndex].f.flow_comp_m_x = flow_comp_m_x * 1000; - u[nrxBufferIndex].f.flow_comp_m_y = flow_comp_m_y * 1000; - u[nrxBufferIndex].f.qual = qual; - u[nrxBufferIndex].f.ground_distance = ground_distance * 1000; - u[nrxBufferIndex].f.gyro_x_rate = gyro_x_rate * getGyroScalingFactor(); - u[nrxBufferIndex].f.gyro_y_rate = gyro_y_rate * getGyroScalingFactor(); - u[nrxBufferIndex].f.gyro_z_rate = gyro_z_rate * getGyroScalingFactor(); - u[nrxBufferIndex].f.gyro_range = getGyroRange(); + if (time_since_last_sonar_update < 255*1000){ + u.f.sonar_timestamp = time_since_last_sonar_update/1000;//convert to ms + } + else{ + u.f.sonar_timestamp = 255; + } - uint32_t sonar_time_interrupt = get_sonar_measure_time_interrupt(); - uint32_t sonar_time = get_sonar_measure_time(); - uint32_t time; - if (sonar_time < sonar_time_interrupt) - time = sonar_time; - else - time = sonar_time_interrupt; + union + { + i2c_integral_frame f; + char c_integral[I2C_INTEGRAL_FRAME_SIZE]; + } u_integral; + + static uint16_t accumulated_flow_x = 0; + static uint16_t accumulated_flow_y = 0; + static uint16_t accumulated_framecount = 0; + static int16_t accumulated_gyro_x = 0; + static int16_t accumulated_gyro_y = 0; + static int16_t accumulated_gyro_z = 0; + static uint32_t time_between_readings = 0; + static uint32_t lasttime = 0; + + + //accumulate flow and gyro values between sucessive I2C readings + if(stop_accumulation==0 && readout_done==1){ + // reset if readout has been performed + time_between_readings = time_between_readings/accumulated_framecount; //fill in minimal time between frames + accumulated_flow_x = pixel_flow_x_sum; + accumulated_flow_y = pixel_flow_y_sum; + accumulated_framecount = 1; + accumulated_gyro_x = gyro_x_rate * getGyroScalingFactor(); + accumulated_gyro_y = gyro_y_rate * getGyroScalingFactor(); + accumulated_gyro_z = gyro_z_rate * getGyroScalingFactor(); + } + else{ + // accumulate if there is no readout + time_between_readings += (get_boot_time_us()-lasttime); + accumulated_flow_x += pixel_flow_x_sum; + accumulated_flow_y += pixel_flow_y_sum; + accumulated_framecount++; + accumulated_gyro_x += gyro_x_rate * getGyroScalingFactor(); + accumulated_gyro_y += gyro_y_rate * getGyroScalingFactor(); + accumulated_gyro_z += gyro_z_rate * getGyroScalingFactor(); + } - time = get_boot_time_ms() - time; + lasttime = get_boot_time_us(); + stop_accumulation=readout_done; - if (time > 255) - time = 255; + u_integral.f.frame_count_since_last_readout = accumulated_framecount; + u_integral.f.gyro_x_rate_integral =accumulated_gyro_x; + u_integral.f.gyro_y_rate_integral =accumulated_gyro_y; + u_integral.f.gyro_z_rate_integral =accumulated_gyro_z; + u_integral.f.gyro_range = getGyroRange(); + u_integral.f.pixel_flow_x_integral =accumulated_flow_x; + u_integral.f.pixel_flow_y_integral =accumulated_flow_y; + u_integral.f.time_since_last_readout = time_between_readings; + u_integral.f.ground_distance = ground_distance * 1000; + u_integral.f.sonar_timestamp = time_since_last_sonar_update; - u[nrxBufferIndex].f.sonar_timestamp = time; + int notpublishedIndex = 1 - publishedIndex; // choose not the current published buffer + // fill I2C transmitbuffer with values for (i = 0; i < I2C_FRAME_SIZE; i++) - txData[txBufferIndex][i] = u[nrxBufferIndex].c[i]; + txData[notpublishedIndex][i] = u.c[i]; + + for (i = I2C_FRAME_SIZE; i < I2C_INTEGRAL_FRAME_SIZE+I2C_FRAME_SIZE; i++) + txData[notpublishedIndex][i] = u_integral.c_integral[i-I2C_FRAME_SIZE]; + + + if(readout_done){ + publishedIndex = 1 -publishedIndex; //swap buffers if I2C bus is idle + } - txBufferIndex = 1 - txBufferIndex; frame_count++; } diff --git a/src/main.c b/src/main.c index e0e2a44..c9eccd6 100644 --- a/src/main.c +++ b/src/main.c @@ -75,8 +75,10 @@ uint8_t* image_buffer_8bit_1 = ((uint8_t*) 0x10000000); uint8_t* image_buffer_8bit_2 = ((uint8_t*) ( 0x10000000 | FULL_IMAGE_SIZE )); uint8_t buffer_reset_needed; -/* boot time in milli seconds */ +/* boot time in milliseconds ticks */ volatile uint32_t boot_time_ms = 0; +/* boot time in 10 microseconds ticks */ +volatile uint32_t boot_time10_us = 0; /* timer constants */ #define NTIMERS 8 @@ -88,11 +90,13 @@ volatile uint32_t boot_time_ms = 0; #define TIMER_RECEIVE 5 #define TIMER_PARAMS 6 #define TIMER_IMAGE 7 -#define LED_TIMER_COUNT 500 -#define SONAR_TIMER_COUNT 100 -#define SYSTEM_STATE_COUNT 1000 -#define PARAMS_COUNT 100 +#define MS_TIMER_COUNT 100 /* steps in 10 microseconds ticks */ +#define LED_TIMER_COUNT 500 /* steps in milliseconds ticks */ +#define SONAR_TIMER_COUNT 100 /* steps in milliseconds ticks */ +#define SYSTEM_STATE_COUNT 1000/* steps in milliseconds ticks */ +#define PARAMS_COUNT 100 /* steps in milliseconds ticks */ static volatile unsigned timer[NTIMERS]; +static volatile unsigned timer_ms = MS_TIMER_COUNT; /* timer/system booleans */ bool send_system_state_now = true; @@ -101,11 +105,11 @@ bool send_params_now = true; bool send_image_now = true; /** - * @brief Increment boot_time variable and decrement timer array. + * @brief Increment boot_time_ms variable and decrement timer array. * @param None * @retval None */ -void timer_update(void) +void timer_update_ms(void) { boot_time_ms++; @@ -114,6 +118,7 @@ void timer_update(void) if (timer[i] > 0) timer[i]--; + if (timer[TIMER_LED] == 0) { /* blink activitiy */ @@ -152,11 +157,37 @@ void timer_update(void) } } +/** + * @brief Increment boot_time10_us variable and decrement millisecond timer, triggered by timer interrupt + * @param None + * @retval None + */ +void timer_update(void) +{ + boot_time10_us++; + + /* decrements every 10 microseconds*/ + timer_ms--; + + if (timer_ms == 0) + { + timer_update_ms(); + timer_ms = MS_TIMER_COUNT; + } + +} + + uint32_t get_boot_time_ms(void) { return boot_time_ms; } +uint32_t get_boot_time_us(void) +{ + return boot_time10_us*10;// *10 to return microseconds +} + void delay(unsigned msec) { timer[TIMER_DELAY] = msec; @@ -188,7 +219,7 @@ int main(void) SCB_CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); /* set CP10 Full Access and set CP11 Full Access */ /* init clock */ - if (SysTick_Config(SystemCoreClock / 1000)) + if (SysTick_Config(SystemCoreClock / 100000))/*set timer to trigger interrupt every 10 microsecond */ { /* capture clock error */ LEDOn(LED_ERR); @@ -321,8 +352,8 @@ int main(void) const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 6.0f) * 1000.0f; //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled /* debug */ - float x_rate_pixel = x_rate * (get_time_between_images() / 1000.0f) * focal_length_px; - float y_rate_pixel = y_rate * (get_time_between_images() / 1000.0f) * focal_length_px; + float x_rate_pixel = x_rate * (get_time_between_images() / 1000000.0f) * focal_length_px; + float y_rate_pixel = y_rate * (get_time_between_images() / 1000000.0f) * focal_length_px; //FIXME for the old sensor PX4FLOW v1.2 uncomment this!!!! // x_rate = x_rate_raw_sensor; // change x and y rates @@ -356,8 +387,8 @@ int main(void) * x / f = X / Z * y / f = Y / Z */ - float flow_compx = pixel_flow_x / focal_length_px / (get_time_between_images() / 1000.0f); - float flow_compy = pixel_flow_y / focal_length_px / (get_time_between_images() / 1000.0f); + float flow_compx = pixel_flow_x / focal_length_px / (get_time_between_images() / 1000000.0f); + float flow_compy = pixel_flow_y / focal_length_px / (get_time_between_images() / 1000000.0f); /* integrate velocity and output values only if distance is valid */ if (distance_valid) @@ -406,7 +437,7 @@ int main(void) if(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM) { /* send bottom flow if activated */ - if (counter % 2 == 0) + if (counter % 1 == 0) { float flow_comp_m_x = 0.0f; float flow_comp_m_y = 0.0f; @@ -430,12 +461,12 @@ int main(void) if (valid_frame_count > 0) { // send flow - mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_ms() * 1000, global_data.param[PARAM_SENSOR_ID], + mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, flow_comp_m_x, flow_comp_m_y, qual, ground_distance); if (global_data.param[PARAM_USB_SEND_FLOW]) - mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_ms() * 1000, global_data.param[PARAM_SENSOR_ID], + mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, flow_comp_m_x, flow_comp_m_y, qual, ground_distance); @@ -446,12 +477,12 @@ int main(void) else { // send distance - mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_ms() * 1000, global_data.param[PARAM_SENSOR_ID], + mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, 0.0f, 0.0f, 0, ground_distance); if (global_data.param[PARAM_USB_SEND_FLOW]) - mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_ms() * 1000, global_data.param[PARAM_SENSOR_ID], + mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, 0.0f, 0.0f, 0, ground_distance); @@ -461,7 +492,7 @@ int main(void) if(global_data.param[PARAM_USB_SEND_GYRO]) { - mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_ms() * 1000, x_rate, y_rate, z_rate); + mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_us(), x_rate, y_rate, z_rate); } velocity_x_sum = 0.0f; diff --git a/src/mt9v034.c b/src/mt9v034.c index 304dad4..7be73b8 100644 --- a/src/mt9v034.c +++ b/src/mt9v034.c @@ -126,10 +126,10 @@ void mt9v034_context_configuration(void) if(global_data.param[PARAM_IMAGE_LOW_LIGHT]) { min_exposure = 0x0001; - max_exposure = 0x0060; - desired_brightness = 30; // VALID RANGE: 1-64 - resolution_ctrl = 0x0303; - hdr_enabled = 0x0101; // on + max_exposure = 0x0040; + desired_brightness = 58; // VALID RANGE: 8-64 + resolution_ctrl = 0x0202;//10 bit linear + hdr_enabled = 0x0000; // off aec_agc_enabled = 0x0303; // on coarse_sw1 = 0x01BB; // default from context A coarse_sw2 = 0x01D9; // default from context A @@ -140,8 +140,8 @@ void mt9v034_context_configuration(void) { min_exposure = 0x0001; max_exposure = 0x0040; - desired_brightness = 50; // VALID RANGE: 1-64 - resolution_ctrl = 0x0202; + desired_brightness = 16; // VALID RANGE: 8-64 + resolution_ctrl = 0x0202;//10bit linear hdr_enabled = 0x0000; // off aec_agc_enabled = 0x0303; // on coarse_sw1 = 0x01BB; // default from context A diff --git a/src/sonar.c b/src/sonar.c index 64e2114..eceb33f 100644 --- a/src/sonar.c +++ b/src/sonar.c @@ -50,7 +50,7 @@ #include "sonar.h" extern int atoi (__const char *__nptr); -extern uint32_t get_boot_time_ms(void); +extern uint32_t get_boot_time_us(void); static char data_buffer[5]; // array for collecting decoded data @@ -123,9 +123,9 @@ void UART4_IRQHandler(void) { /* it is in normal sensor range, take it */ last_measure_time = measure_time; - measure_time = get_boot_time_ms(); + measure_time = get_boot_time_us(); sonar_measure_time_interrupt = measure_time; - dt = ((float)(measure_time - last_measure_time)) / 1000.0f; + dt = ((float)(measure_time - last_measure_time)) / 1000000.0f; valid_data = temp; new_value = 1; @@ -175,7 +175,7 @@ void sonar_read(float* sonar_value_filtered, float* sonar_value_raw) if(new_value) { sonar_filter(); new_value = 0; - sonar_measure_time = get_boot_time_ms(); + sonar_measure_time = get_boot_time_us(); } *sonar_value_filtered = x_post; From 9f143d73199eaddad07a357c66391b877504df63 Mon Sep 17 00:00:00 2001 From: dominiho Date: Mon, 18 Aug 2014 18:54:44 +0200 Subject: [PATCH 02/14] readout check of new I2C frame prohibits compatibility with old version, implemented 2 checks, compatible with old I2C readout know --- src/i2c.c | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/src/i2c.c b/src/i2c.c index 8fec2fb..6bdb7b7 100644 --- a/src/i2c.c +++ b/src/i2c.c @@ -45,7 +45,8 @@ static char offset = 0; uint8_t dataRX=0; uint8_t txData[2][I2C_FRAME_SIZE+I2C_INTEGRAL_FRAME_SIZE]; uint8_t publishedIndex = 0; -uint8_t readout_done = 0; +uint8_t readout_done_frame1 = 0; +uint8_t readout_done_frame2 = 0; uint8_t stop_accumulation = 0; @@ -142,7 +143,8 @@ void I2C1_EV_IRQHandler(void) rxDataIndex++; //reset Index and indicate sending txDataIndex = 0; - readout_done= 0; + readout_done_frame1 = 0; + readout_done_frame2 = 0; break; } case I2C_EVENT_SLAVE_BYTE_TRANSMITTING : @@ -153,8 +155,12 @@ void I2C1_EV_IRQHandler(void) txDataIndex++; //check whether last byte is read what indicates readout of integral frame and force reset of accumulation + if(dataRX+txDataIndex>=(I2C_FRAME_SIZE)){ + readout_done_frame1=1; + } + if(dataRX+txDataIndex>=(I2C_FRAME_SIZE+I2C_INTEGRAL_FRAME_SIZE)){ - readout_done=1; + readout_done_frame2=1; } break; @@ -241,7 +247,7 @@ void update_TX_buffer(float pixel_flow_x_sum, float pixel_flow_y_sum, float flow //accumulate flow and gyro values between sucessive I2C readings - if(stop_accumulation==0 && readout_done==1){ + if(stop_accumulation==0 && readout_done_frame2==1){ // reset if readout has been performed time_between_readings = time_between_readings/accumulated_framecount; //fill in minimal time between frames accumulated_flow_x = pixel_flow_x_sum; @@ -263,7 +269,7 @@ void update_TX_buffer(float pixel_flow_x_sum, float pixel_flow_y_sum, float flow } lasttime = get_boot_time_us(); - stop_accumulation=readout_done; + stop_accumulation=readout_done_frame2; u_integral.f.frame_count_since_last_readout = accumulated_framecount; u_integral.f.gyro_x_rate_integral =accumulated_gyro_x; @@ -279,14 +285,15 @@ void update_TX_buffer(float pixel_flow_x_sum, float pixel_flow_y_sum, float flow int notpublishedIndex = 1 - publishedIndex; // choose not the current published buffer // fill I2C transmitbuffer with values - for (i = 0; i < I2C_FRAME_SIZE; i++) + for (i = 0; i < I2C_FRAME_SIZE; i++){ txData[notpublishedIndex][i] = u.c[i]; + } for (i = I2C_FRAME_SIZE; i < I2C_INTEGRAL_FRAME_SIZE+I2C_FRAME_SIZE; i++) txData[notpublishedIndex][i] = u_integral.c_integral[i-I2C_FRAME_SIZE]; - if(readout_done){ + if(readout_done_frame1){ //checks frame1 only! todo implement both checks while maintaining compatibility publishedIndex = 1 -publishedIndex; //swap buffers if I2C bus is idle } From 1d3fd6ec578951448713bc29ef27c284821d9f5b Mon Sep 17 00:00:00 2001 From: dominiho Date: Tue, 19 Aug 2014 09:53:24 +0200 Subject: [PATCH 03/14] I2C frame1 and frame2 are now totally independent, both protected during readout --- src/i2c.c | 39 +++++++++++++++++++++++++++++---------- 1 file changed, 29 insertions(+), 10 deletions(-) diff --git a/src/i2c.c b/src/i2c.c index 6bdb7b7..52ff35a 100644 --- a/src/i2c.c +++ b/src/i2c.c @@ -43,10 +43,14 @@ static char offset = 0; uint8_t dataRX=0; -uint8_t txData[2][I2C_FRAME_SIZE+I2C_INTEGRAL_FRAME_SIZE]; -uint8_t publishedIndex = 0; -uint8_t readout_done_frame1 = 0; -uint8_t readout_done_frame2 = 0; +uint8_t txDataFrame1[2][I2C_FRAME_SIZE]; +uint8_t txDataFrame2[2][I2C_INTEGRAL_FRAME_SIZE]; +uint8_t publishedIndexFrame1 = 0; +uint8_t publishedIndexFrame2 = 0; +uint8_t notpublishedIndexFrame1 = 1; +uint8_t notpublishedIndexFrame2 = 1; +uint8_t readout_done_frame1 = 1; +uint8_t readout_done_frame2 = 1; uint8_t stop_accumulation = 0; @@ -151,7 +155,13 @@ void I2C1_EV_IRQHandler(void) case I2C_EVENT_SLAVE_BYTE_TRANSMITTED : { //todo check overrun condition - I2C_SendData(I2C1, txData[publishedIndex][dataRX+txDataIndex]); + if(dataRX+txDataIndex<(I2C_FRAME_SIZE)){ + I2C_SendData(I2C1, txDataFrame1[publishedIndexFrame1][dataRX+txDataIndex]); + } + else{ + I2C_SendData(I2C1, txDataFrame2[publishedIndexFrame2][dataRX+txDataIndex]); + } + //I2C_SendData(I2C1, txData[publishedIndex][dataRX+txDataIndex]); txDataIndex++; //check whether last byte is read what indicates readout of integral frame and force reset of accumulation @@ -282,23 +292,32 @@ void update_TX_buffer(float pixel_flow_x_sum, float pixel_flow_y_sum, float flow u_integral.f.ground_distance = ground_distance * 1000; u_integral.f.sonar_timestamp = time_since_last_sonar_update; - int notpublishedIndex = 1 - publishedIndex; // choose not the current published buffer + + + notpublishedIndexFrame1 = 1 - publishedIndexFrame1; // choose not the current published 1 buffer + notpublishedIndexFrame2 = 1 - publishedIndexFrame2; // choose not the current published 2 buffer // fill I2C transmitbuffer with values for (i = 0; i < I2C_FRAME_SIZE; i++){ - txData[notpublishedIndex][i] = u.c[i]; + txDataFrame1[notpublishedIndexFrame1][i] = u.c[i]; } for (i = I2C_FRAME_SIZE; i < I2C_INTEGRAL_FRAME_SIZE+I2C_FRAME_SIZE; i++) - txData[notpublishedIndex][i] = u_integral.c_integral[i-I2C_FRAME_SIZE]; + txDataFrame2[notpublishedIndexFrame2][i] = u_integral.c_integral[i-I2C_FRAME_SIZE]; - if(readout_done_frame1){ //checks frame1 only! todo implement both checks while maintaining compatibility - publishedIndex = 1 -publishedIndex; //swap buffers if I2C bus is idle + if(readout_done_frame1){//swap buffers frame1 if I2C bus is idle + publishedIndexFrame1 = 1 -publishedIndexFrame1; } + if(readout_done_frame2){ //swap buffers frame2 if I2C bus is idle + publishedIndexFrame2 = 1 -publishedIndexFrame2; + } + + frame_count++; + } char readI2CAddressOffset() From 6d584340e5691e81146fbd3fce17881380fec8ba Mon Sep 17 00:00:00 2001 From: dominiho Date: Mon, 27 Oct 2014 13:27:01 +0100 Subject: [PATCH 04/14] added serial output throttle factor, adjusted i2c integral frame to output millirad*10, cleaned up main loop and I2C tx update function --- inc/i2c.h | 2 +- inc/i2c_frame.h | 6 +- inc/settings.h | 1 + src/i2c.c | 552 +++++++++++++++++++++++++----------------------- src/main.c | 65 +++--- src/settings.c | 6 +- 6 files changed, 322 insertions(+), 310 deletions(-) diff --git a/inc/i2c.h b/inc/i2c.h index 3166cef..6712757 100644 --- a/inc/i2c.h +++ b/inc/i2c.h @@ -38,7 +38,7 @@ */ void i2c_init(); -void update_TX_buffer(float pixel_flow_x_sum, float pixel_flow_y_sum, float flow_comp_m_x, float flow_comp_m_y, uint16_t qual, +void update_TX_buffer(float pixel_flow_x, float pixel_flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t qual, float ground_distance, float x_rate, float y_rate, float z_rate); char i2c_get_ownaddress1(); #endif /* I2C_H_ */ diff --git a/inc/i2c_frame.h b/inc/i2c_frame.h index 14ddbf2..6a993cb 100644 --- a/inc/i2c_frame.h +++ b/inc/i2c_frame.h @@ -63,10 +63,10 @@ typedef struct i2c_integral_frame int16_t gyro_x_rate_integral; int16_t gyro_y_rate_integral; int16_t gyro_z_rate_integral; - uint32_t time_since_last_readout; + uint32_t integration_timespan; uint32_t sonar_timestamp; - int16_t ground_distance; - uint8_t gyro_range; + uint16_t ground_distance; + uint8_t qual; } __attribute__((packed)) i2c_integral_frame; #define I2C_INTEGRAL_FRAME_SIZE (sizeof(i2c_integral_frame)) diff --git a/inc/settings.h b/inc/settings.h index 5be0748..7182d5a 100644 --- a/inc/settings.h +++ b/inc/settings.h @@ -138,6 +138,7 @@ enum PARAM_BOTTOM_FLOW_GYRO_COMPENSATION, PARAM_BOTTOM_FLOW_LP_FILTERED, PARAM_BOTTOM_FLOW_WEIGHT_NEW, + PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR, PARAM_SENSOR_POSITION, DEBUG_VARIABLE, diff --git a/src/i2c.c b/src/i2c.c index 52ff35a..99d67d4 100644 --- a/src/i2c.c +++ b/src/i2c.c @@ -41,8 +41,11 @@ #include "sonar.h" #include "main.h" +#include "mavlink_bridge_header.h" +#include + static char offset = 0; -uint8_t dataRX=0; +uint8_t dataRX = 0; uint8_t txDataFrame1[2][I2C_FRAME_SIZE]; uint8_t txDataFrame2[2][I2C_INTEGRAL_FRAME_SIZE]; uint8_t publishedIndexFrame1 = 0; @@ -53,285 +56,296 @@ uint8_t readout_done_frame1 = 1; uint8_t readout_done_frame2 = 1; uint8_t stop_accumulation = 0; - -void i2c_init() -{ - - I2C_DeInit(I2C1 ); //Deinit and reset the I2C to avoid it locking up - I2C_SoftwareResetCmd(I2C1, ENABLE); - I2C_SoftwareResetCmd(I2C1, DISABLE); - - GPIO_InitTypeDef gpio_init; - - RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); - - gpio_init.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9; - gpio_init.GPIO_Mode = GPIO_Mode_AF; - gpio_init.GPIO_Speed = GPIO_Speed_50MHz; - //Pull up resistor - gpio_init.GPIO_PuPd = GPIO_PuPd_UP; - //Open Drain - gpio_init.GPIO_OType = GPIO_OType_OD; - GPIO_Init(GPIOB, &gpio_init); - - GPIO_InitTypeDef gpio_init2; - - gpio_init2.GPIO_Pin = GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15; - gpio_init2.GPIO_Mode = GPIO_Mode_IN; - gpio_init2.GPIO_Speed = GPIO_Speed_50MHz; - gpio_init2.GPIO_PuPd = GPIO_PuPd_UP; - GPIO_Init(GPIOC, &gpio_init2); - - GPIO_PinAFConfig(GPIOB, GPIO_PinSource8, GPIO_AF_I2C1 ); // SCL - GPIO_PinAFConfig(GPIOB, GPIO_PinSource9, GPIO_AF_I2C1 ); // SDA - - NVIC_InitTypeDef NVIC_InitStructure, NVIC_InitStructure2; - - NVIC_InitStructure.NVIC_IRQChannel = I2C1_EV_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); - - NVIC_InitStructure2.NVIC_IRQChannel = I2C1_ER_IRQn; - NVIC_InitStructure2.NVIC_IRQChannelPreemptionPriority = 0; - NVIC_InitStructure2.NVIC_IRQChannelSubPriority = 0; - NVIC_InitStructure2.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure2); - - I2C_ITConfig(I2C1, I2C_IT_EVT, ENABLE); - I2C_ITConfig(I2C1, I2C_IT_ERR, ENABLE); - I2C_ITConfig(I2C1, I2C_IT_BUF, ENABLE); - - I2C_InitTypeDef i2c_init; - i2c_init.I2C_ClockSpeed = 100000; - i2c_init.I2C_Mode = I2C_Mode_I2C; - i2c_init.I2C_DutyCycle = I2C_DutyCycle_2; - i2c_init.I2C_OwnAddress1 = i2c_get_ownaddress1(); - i2c_init.I2C_Ack = I2C_Ack_Enable; - i2c_init.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; - I2C_Init(I2C1, &i2c_init); - - I2C_StretchClockCmd(I2C1, ENABLE); - I2C_Cmd(I2C1, ENABLE); +void i2c_init() { + + I2C_DeInit(I2C1 ); //Deinit and reset the I2C to avoid it locking up + I2C_SoftwareResetCmd(I2C1, ENABLE); + I2C_SoftwareResetCmd(I2C1, DISABLE); + + GPIO_InitTypeDef gpio_init; + + RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); + + gpio_init.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9; + gpio_init.GPIO_Mode = GPIO_Mode_AF; + gpio_init.GPIO_Speed = GPIO_Speed_50MHz; + //Pull up resistor + gpio_init.GPIO_PuPd = GPIO_PuPd_UP; + //Open Drain + gpio_init.GPIO_OType = GPIO_OType_OD; + GPIO_Init(GPIOB, &gpio_init); + + GPIO_InitTypeDef gpio_init2; + + gpio_init2.GPIO_Pin = GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15; + gpio_init2.GPIO_Mode = GPIO_Mode_IN; + gpio_init2.GPIO_Speed = GPIO_Speed_50MHz; + gpio_init2.GPIO_PuPd = GPIO_PuPd_UP; + GPIO_Init(GPIOC, &gpio_init2); + + GPIO_PinAFConfig(GPIOB, GPIO_PinSource8, GPIO_AF_I2C1 ); // SCL + GPIO_PinAFConfig(GPIOB, GPIO_PinSource9, GPIO_AF_I2C1 ); // SDA + + NVIC_InitTypeDef NVIC_InitStructure, NVIC_InitStructure2; + + NVIC_InitStructure.NVIC_IRQChannel = I2C1_EV_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + + NVIC_InitStructure2.NVIC_IRQChannel = I2C1_ER_IRQn; + NVIC_InitStructure2.NVIC_IRQChannelPreemptionPriority = 0; + NVIC_InitStructure2.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure2.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure2); + + I2C_ITConfig(I2C1, I2C_IT_EVT, ENABLE); + I2C_ITConfig(I2C1, I2C_IT_ERR, ENABLE); + I2C_ITConfig(I2C1, I2C_IT_BUF, ENABLE); + + I2C_InitTypeDef i2c_init; + i2c_init.I2C_ClockSpeed = 400000; + i2c_init.I2C_Mode = I2C_Mode_I2C; + i2c_init.I2C_DutyCycle = I2C_DutyCycle_2; + i2c_init.I2C_OwnAddress1 = i2c_get_ownaddress1(); + i2c_init.I2C_Ack = I2C_Ack_Enable; + i2c_init.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; + I2C_Init(I2C1, &i2c_init); + + I2C_StretchClockCmd(I2C1, ENABLE); + I2C_Cmd(I2C1, ENABLE); } -void I2C1_EV_IRQHandler(void) -{ - - //uint8_t dataRX; - static uint8_t txDataIndex = 0x00; - static uint8_t rxDataIndex = 0x00; - switch (I2C_GetLastEvent(I2C1 )) - { - - case I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED : - { - I2C1 ->SR1; - I2C1 ->SR2; - rxDataIndex = 0; - break; - } - case I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED : - { - I2C1 ->SR1; - I2C1 ->SR2; - break; - } - case I2C_EVENT_SLAVE_BYTE_RECEIVED : - { - //receive address offset - dataRX = I2C_ReceiveData(I2C1 ); - rxDataIndex++; - //reset Index and indicate sending - txDataIndex = 0; - readout_done_frame1 = 0; - readout_done_frame2 = 0; - break; - } - case I2C_EVENT_SLAVE_BYTE_TRANSMITTING : - case I2C_EVENT_SLAVE_BYTE_TRANSMITTED : - { - //todo check overrun condition - if(dataRX+txDataIndex<(I2C_FRAME_SIZE)){ - I2C_SendData(I2C1, txDataFrame1[publishedIndexFrame1][dataRX+txDataIndex]); - } - else{ - I2C_SendData(I2C1, txDataFrame2[publishedIndexFrame2][dataRX+txDataIndex]); - } - //I2C_SendData(I2C1, txData[publishedIndex][dataRX+txDataIndex]); - txDataIndex++; - - //check whether last byte is read what indicates readout of integral frame and force reset of accumulation - if(dataRX+txDataIndex>=(I2C_FRAME_SIZE)){ - readout_done_frame1=1; - } - - if(dataRX+txDataIndex>=(I2C_FRAME_SIZE+I2C_INTEGRAL_FRAME_SIZE)){ - readout_done_frame2=1; - } - - break; - } - - case I2C_EVENT_SLAVE_ACK_FAILURE : - { - I2C1 ->SR1 &= 0x00FF; - break; - } - - case I2C_EVENT_SLAVE_STOP_DETECTED : - { - I2C1 ->SR1; - I2C1 ->CR1 |= 0x1; - break; - } - } +void I2C1_EV_IRQHandler(void) { + + //uint8_t dataRX; + static uint8_t txDataIndex1 = 0x00; + static uint8_t txDataIndex2 = 0x00; + static uint8_t rxDataIndex = 0x00; + switch (I2C_GetLastEvent(I2C1 )) { + + case I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED : { + I2C1 ->SR1; + I2C1 ->SR2; + rxDataIndex = 0; + break; + } + case I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED : { + I2C1 ->SR1; + I2C1 ->SR2; + break; + } + case I2C_EVENT_SLAVE_BYTE_RECEIVED : { + //receive address offset + dataRX = I2C_ReceiveData(I2C1 ); + rxDataIndex++; + //set Index + txDataIndex1 = dataRX; + if (dataRX > I2C_FRAME_SIZE) { + txDataIndex2 = dataRX - I2C_FRAME_SIZE; + } + else { + txDataIndex2 = 0; + } + //indicate sending + readout_done_frame1 = 0; + readout_done_frame2 = 0; + break; + } + case I2C_EVENT_SLAVE_BYTE_TRANSMITTING : + case I2C_EVENT_SLAVE_BYTE_TRANSMITTED : { + + if (txDataIndex1 < (I2C_FRAME_SIZE)) { + I2C_SendData(I2C1, + txDataFrame1[publishedIndexFrame1][txDataIndex1]); + txDataIndex1++; + } else { + I2C_SendData(I2C1, + txDataFrame2[publishedIndexFrame2][txDataIndex2]); + if (txDataIndex2 < I2C_INTEGRAL_FRAME_SIZE) { + txDataIndex2++; + } + + } + + //check whether last byte is read frame1 + if (txDataIndex1 >= (I2C_FRAME_SIZE-1)) { + readout_done_frame1 = 1; + } + + //check whether last byte is read fram2 and reset accumulation + if (txDataIndex2 >= (I2C_INTEGRAL_FRAME_SIZE-1)) { + readout_done_frame2 = 1; + stop_accumulation = 1; + } + + break; + } + + case I2C_EVENT_SLAVE_ACK_FAILURE : { + I2C1 ->SR1 &= 0x00FF; + break; + } + + case I2C_EVENT_SLAVE_STOP_DETECTED : { + I2C1 ->SR1; + I2C1 ->CR1 |= 0x1; + break; + } + } } -void I2C1_ER_IRQHandler(void) -{ +void I2C1_ER_IRQHandler(void) { - /* Read SR1 register to get I2C error */ - if ((I2C_ReadRegister(I2C1, I2C_Register_SR1 ) & 0xFF00) != 0x00) - { - /* Clears error flags */ - I2C1 ->SR1 &= 0x00FF; - } + /* Read SR1 register to get I2C error */ + if ((I2C_ReadRegister(I2C1, I2C_Register_SR1 ) & 0xFF00) != 0x00) { + /* Clears error flags */ + I2C1 ->SR1 &= 0x00FF; + } } -void update_TX_buffer(float pixel_flow_x_sum, float pixel_flow_y_sum, float flow_comp_m_x, float flow_comp_m_y, uint16_t qual, - float ground_distance, float gyro_x_rate, float gyro_y_rate, float gyro_z_rate) -{ - static uint16_t frame_count = 0; - int i; - union - { - i2c_frame f; - char c[I2C_FRAME_SIZE]; - } u; - - u.f.frame_count = frame_count; - u.f.pixel_flow_x_sum = pixel_flow_x_sum; - u.f.pixel_flow_y_sum = pixel_flow_y_sum; - u.f.flow_comp_m_x = flow_comp_m_x * 1000; - u.f.flow_comp_m_y = flow_comp_m_y * 1000; - u.f.qual = qual; - u.f.ground_distance = ground_distance * 1000; - - u.f.gyro_x_rate = gyro_x_rate * getGyroScalingFactor(); - u.f.gyro_y_rate = gyro_y_rate * getGyroScalingFactor(); - u.f.gyro_z_rate = gyro_z_rate * getGyroScalingFactor(); - u.f.gyro_range = getGyroRange(); - - - uint32_t time_since_last_sonar_update; - - time_since_last_sonar_update = (get_boot_time_us() - get_sonar_measure_time()); - - - if (time_since_last_sonar_update < 255*1000){ - u.f.sonar_timestamp = time_since_last_sonar_update/1000;//convert to ms - } - else{ - u.f.sonar_timestamp = 255; - } - - - union - { - i2c_integral_frame f; - char c_integral[I2C_INTEGRAL_FRAME_SIZE]; - } u_integral; - - static uint16_t accumulated_flow_x = 0; - static uint16_t accumulated_flow_y = 0; - static uint16_t accumulated_framecount = 0; - static int16_t accumulated_gyro_x = 0; - static int16_t accumulated_gyro_y = 0; - static int16_t accumulated_gyro_z = 0; - static uint32_t time_between_readings = 0; - static uint32_t lasttime = 0; - - - //accumulate flow and gyro values between sucessive I2C readings - if(stop_accumulation==0 && readout_done_frame2==1){ - // reset if readout has been performed - time_between_readings = time_between_readings/accumulated_framecount; //fill in minimal time between frames - accumulated_flow_x = pixel_flow_x_sum; - accumulated_flow_y = pixel_flow_y_sum; - accumulated_framecount = 1; - accumulated_gyro_x = gyro_x_rate * getGyroScalingFactor(); - accumulated_gyro_y = gyro_y_rate * getGyroScalingFactor(); - accumulated_gyro_z = gyro_z_rate * getGyroScalingFactor(); - } - else{ - // accumulate if there is no readout - time_between_readings += (get_boot_time_us()-lasttime); - accumulated_flow_x += pixel_flow_x_sum; - accumulated_flow_y += pixel_flow_y_sum; - accumulated_framecount++; - accumulated_gyro_x += gyro_x_rate * getGyroScalingFactor(); - accumulated_gyro_y += gyro_y_rate * getGyroScalingFactor(); - accumulated_gyro_z += gyro_z_rate * getGyroScalingFactor(); - } - - lasttime = get_boot_time_us(); - stop_accumulation=readout_done_frame2; - - u_integral.f.frame_count_since_last_readout = accumulated_framecount; - u_integral.f.gyro_x_rate_integral =accumulated_gyro_x; - u_integral.f.gyro_y_rate_integral =accumulated_gyro_y; - u_integral.f.gyro_z_rate_integral =accumulated_gyro_z; - u_integral.f.gyro_range = getGyroRange(); - u_integral.f.pixel_flow_x_integral =accumulated_flow_x; - u_integral.f.pixel_flow_y_integral =accumulated_flow_y; - u_integral.f.time_since_last_readout = time_between_readings; - u_integral.f.ground_distance = ground_distance * 1000; - u_integral.f.sonar_timestamp = time_since_last_sonar_update; - - - - notpublishedIndexFrame1 = 1 - publishedIndexFrame1; // choose not the current published 1 buffer - notpublishedIndexFrame2 = 1 - publishedIndexFrame2; // choose not the current published 2 buffer - - // fill I2C transmitbuffer with values - for (i = 0; i < I2C_FRAME_SIZE; i++){ - txDataFrame1[notpublishedIndexFrame1][i] = u.c[i]; - } - - for (i = I2C_FRAME_SIZE; i < I2C_INTEGRAL_FRAME_SIZE+I2C_FRAME_SIZE; i++) - txDataFrame2[notpublishedIndexFrame2][i] = u_integral.c_integral[i-I2C_FRAME_SIZE]; - - - if(readout_done_frame1){//swap buffers frame1 if I2C bus is idle - publishedIndexFrame1 = 1 -publishedIndexFrame1; - } - - if(readout_done_frame2){ //swap buffers frame2 if I2C bus is idle - publishedIndexFrame2 = 1 -publishedIndexFrame2; - } - - - frame_count++; - +void update_TX_buffer(float pixel_flow_x, float pixel_flow_y, + float flow_comp_m_x, float flow_comp_m_y, uint8_t qual, + float ground_distance, float gyro_x_rate, float gyro_y_rate, + float gyro_z_rate) { + static uint16_t frame_count = 0; + int i; + union { + i2c_frame f; + char c[I2C_FRAME_SIZE]; + } u; + + u.f.frame_count = frame_count; + u.f.pixel_flow_x_sum = pixel_flow_x * 10.0f; + u.f.pixel_flow_y_sum = pixel_flow_y * 10.0f; + u.f.flow_comp_m_x = flow_comp_m_x * 1000; + u.f.flow_comp_m_y = flow_comp_m_y * 1000; + u.f.qual = qual; + u.f.ground_distance = ground_distance * 1000; + + u.f.gyro_x_rate = gyro_x_rate * getGyroScalingFactor(); + u.f.gyro_y_rate = gyro_y_rate * getGyroScalingFactor(); + u.f.gyro_z_rate = gyro_z_rate * getGyroScalingFactor(); + u.f.gyro_range = getGyroRange(); + + uint32_t time_since_last_sonar_update; + + time_since_last_sonar_update = (get_boot_time_us() + - get_sonar_measure_time()); + + if (time_since_last_sonar_update < 255 * 1000) { + u.f.sonar_timestamp = time_since_last_sonar_update / 1000; //convert to ms + } else { + u.f.sonar_timestamp = 255; + } + + union { + i2c_integral_frame f; + char c_integral[I2C_INTEGRAL_FRAME_SIZE]; + } u_integral; + + static float accumulated_flow_x = 0; + static float accumulated_flow_y = 0; + static uint16_t accumulated_framecount = 0; + static uint16_t accumulated_quality = 0; + static float accumulated_gyro_x = 0; + static float accumulated_gyro_y = 0; + static float accumulated_gyro_z = 0; + static uint32_t integration_timespan = 0; + static uint32_t lasttime = 0; + + /* calculate focal_length in pixel */ + const float focal_length_px = ((global_data.param[PARAM_FOCAL_LENGTH_MM]) + / (4.0f * 6.0f) * 1000.0f); //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled + + // reset if readout has been performed + if (stop_accumulation == 1) { + + //debug output +// mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), +// global_data.param[PARAM_SENSOR_ID], accumulated_flow_x * 10.0f, +// accumulated_gyro_x * 10.0f, integration_timespan, +// accumulated_framecount, (uint8_t) (accumulated_quality / accumulated_framecount), ground_distance); + + integration_timespan = 0; + accumulated_flow_x = 0; //mrad + accumulated_flow_y = 0; //mrad + accumulated_framecount = 0; + accumulated_quality = 0; + accumulated_gyro_x = 0; //mrad + accumulated_gyro_y = 0; //mrad + accumulated_gyro_z = 0; //mrad + stop_accumulation = 0; + } + + //accumulate flow and gyro values between sucessive I2C readings + //update only if qual !=0 + if (qual > 0) { + uint32_t deltatime = (get_boot_time_us() - lasttime); + integration_timespan += deltatime; + accumulated_flow_x += pixel_flow_y * 1000.0f / focal_length_px * 1.0f;//mrad axis swapped to align x flow around y axis + accumulated_flow_y += pixel_flow_x * 1000.0f / focal_length_px * -1.0f; //mrad + accumulated_framecount++; + accumulated_quality += qual; + accumulated_gyro_x += gyro_x_rate * deltatime * 0.001f; //mrad gyro_x_rate * 1000.0f*deltatime/1000000.0f; + accumulated_gyro_y += gyro_y_rate * deltatime * 0.001f; //mrad + accumulated_gyro_z += gyro_z_rate * deltatime * 0.001f; //mrad + } + + //update lasttime + lasttime = get_boot_time_us(); + + u_integral.f.frame_count_since_last_readout = accumulated_framecount; + u_integral.f.gyro_x_rate_integral = accumulated_gyro_x * 10.0f; //mrad*10 + u_integral.f.gyro_y_rate_integral = accumulated_gyro_y * 10.0f; //mrad*10 + u_integral.f.gyro_z_rate_integral = accumulated_gyro_z * 10.0f; //mrad*10 + u_integral.f.pixel_flow_x_integral = accumulated_flow_x * 10.0f; //mrad*10 + u_integral.f.pixel_flow_y_integral = accumulated_flow_y * 10.0f; //mrad*10 + u_integral.f.integration_timespan = integration_timespan; //microseconds + u_integral.f.ground_distance = ground_distance * 1000; //mmeters + u_integral.f.sonar_timestamp = time_since_last_sonar_update; //microseconds + u_integral.f.qual = + (uint8_t) (accumulated_quality / accumulated_framecount); //0-255 linear quality measurement 0=bad, 255=best + + notpublishedIndexFrame1 = 1 - publishedIndexFrame1; // choose not the current published 1 buffer + notpublishedIndexFrame2 = 1 - publishedIndexFrame2; // choose not the current published 2 buffer + + // fill I2C transmitbuffer1 with frame1 values + for (i = 0; i < I2C_FRAME_SIZE; i++) { + txDataFrame1[notpublishedIndexFrame1][i] = u.c[i]; + } + + // fill I2C transmitbuffer2 with frame2 values + for (i = 0; i < I2C_INTEGRAL_FRAME_SIZE; i++) + txDataFrame2[notpublishedIndexFrame2][i] = u_integral.c_integral[i]; + + //swap buffers frame1 if I2C bus is idle + if (readout_done_frame1) { + publishedIndexFrame1 = 1 - publishedIndexFrame1; + } + + //swap buffers frame2 if I2C bus is idle + if (readout_done_frame2) { + publishedIndexFrame2 = 1 - publishedIndexFrame2; + } + + frame_count++; } -char readI2CAddressOffset() -{ +char readI2CAddressOffset() { //read 3bit address offset of 7 bit address - offset = 0x00; - offset = GPIO_ReadInputData(GPIOC) >> 13;//bit 0 - offset = offset | ((GPIO_ReadInputData(GPIOC) >> 14)<<1);//bit 1 - offset = offset | ((GPIO_ReadInputData(GPIOC) >> 15)<<2);//bit 2 - offset = (~offset) & 0x07; - return offset; + offset = 0x00; + offset = GPIO_ReadInputData(GPIOC ) >> 13; //bit 0 + offset = offset | ((GPIO_ReadInputData(GPIOC ) >> 14) << 1); //bit 1 + offset = offset | ((GPIO_ReadInputData(GPIOC ) >> 15) << 2); //bit 2 + offset = (~offset) & 0x07; + return offset; } -char i2c_get_ownaddress1() -{ - return (I2C1_OWNADDRESS_1_BASE + readI2CAddressOffset()) << 1;//add offset to base and shift 1 bit to generate valid 7 bit address +char i2c_get_ownaddress1() { + return (I2C1_OWNADDRESS_1_BASE + readI2CAddressOffset()) << 1; //add offset to base and shift 1 bit to generate valid 7 bit address } diff --git a/src/main.c b/src/main.c index c9eccd6..6ddf06d 100644 --- a/src/main.c +++ b/src/main.c @@ -431,17 +431,33 @@ int main(void) counter++; - /* TODO for debugging */ - //mavlink_msg_named_value_float_send(MAVLINK_COMM_2, boot_time_ms, "blabla", blabla); - if(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM) { /* send bottom flow if activated */ - if (counter % 1 == 0) + + float ground_distance = 0.0f; + + + if(global_data.param[PARAM_SONAR_FILTERED]) + { + ground_distance = sonar_distance_filtered; + } + else + { + ground_distance = sonar_distance_raw; + } + + //update I2C transmitbuffer + update_TX_buffer(pixel_flow_x, pixel_flow_y, velocity_x_sum/valid_frame_count, velocity_y_sum/valid_frame_count, qual, + ground_distance, x_rate, y_rate, z_rate); + + //serial mavlink + usb mavlink output throttled + if (counter % (uint32_t)global_data.param[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] == 0)//throttling factor { + float flow_comp_m_x = 0.0f; float flow_comp_m_y = 0.0f; - float ground_distance = 0.0f; + if(global_data.param[PARAM_BOTTOM_FLOW_LP_FILTERED]) { flow_comp_m_x = velocity_x_lp; @@ -453,42 +469,19 @@ int main(void) flow_comp_m_y = velocity_y_sum/valid_frame_count; } - if(global_data.param[PARAM_SONAR_FILTERED]) - ground_distance = sonar_distance_filtered; - else - ground_distance = sonar_distance_raw; - if (valid_frame_count > 0) - { - // send flow - mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], - pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, - flow_comp_m_x, flow_comp_m_y, qual, ground_distance); + // send flow + mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], + pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, + flow_comp_m_x, flow_comp_m_y, qual, ground_distance); - if (global_data.param[PARAM_USB_SEND_FLOW]) - mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], + if (global_data.param[PARAM_USB_SEND_FLOW]) + { + mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, - flow_comp_m_x, flow_comp_m_y, qual, ground_distance); - - update_TX_buffer(pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, flow_comp_m_x, flow_comp_m_y, qual, - ground_distance, x_rate, y_rate, z_rate); - + flow_comp_m_x, flow_comp_m_y, qual, ground_distance); } - else - { - // send distance - mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], - pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, - 0.0f, 0.0f, 0, ground_distance); - if (global_data.param[PARAM_USB_SEND_FLOW]) - mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], - pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, - 0.0f, 0.0f, 0, ground_distance); - - update_TX_buffer(pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, 0.0f, 0.0f, 0, ground_distance, x_rate, y_rate, - z_rate); - } if(global_data.param[PARAM_USB_SEND_GYRO]) { diff --git a/src/settings.c b/src/settings.c index f84f07a..f5a02c6 100644 --- a/src/settings.c +++ b/src/settings.c @@ -176,7 +176,7 @@ void global_data_reset_param_defaults(void){ global_data.param_access[PARAM_BOTTOM_FLOW_HIST_FILTER] = READ_WRITE; // global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION] = 0; - global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION] = 1; + global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION] = 0; strcpy(global_data.param_name[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION], "BFLOW_GYRO_COM"); global_data.param_access[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION] = READ_WRITE; @@ -188,6 +188,10 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_BOTTOM_FLOW_WEIGHT_NEW], "BFLOW_W_NEW"); global_data.param_access[PARAM_BOTTOM_FLOW_WEIGHT_NEW] = READ_WRITE; + global_data.param[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] = 10.0f; + strcpy(global_data.param_name[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR], "BFLOW_THROTT"); + global_data.param_access[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] = READ_WRITE; + global_data.param[DEBUG_VARIABLE] = 1; strcpy(global_data.param_name[DEBUG_VARIABLE], "DEBUG"); global_data.param_access[DEBUG_VARIABLE] = READ_WRITE; From 87a9b1ef57926ea49d032cc344bf3ac3a366bbbb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Oct 2014 17:08:07 +0100 Subject: [PATCH 05/14] Update MAVLink, use default dialect --- mavlink/include/mavlink/config.h | 1 - .../v1.0/ardupilotmega/ardupilotmega.h | 205 - .../mavlink/v1.0/ardupilotmega/mavlink.h | 27 - .../v1.0/ardupilotmega/mavlink_msg_ahrs.h | 309 -- .../v1.0/ardupilotmega/mavlink_msg_ahrs2.h | 287 -- .../mavlink_msg_airspeed_autocal.h | 419 -- .../v1.0/ardupilotmega/mavlink_msg_ap_adc.h | 287 -- .../v1.0/ardupilotmega/mavlink_msg_data16.h | 215 - .../v1.0/ardupilotmega/mavlink_msg_data32.h | 215 - .../v1.0/ardupilotmega/mavlink_msg_data64.h | 215 - .../v1.0/ardupilotmega/mavlink_msg_data96.h | 215 - .../mavlink_msg_digicam_configure.h | 397 -- .../mavlink_msg_digicam_control.h | 375 -- .../mavlink_msg_fence_fetch_point.h | 221 - .../ardupilotmega/mavlink_msg_fence_point.h | 287 -- .../ardupilotmega/mavlink_msg_fence_status.h | 243 -- .../v1.0/ardupilotmega/mavlink_msg_hwstatus.h | 199 - .../ardupilotmega/mavlink_msg_limits_status.h | 353 -- .../v1.0/ardupilotmega/mavlink_msg_meminfo.h | 199 - .../mavlink_msg_mount_configure.h | 287 -- .../ardupilotmega/mavlink_msg_mount_control.h | 287 -- .../ardupilotmega/mavlink_msg_mount_status.h | 265 -- .../v1.0/ardupilotmega/mavlink_msg_radio.h | 309 -- .../mavlink_msg_rally_fetch_point.h | 221 - .../ardupilotmega/mavlink_msg_rally_point.h | 375 -- .../ardupilotmega/mavlink_msg_rangefinder.h | 199 - .../mavlink_msg_sensor_offsets.h | 419 -- .../mavlink_msg_set_mag_offsets.h | 265 -- .../v1.0/ardupilotmega/mavlink_msg_simstate.h | 397 -- .../v1.0/ardupilotmega/mavlink_msg_wind.h | 221 - .../mavlink/v1.0/ardupilotmega/testsuite.h | 1488 ------- .../mavlink/v1.0/ardupilotmega/version.h | 12 - .../include/mavlink/v1.0/autoquad/autoquad.h | 123 - .../include/mavlink/v1.0/autoquad/mavlink.h | 27 - .../autoquad/mavlink_msg_aq_telemetry_f.h | 617 --- .../include/mavlink/v1.0/autoquad/testsuite.h | 118 - .../include/mavlink/v1.0/autoquad/version.h | 12 - mavlink/include/mavlink/v1.0/checksum.h | 7 +- mavlink/include/mavlink/v1.0/common/common.h | 340 +- mavlink/include/mavlink/v1.0/common/mavlink.h | 2 +- .../v1.0/common/mavlink_msg_attitude.h | 44 + .../common/mavlink_msg_attitude_quaternion.h | 86 +- .../mavlink_msg_attitude_quaternion_cov.h | 322 ++ .../v1.0/common/mavlink_msg_attitude_target.h | 345 ++ .../v1.0/common/mavlink_msg_auth_key.h | 32 + .../common/mavlink_msg_autopilot_version.h | 249 ++ .../v1.0/common/mavlink_msg_battery_status.h | 298 +- .../mavlink_msg_change_operator_control.h | 36 + .../mavlink_msg_change_operator_control_ack.h | 36 + .../v1.0/common/mavlink_msg_command_ack.h | 34 + .../v1.0/common/mavlink_msg_command_int.h | 497 +++ .../v1.0/common/mavlink_msg_command_long.h | 52 + .../v1.0/common/mavlink_msg_data_stream.h | 36 + .../mavlink_msg_data_transmission_handshake.h | 44 + .../mavlink/v1.0/common/mavlink_msg_debug.h | 36 + .../v1.0/common/mavlink_msg_debug_vect.h | 38 + .../v1.0/common/mavlink_msg_distance_sensor.h | 377 ++ .../common/mavlink_msg_encapsulated_data.h | 32 + .../mavlink_msg_file_transfer_dir_list.h | 215 - .../mavlink_msg_file_transfer_protocol.h | 273 ++ .../common/mavlink_msg_file_transfer_res.h | 199 - .../common/mavlink_msg_file_transfer_start.h | 259 -- .../common/mavlink_msg_global_position_int.h | 58 +- .../mavlink_msg_global_position_int_cov.h | 441 ++ ...mavlink_msg_global_position_setpoint_int.h | 265 -- ...link_msg_global_vision_position_estimate.h | 44 + .../v1.0/common/mavlink_msg_gps2_raw.h | 64 +- .../v1.0/common/mavlink_msg_gps2_rtk.h | 497 +++ .../common/mavlink_msg_gps_global_origin.h | 36 + .../v1.0/common/mavlink_msg_gps_inject_data.h | 36 + .../v1.0/common/mavlink_msg_gps_raw_int.h | 60 +- .../mavlink/v1.0/common/mavlink_msg_gps_rtk.h | 497 +++ .../v1.0/common/mavlink_msg_gps_status.h | 40 + .../v1.0/common/mavlink_msg_heartbeat.h | 52 +- .../v1.0/common/mavlink_msg_highres_imu.h | 60 + .../v1.0/common/mavlink_msg_hil_controls.h | 52 + .../mavlink/v1.0/common/mavlink_msg_hil_gps.h | 56 + .../common/mavlink_msg_hil_optical_flow.h | 46 + .../common/mavlink_msg_hil_rc_inputs_raw.h | 58 + .../v1.0/common/mavlink_msg_hil_sensor.h | 60 + .../v1.0/common/mavlink_msg_hil_state.h | 62 + .../common/mavlink_msg_hil_state_quaternion.h | 70 +- .../common/mavlink_msg_local_position_ned.h | 44 + .../mavlink_msg_local_position_ned_cov.h | 417 ++ ..._local_position_ned_system_global_offset.h | 44 + .../mavlink_msg_local_position_setpoint.h | 265 -- .../v1.0/common/mavlink_msg_log_data.h | 36 + .../v1.0/common/mavlink_msg_log_entry.h | 40 + .../v1.0/common/mavlink_msg_log_erase.h | 34 + .../common/mavlink_msg_log_request_data.h | 40 + .../v1.0/common/mavlink_msg_log_request_end.h | 34 + .../common/mavlink_msg_log_request_list.h | 38 + .../v1.0/common/mavlink_msg_manual_control.h | 42 + .../v1.0/common/mavlink_msg_manual_setpoint.h | 44 + .../v1.0/common/mavlink_msg_memory_vect.h | 36 + .../v1.0/common/mavlink_msg_mission_ack.h | 36 + .../common/mavlink_msg_mission_clear_all.h | 34 + .../v1.0/common/mavlink_msg_mission_count.h | 36 + .../v1.0/common/mavlink_msg_mission_current.h | 32 + .../v1.0/common/mavlink_msg_mission_item.h | 58 + .../common/mavlink_msg_mission_item_int.h | 521 +++ .../common/mavlink_msg_mission_item_reached.h | 32 + .../v1.0/common/mavlink_msg_mission_request.h | 36 + .../common/mavlink_msg_mission_request_list.h | 34 + ...mavlink_msg_mission_request_partial_list.h | 38 + .../common/mavlink_msg_mission_set_current.h | 36 + .../mavlink_msg_mission_write_partial_list.h | 38 + .../common/mavlink_msg_named_value_float.h | 34 + .../v1.0/common/mavlink_msg_named_value_int.h | 34 + .../mavlink_msg_nav_controller_output.h | 46 + .../common/mavlink_msg_omnidirectional_flow.h | 282 -- .../v1.0/common/mavlink_msg_optical_flow.h | 46 + .../common/mavlink_msg_optical_flow_rad.h | 377 ++ .../common/mavlink_msg_param_request_list.h | 34 + .../common/mavlink_msg_param_request_read.h | 36 + .../v1.0/common/mavlink_msg_param_set.h | 38 + .../v1.0/common/mavlink_msg_param_value.h | 38 + .../mavlink/v1.0/common/mavlink_msg_ping.h | 48 +- .../mavlink_msg_position_target_global_int.h | 521 +++ .../mavlink_msg_position_target_local_ned.h | 521 +++ .../v1.0/common/mavlink_msg_power_status.h | 257 ++ .../v1.0/common/mavlink_msg_radio_status.h | 44 + .../mavlink/v1.0/common/mavlink_msg_raw_imu.h | 50 + .../v1.0/common/mavlink_msg_raw_pressure.h | 40 + .../v1.0/common/mavlink_msg_rc_channels.h | 689 ++++ .../common/mavlink_msg_rc_channels_override.h | 50 + .../v1.0/common/mavlink_msg_rc_channels_raw.h | 52 + .../common/mavlink_msg_rc_channels_scaled.h | 52 + .../common/mavlink_msg_request_data_stream.h | 40 + ...msg_roll_pitch_yaw_rates_thrust_setpoint.h | 265 -- ...msg_roll_pitch_yaw_speed_thrust_setpoint.h | 265 -- ...vlink_msg_roll_pitch_yaw_thrust_setpoint.h | 265 -- .../common/mavlink_msg_safety_allowed_area.h | 44 + .../mavlink_msg_safety_set_allowed_area.h | 48 + .../v1.0/common/mavlink_msg_scaled_imu.h | 50 + .../v1.0/common/mavlink_msg_scaled_imu2.h | 50 + .../v1.0/common/mavlink_msg_scaled_pressure.h | 38 + .../v1.0/common/mavlink_msg_serial_control.h | 321 ++ .../common/mavlink_msg_servo_output_raw.h | 50 + .../common/mavlink_msg_set_attitude_target.h | 393 ++ ...ink_msg_set_global_position_setpoint_int.h | 265 -- .../mavlink_msg_set_gps_global_origin.h | 38 + .../mavlink_msg_set_local_position_setpoint.h | 309 -- .../v1.0/common/mavlink_msg_set_mode.h | 36 + ...vlink_msg_set_position_target_global_int.h | 569 +++ ...avlink_msg_set_position_target_local_ned.h | 569 +++ .../mavlink_msg_set_quad_motors_setpoint.h | 265 -- ...set_quad_swarm_led_roll_pitch_yaw_thrust.h | 353 -- ...msg_set_quad_swarm_roll_pitch_yaw_thrust.h | 284 -- ...link_msg_set_roll_pitch_yaw_speed_thrust.h | 287 -- .../mavlink_msg_set_roll_pitch_yaw_thrust.h | 287 -- .../v1.0/common/mavlink_msg_setpoint_6dof.h | 309 -- .../v1.0/common/mavlink_msg_setpoint_8dof.h | 353 -- .../v1.0/common/mavlink_msg_sim_state.h | 112 +- .../common/mavlink_msg_state_correction.h | 353 -- .../v1.0/common/mavlink_msg_statustext.h | 32 + .../v1.0/common/mavlink_msg_sys_status.h | 56 + .../v1.0/common/mavlink_msg_system_time.h | 34 + .../v1.0/common/mavlink_msg_terrain_check.h | 233 ++ .../v1.0/common/mavlink_msg_terrain_data.h | 297 ++ .../v1.0/common/mavlink_msg_terrain_report.h | 353 ++ .../v1.0/common/mavlink_msg_terrain_request.h | 281 ++ .../v1.0/common/mavlink_msg_v2_extension.h | 297 ++ .../mavlink/v1.0/common/mavlink_msg_vfr_hud.h | 42 + .../mavlink_msg_vicon_position_estimate.h | 44 + .../mavlink_msg_vision_position_estimate.h | 44 + .../mavlink_msg_vision_speed_estimate.h | 38 + .../include/mavlink/v1.0/common/testsuite.h | 3021 +++++++------- mavlink/include/mavlink/v1.0/common/version.h | 4 +- .../mavlink/v1.0/matrixpilot/matrixpilot.h | 133 - .../mavlink/v1.0/matrixpilot/mavlink.h | 27 - .../v1.0/matrixpilot/mavlink_msg_airspeeds.h | 309 -- .../v1.0/matrixpilot/mavlink_msg_altitudes.h | 309 -- ...avlink_msg_flexifunction_buffer_function.h | 303 -- ...nk_msg_flexifunction_buffer_function_ack.h | 243 -- .../mavlink_msg_flexifunction_command.h | 221 - .../mavlink_msg_flexifunction_command_ack.h | 199 - .../mavlink_msg_flexifunction_directory.h | 281 -- .../mavlink_msg_flexifunction_directory_ack.h | 287 -- .../mavlink_msg_flexifunction_read_req.h | 243 -- .../mavlink_msg_flexifunction_set.h | 199 - .../mavlink_msg_serial_udb_extra_f13.h | 243 -- .../mavlink_msg_serial_udb_extra_f14.h | 397 -- .../mavlink_msg_serial_udb_extra_f15.h | 200 - .../mavlink_msg_serial_udb_extra_f16.h | 200 - .../mavlink_msg_serial_udb_extra_f2_a.h | 771 ---- .../mavlink_msg_serial_udb_extra_f2_b.h | 881 ---- .../mavlink_msg_serial_udb_extra_f4.h | 375 -- .../mavlink_msg_serial_udb_extra_f5.h | 287 -- .../mavlink_msg_serial_udb_extra_f6.h | 265 -- .../mavlink_msg_serial_udb_extra_f7.h | 287 -- .../mavlink_msg_serial_udb_extra_f8.h | 309 -- .../mavlink/v1.0/matrixpilot/testsuite.h | 1240 ------ .../mavlink/v1.0/matrixpilot/version.h | 12 - .../mavlink/v1.0/mavlink_conversions.h | 50 + .../include/mavlink/v1.0/mavlink_helpers.h | 36 +- mavlink/include/mavlink/v1.0/mavlink_types.h | 56 +- .../v1.0/{pixhawk => minimal}/mavlink.h | 6 +- .../v1.0/minimal/mavlink_msg_heartbeat.h | 326 ++ .../include/mavlink/v1.0/minimal/minimal.h | 154 + .../include/mavlink/v1.0/minimal/testsuite.h | 83 + .../include/mavlink/v1.0/minimal/version.h | 12 + .../pixhawk/mavlink_msg_attitude_control.h | 353 -- .../v1.0/pixhawk/mavlink_msg_brief_feature.h | 325 -- .../pixhawk/mavlink_msg_image_available.h | 661 --- .../mavlink_msg_image_trigger_control.h | 177 - .../pixhawk/mavlink_msg_image_triggered.h | 419 -- .../mavlink/v1.0/pixhawk/mavlink_msg_marker.h | 309 -- .../pixhawk/mavlink_msg_pattern_detected.h | 237 -- .../pixhawk/mavlink_msg_point_of_interest.h | 325 -- ...mavlink_msg_point_of_interest_connection.h | 391 -- .../mavlink_msg_position_control_setpoint.h | 265 -- .../v1.0/pixhawk/mavlink_msg_raw_aux.h | 309 -- .../pixhawk/mavlink_msg_set_cam_shutter.h | 287 -- .../mavlink_msg_set_position_control_offset.h | 287 -- .../pixhawk/mavlink_msg_watchdog_command.h | 243 -- .../pixhawk/mavlink_msg_watchdog_heartbeat.h | 199 - .../mavlink_msg_watchdog_process_info.h | 260 -- .../mavlink_msg_watchdog_process_status.h | 287 -- .../include/mavlink/v1.0/pixhawk/pixhawk.h | 127 - .../include/mavlink/v1.0/pixhawk/pixhawk.pb.h | 3663 ----------------- .../include/mavlink/v1.0/pixhawk/testsuite.h | 996 ----- .../include/mavlink/v1.0/pixhawk/version.h | 12 - mavlink/include/mavlink/v1.0/protocol.h | 79 +- .../include/mavlink/v1.0/sensesoar/mavlink.h | 27 - .../sensesoar/mavlink_msg_cmd_airspeed_ack.h | 219 - .../sensesoar/mavlink_msg_cmd_airspeed_chng.h | 219 - .../v1.0/sensesoar/mavlink_msg_filt_rot_vel.h | 187 - .../v1.0/sensesoar/mavlink_msg_llc_out.h | 220 - .../v1.0/sensesoar/mavlink_msg_obs_air_temp.h | 187 - .../sensesoar/mavlink_msg_obs_air_velocity.h | 251 -- .../v1.0/sensesoar/mavlink_msg_obs_attitude.h | 187 - .../v1.0/sensesoar/mavlink_msg_obs_bias.h | 220 - .../v1.0/sensesoar/mavlink_msg_obs_position.h | 251 -- .../v1.0/sensesoar/mavlink_msg_obs_qff.h | 187 - .../v1.0/sensesoar/mavlink_msg_obs_velocity.h | 187 - .../v1.0/sensesoar/mavlink_msg_obs_wind.h | 187 - .../v1.0/sensesoar/mavlink_msg_pm_elec.h | 245 -- .../v1.0/sensesoar/mavlink_msg_sys_stat.h | 283 -- .../mavlink/v1.0/sensesoar/sensesoar.h | 77 - .../mavlink/v1.0/sensesoar/testsuite.h | 676 --- .../include/mavlink/v1.0/sensesoar/version.h | 12 - 242 files changed, 16275 insertions(+), 39784 deletions(-) delete mode 100644 mavlink/include/mavlink/config.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/version.h delete mode 100644 mavlink/include/mavlink/v1.0/autoquad/autoquad.h delete mode 100644 mavlink/include/mavlink/v1.0/autoquad/mavlink.h delete mode 100644 mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h delete mode 100644 mavlink/include/mavlink/v1.0/autoquad/testsuite.h delete mode 100644 mavlink/include/mavlink/v1.0/autoquad/version.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion_cov.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_target.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_autopilot_version.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_command_int.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_protocol.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int_cov.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_rtk.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_rtk.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_cov.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_int.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow_rad.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_position_target_global_int.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_position_target_local_ned.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_power_status.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_serial_control.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_set_attitude_target.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_set_position_target_global_int.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_set_position_target_local_ned.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_terrain_check.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_terrain_data.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_terrain_report.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_terrain_request.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_v2_extension.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/testsuite.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/version.h rename mavlink/include/mavlink/v1.0/{pixhawk => minimal}/mavlink.h (74%) create mode 100644 mavlink/include/mavlink/v1.0/minimal/mavlink_msg_heartbeat.h create mode 100644 mavlink/include/mavlink/v1.0/minimal/minimal.h create mode 100644 mavlink/include/mavlink/v1.0/minimal/testsuite.h create mode 100644 mavlink/include/mavlink/v1.0/minimal/version.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/pixhawk.pb.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/testsuite.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/version.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/testsuite.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/version.h diff --git a/mavlink/include/mavlink/config.h b/mavlink/include/mavlink/config.h deleted file mode 100644 index db7db0d..0000000 --- a/mavlink/include/mavlink/config.h +++ /dev/null @@ -1 +0,0 @@ -#define MAVLINK_VERSION "1.0.7" diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h deleted file mode 100644 index 2acb796..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h +++ /dev/null @@ -1,205 +0,0 @@ -/** @file - * @brief MAVLink comm protocol generated from ardupilotmega.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef ARDUPILOTMEGA_H -#define ARDUPILOTMEGA_H - -#ifdef __cplusplus -extern "C" { -#endif - -// MESSAGE LENGTHS AND CRCS - -#ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 0, 0, 0, 0, 0, 13, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 0, 24, 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, 0, 0, 36, 30, 18, 18, 51, 9, 0} -#endif - -#ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 0, 0, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 0, 47, 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, 0, 0, 204, 49, 170, 44, 83, 46, 0} -#endif - -#ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"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_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, MAVLINK_MESSAGE_INFO_RALLY_POINT, MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_AHRS2, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} -#endif - -#include "../protocol.h" - -#define MAVLINK_ENABLED_ARDUPILOTMEGA - -// ENUM DEFINITIONS - - -/** @brief Enumeration of possible mount operation modes */ -#ifndef HAVE_ENUM_MAV_MOUNT_MODE -#define HAVE_ENUM_MAV_MOUNT_MODE -enum MAV_MOUNT_MODE -{ - MAV_MOUNT_MODE_RETRACT=0, /* Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization | */ - MAV_MOUNT_MODE_NEUTRAL=1, /* Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM. | */ - MAV_MOUNT_MODE_MAVLINK_TARGETING=2, /* Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | */ - MAV_MOUNT_MODE_RC_TARGETING=3, /* Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | */ - MAV_MOUNT_MODE_GPS_POINT=4, /* Load neutral position and start to point to Lat,Lon,Alt | */ - MAV_MOUNT_MODE_ENUM_END=5, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_MAV_CMD -#define HAVE_ENUM_MAV_CMD -enum MAV_CMD -{ - MAV_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ - MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ - MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ - MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ - MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ - MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ - MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch(deg*100) or lat, depending on mount mode.| roll(deg*100) or lon depending on mount mode| yaw(deg*100) or alt (in cm) depending on mount mode| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ - MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ - MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ - MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ - MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - MAV_CMD_ENUM_END=501, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_FENCE_ACTION -#define HAVE_ENUM_FENCE_ACTION -enum FENCE_ACTION -{ - FENCE_ACTION_NONE=0, /* Disable fenced mode | */ - FENCE_ACTION_GUIDED=1, /* Switched to guided mode to return point (fence point 0) | */ - FENCE_ACTION_REPORT=2, /* Report fence breach, but don't take action | */ - FENCE_ACTION_GUIDED_THR_PASS=3, /* Switched to guided mode to return point (fence point 0) with manual throttle control | */ - FENCE_ACTION_ENUM_END=4, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_FENCE_BREACH -#define HAVE_ENUM_FENCE_BREACH -enum FENCE_BREACH -{ - FENCE_BREACH_NONE=0, /* No last fence breach | */ - FENCE_BREACH_MINALT=1, /* Breached minimum altitude | */ - FENCE_BREACH_MAXALT=2, /* Breached maximum altitude | */ - FENCE_BREACH_BOUNDARY=3, /* Breached fence boundary | */ - FENCE_BREACH_ENUM_END=4, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_LIMITS_STATE -#define HAVE_ENUM_LIMITS_STATE -enum LIMITS_STATE -{ - LIMITS_INIT=0, /* pre-initialization | */ - LIMITS_DISABLED=1, /* disabled | */ - LIMITS_ENABLED=2, /* checking limits | */ - LIMITS_TRIGGERED=3, /* a limit has been breached | */ - LIMITS_RECOVERING=4, /* taking action eg. RTL | */ - LIMITS_RECOVERED=5, /* we're no longer in breach of a limit | */ - LIMITS_STATE_ENUM_END=6, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_LIMIT_MODULE -#define HAVE_ENUM_LIMIT_MODULE -enum LIMIT_MODULE -{ - LIMIT_GPSLOCK=1, /* pre-initialization | */ - LIMIT_GEOFENCE=2, /* disabled | */ - LIMIT_ALTITUDE=4, /* checking limits | */ - LIMIT_MODULE_ENUM_END=5, /* | */ -}; -#endif - -/** @brief Flags in RALLY_POINT message */ -#ifndef HAVE_ENUM_RALLY_FLAGS -#define HAVE_ENUM_RALLY_FLAGS -enum RALLY_FLAGS -{ - FAVORABLE_WIND=1, /* Flag set when requiring favorable winds for landing. | */ - LAND_IMMEDIATELY=2, /* Flag set when plane is to immediately descend to break altitude and land without GCS intervention. Flag not set when plane is to loiter at Rally point until commanded to land. | */ - RALLY_FLAGS_ENUM_END=3, /* | */ -}; -#endif - -#include "../common/common.h" - -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -// MESSAGE DEFINITIONS -#include "./mavlink_msg_sensor_offsets.h" -#include "./mavlink_msg_set_mag_offsets.h" -#include "./mavlink_msg_meminfo.h" -#include "./mavlink_msg_ap_adc.h" -#include "./mavlink_msg_digicam_configure.h" -#include "./mavlink_msg_digicam_control.h" -#include "./mavlink_msg_mount_configure.h" -#include "./mavlink_msg_mount_control.h" -#include "./mavlink_msg_mount_status.h" -#include "./mavlink_msg_fence_point.h" -#include "./mavlink_msg_fence_fetch_point.h" -#include "./mavlink_msg_fence_status.h" -#include "./mavlink_msg_ahrs.h" -#include "./mavlink_msg_simstate.h" -#include "./mavlink_msg_hwstatus.h" -#include "./mavlink_msg_radio.h" -#include "./mavlink_msg_limits_status.h" -#include "./mavlink_msg_wind.h" -#include "./mavlink_msg_data16.h" -#include "./mavlink_msg_data32.h" -#include "./mavlink_msg_data64.h" -#include "./mavlink_msg_data96.h" -#include "./mavlink_msg_rangefinder.h" -#include "./mavlink_msg_airspeed_autocal.h" -#include "./mavlink_msg_rally_point.h" -#include "./mavlink_msg_rally_fetch_point.h" -#include "./mavlink_msg_ahrs2.h" - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // ARDUPILOTMEGA_H diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink.h deleted file mode 100644 index 551938f..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink.h +++ /dev/null @@ -1,27 +0,0 @@ -/** @file - * @brief MAVLink comm protocol built from ardupilotmega.xml - * @see http://pixhawk.ethz.ch/software/mavlink - */ -#ifndef MAVLINK_H -#define MAVLINK_H - -#ifndef MAVLINK_STX -#define MAVLINK_STX 254 -#endif - -#ifndef MAVLINK_ENDIAN -#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN -#endif - -#ifndef MAVLINK_ALIGNED_FIELDS -#define MAVLINK_ALIGNED_FIELDS 1 -#endif - -#ifndef MAVLINK_CRC_EXTRA -#define MAVLINK_CRC_EXTRA 1 -#endif - -#include "version.h" -#include "ardupilotmega.h" - -#endif // MAVLINK_H diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h deleted file mode 100644 index c3ead11..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h +++ /dev/null @@ -1,309 +0,0 @@ -// MESSAGE AHRS PACKING - -#define MAVLINK_MSG_ID_AHRS 163 - -typedef struct __mavlink_ahrs_t -{ - float omegaIx; ///< X gyro drift estimate rad/s - float omegaIy; ///< Y gyro drift estimate rad/s - float omegaIz; ///< Z gyro drift estimate rad/s - float accel_weight; ///< average accel_weight - float renorm_val; ///< average renormalisation value - float error_rp; ///< average error_roll_pitch value - float error_yaw; ///< average error_yaw value -} mavlink_ahrs_t; - -#define MAVLINK_MSG_ID_AHRS_LEN 28 -#define MAVLINK_MSG_ID_163_LEN 28 - -#define MAVLINK_MSG_ID_AHRS_CRC 127 -#define MAVLINK_MSG_ID_163_CRC 127 - - - -#define MAVLINK_MESSAGE_INFO_AHRS { \ - "AHRS", \ - 7, \ - { { "omegaIx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ahrs_t, omegaIx) }, \ - { "omegaIy", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ahrs_t, omegaIy) }, \ - { "omegaIz", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ahrs_t, omegaIz) }, \ - { "accel_weight", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ahrs_t, accel_weight) }, \ - { "renorm_val", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_ahrs_t, renorm_val) }, \ - { "error_rp", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_ahrs_t, error_rp) }, \ - { "error_yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_ahrs_t, error_yaw) }, \ - } \ -} - - -/** - * @brief Pack a ahrs message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param omegaIx X gyro drift estimate rad/s - * @param omegaIy Y gyro drift estimate rad/s - * @param omegaIz Z gyro drift estimate rad/s - * @param accel_weight average accel_weight - * @param renorm_val average renormalisation value - * @param error_rp average error_roll_pitch value - * @param error_yaw average error_yaw value - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ahrs_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AHRS_LEN]; - _mav_put_float(buf, 0, omegaIx); - _mav_put_float(buf, 4, omegaIy); - _mav_put_float(buf, 8, omegaIz); - _mav_put_float(buf, 12, accel_weight); - _mav_put_float(buf, 16, renorm_val); - _mav_put_float(buf, 20, error_rp); - _mav_put_float(buf, 24, error_yaw); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS_LEN); -#else - mavlink_ahrs_t packet; - packet.omegaIx = omegaIx; - packet.omegaIy = omegaIy; - packet.omegaIz = omegaIz; - packet.accel_weight = accel_weight; - packet.renorm_val = renorm_val; - packet.error_rp = error_rp; - packet.error_yaw = error_yaw; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AHRS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS_LEN); -#endif -} - -/** - * @brief Pack a ahrs message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param omegaIx X gyro drift estimate rad/s - * @param omegaIy Y gyro drift estimate rad/s - * @param omegaIz Z gyro drift estimate rad/s - * @param accel_weight average accel_weight - * @param renorm_val average renormalisation value - * @param error_rp average error_roll_pitch value - * @param error_yaw average error_yaw value - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ahrs_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float omegaIx,float omegaIy,float omegaIz,float accel_weight,float renorm_val,float error_rp,float error_yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AHRS_LEN]; - _mav_put_float(buf, 0, omegaIx); - _mav_put_float(buf, 4, omegaIy); - _mav_put_float(buf, 8, omegaIz); - _mav_put_float(buf, 12, accel_weight); - _mav_put_float(buf, 16, renorm_val); - _mav_put_float(buf, 20, error_rp); - _mav_put_float(buf, 24, error_yaw); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS_LEN); -#else - mavlink_ahrs_t packet; - packet.omegaIx = omegaIx; - packet.omegaIy = omegaIy; - packet.omegaIz = omegaIz; - packet.accel_weight = accel_weight; - packet.renorm_val = renorm_val; - packet.error_rp = error_rp; - packet.error_yaw = error_yaw; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AHRS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS_LEN); -#endif -} - -/** - * @brief Encode a ahrs struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param ahrs C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ahrs_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs_t* ahrs) -{ - return mavlink_msg_ahrs_pack(system_id, component_id, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw); -} - -/** - * @brief Encode a ahrs struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param ahrs C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ahrs_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ahrs_t* ahrs) -{ - return mavlink_msg_ahrs_pack_chan(system_id, component_id, chan, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw); -} - -/** - * @brief Send a ahrs message - * @param chan MAVLink channel to send the message - * - * @param omegaIx X gyro drift estimate rad/s - * @param omegaIy Y gyro drift estimate rad/s - * @param omegaIz Z gyro drift estimate rad/s - * @param accel_weight average accel_weight - * @param renorm_val average renormalisation value - * @param error_rp average error_roll_pitch value - * @param error_yaw average error_yaw value - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_ahrs_send(mavlink_channel_t chan, float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AHRS_LEN]; - _mav_put_float(buf, 0, omegaIx); - _mav_put_float(buf, 4, omegaIy); - _mav_put_float(buf, 8, omegaIz); - _mav_put_float(buf, 12, accel_weight); - _mav_put_float(buf, 16, renorm_val); - _mav_put_float(buf, 20, error_rp); - _mav_put_float(buf, 24, error_yaw); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN); -#endif -#else - mavlink_ahrs_t packet; - packet.omegaIx = omegaIx; - packet.omegaIy = omegaIy; - packet.omegaIz = omegaIz; - packet.accel_weight = accel_weight; - packet.renorm_val = renorm_val; - packet.error_rp = error_rp; - packet.error_yaw = error_yaw; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)&packet, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)&packet, MAVLINK_MSG_ID_AHRS_LEN); -#endif -#endif -} - -#endif - -// MESSAGE AHRS UNPACKING - - -/** - * @brief Get field omegaIx from ahrs message - * - * @return X gyro drift estimate rad/s - */ -static inline float mavlink_msg_ahrs_get_omegaIx(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field omegaIy from ahrs message - * - * @return Y gyro drift estimate rad/s - */ -static inline float mavlink_msg_ahrs_get_omegaIy(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field omegaIz from ahrs message - * - * @return Z gyro drift estimate rad/s - */ -static inline float mavlink_msg_ahrs_get_omegaIz(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field accel_weight from ahrs message - * - * @return average accel_weight - */ -static inline float mavlink_msg_ahrs_get_accel_weight(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field renorm_val from ahrs message - * - * @return average renormalisation value - */ -static inline float mavlink_msg_ahrs_get_renorm_val(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field error_rp from ahrs message - * - * @return average error_roll_pitch value - */ -static inline float mavlink_msg_ahrs_get_error_rp(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field error_yaw from ahrs message - * - * @return average error_yaw value - */ -static inline float mavlink_msg_ahrs_get_error_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Decode a ahrs message into a struct - * - * @param msg The message to decode - * @param ahrs C-struct to decode the message contents into - */ -static inline void mavlink_msg_ahrs_decode(const mavlink_message_t* msg, mavlink_ahrs_t* ahrs) -{ -#if MAVLINK_NEED_BYTE_SWAP - ahrs->omegaIx = mavlink_msg_ahrs_get_omegaIx(msg); - ahrs->omegaIy = mavlink_msg_ahrs_get_omegaIy(msg); - ahrs->omegaIz = mavlink_msg_ahrs_get_omegaIz(msg); - ahrs->accel_weight = mavlink_msg_ahrs_get_accel_weight(msg); - ahrs->renorm_val = mavlink_msg_ahrs_get_renorm_val(msg); - ahrs->error_rp = mavlink_msg_ahrs_get_error_rp(msg); - ahrs->error_yaw = mavlink_msg_ahrs_get_error_yaw(msg); -#else - memcpy(ahrs, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AHRS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h deleted file mode 100644 index f6fde95..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h +++ /dev/null @@ -1,287 +0,0 @@ -// MESSAGE AHRS2 PACKING - -#define MAVLINK_MSG_ID_AHRS2 178 - -typedef struct __mavlink_ahrs2_t -{ - float roll; ///< Roll angle (rad) - float pitch; ///< Pitch angle (rad) - float yaw; ///< Yaw angle (rad) - float altitude; ///< Altitude (MSL) - int32_t lat; ///< Latitude in degrees * 1E7 - int32_t lng; ///< Longitude in degrees * 1E7 -} mavlink_ahrs2_t; - -#define MAVLINK_MSG_ID_AHRS2_LEN 24 -#define MAVLINK_MSG_ID_178_LEN 24 - -#define MAVLINK_MSG_ID_AHRS2_CRC 47 -#define MAVLINK_MSG_ID_178_CRC 47 - - - -#define MAVLINK_MESSAGE_INFO_AHRS2 { \ - "AHRS2", \ - 6, \ - { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ahrs2_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ahrs2_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ahrs2_t, yaw) }, \ - { "altitude", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ahrs2_t, altitude) }, \ - { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_ahrs2_t, lat) }, \ - { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_ahrs2_t, lng) }, \ - } \ -} - - -/** - * @brief Pack a ahrs2 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param altitude Altitude (MSL) - * @param lat Latitude in degrees * 1E7 - * @param lng Longitude in degrees * 1E7 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ahrs2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AHRS2_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, altitude); - _mav_put_int32_t(buf, 16, lat); - _mav_put_int32_t(buf, 20, lng); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS2_LEN); -#else - mavlink_ahrs2_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.altitude = altitude; - packet.lat = lat; - packet.lng = lng; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS2_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AHRS2; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS2_LEN); -#endif -} - -/** - * @brief Pack a ahrs2 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param altitude Altitude (MSL) - * @param lat Latitude in degrees * 1E7 - * @param lng Longitude in degrees * 1E7 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ahrs2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float roll,float pitch,float yaw,float altitude,int32_t lat,int32_t lng) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AHRS2_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, altitude); - _mav_put_int32_t(buf, 16, lat); - _mav_put_int32_t(buf, 20, lng); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS2_LEN); -#else - mavlink_ahrs2_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.altitude = altitude; - packet.lat = lat; - packet.lng = lng; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS2_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AHRS2; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS2_LEN); -#endif -} - -/** - * @brief Encode a ahrs2 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param ahrs2 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ahrs2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs2_t* ahrs2) -{ - return mavlink_msg_ahrs2_pack(system_id, component_id, msg, ahrs2->roll, ahrs2->pitch, ahrs2->yaw, ahrs2->altitude, ahrs2->lat, ahrs2->lng); -} - -/** - * @brief Encode a ahrs2 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param ahrs2 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ahrs2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ahrs2_t* ahrs2) -{ - return mavlink_msg_ahrs2_pack_chan(system_id, component_id, chan, msg, ahrs2->roll, ahrs2->pitch, ahrs2->yaw, ahrs2->altitude, ahrs2->lat, ahrs2->lng); -} - -/** - * @brief Send a ahrs2 message - * @param chan MAVLink channel to send the message - * - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param altitude Altitude (MSL) - * @param lat Latitude in degrees * 1E7 - * @param lng Longitude in degrees * 1E7 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_ahrs2_send(mavlink_channel_t chan, float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AHRS2_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, altitude); - _mav_put_int32_t(buf, 16, lat); - _mav_put_int32_t(buf, 20, lng); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN); -#endif -#else - mavlink_ahrs2_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.altitude = altitude; - packet.lat = lat; - packet.lng = lng; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)&packet, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)&packet, MAVLINK_MSG_ID_AHRS2_LEN); -#endif -#endif -} - -#endif - -// MESSAGE AHRS2 UNPACKING - - -/** - * @brief Get field roll from ahrs2 message - * - * @return Roll angle (rad) - */ -static inline float mavlink_msg_ahrs2_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field pitch from ahrs2 message - * - * @return Pitch angle (rad) - */ -static inline float mavlink_msg_ahrs2_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field yaw from ahrs2 message - * - * @return Yaw angle (rad) - */ -static inline float mavlink_msg_ahrs2_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field altitude from ahrs2 message - * - * @return Altitude (MSL) - */ -static inline float mavlink_msg_ahrs2_get_altitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field lat from ahrs2 message - * - * @return Latitude in degrees * 1E7 - */ -static inline int32_t mavlink_msg_ahrs2_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 16); -} - -/** - * @brief Get field lng from ahrs2 message - * - * @return Longitude in degrees * 1E7 - */ -static inline int32_t mavlink_msg_ahrs2_get_lng(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 20); -} - -/** - * @brief Decode a ahrs2 message into a struct - * - * @param msg The message to decode - * @param ahrs2 C-struct to decode the message contents into - */ -static inline void mavlink_msg_ahrs2_decode(const mavlink_message_t* msg, mavlink_ahrs2_t* ahrs2) -{ -#if MAVLINK_NEED_BYTE_SWAP - ahrs2->roll = mavlink_msg_ahrs2_get_roll(msg); - ahrs2->pitch = mavlink_msg_ahrs2_get_pitch(msg); - ahrs2->yaw = mavlink_msg_ahrs2_get_yaw(msg); - ahrs2->altitude = mavlink_msg_ahrs2_get_altitude(msg); - ahrs2->lat = mavlink_msg_ahrs2_get_lat(msg); - ahrs2->lng = mavlink_msg_ahrs2_get_lng(msg); -#else - memcpy(ahrs2, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AHRS2_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h deleted file mode 100644 index d046f2a..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h +++ /dev/null @@ -1,419 +0,0 @@ -// MESSAGE AIRSPEED_AUTOCAL PACKING - -#define MAVLINK_MSG_ID_AIRSPEED_AUTOCAL 174 - -typedef struct __mavlink_airspeed_autocal_t -{ - float vx; ///< GPS velocity north m/s - float vy; ///< GPS velocity east m/s - float vz; ///< GPS velocity down m/s - float diff_pressure; ///< Differential pressure pascals - float EAS2TAS; ///< Estimated to true airspeed ratio - float ratio; ///< Airspeed ratio - float state_x; ///< EKF state x - float state_y; ///< EKF state y - float state_z; ///< EKF state z - float Pax; ///< EKF Pax - float Pby; ///< EKF Pby - float Pcz; ///< EKF Pcz -} mavlink_airspeed_autocal_t; - -#define MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN 48 -#define MAVLINK_MSG_ID_174_LEN 48 - -#define MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC 167 -#define MAVLINK_MSG_ID_174_CRC 167 - - - -#define MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL { \ - "AIRSPEED_AUTOCAL", \ - 12, \ - { { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_airspeed_autocal_t, vx) }, \ - { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_airspeed_autocal_t, vy) }, \ - { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_airspeed_autocal_t, vz) }, \ - { "diff_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_airspeed_autocal_t, diff_pressure) }, \ - { "EAS2TAS", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_airspeed_autocal_t, EAS2TAS) }, \ - { "ratio", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_airspeed_autocal_t, ratio) }, \ - { "state_x", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_airspeed_autocal_t, state_x) }, \ - { "state_y", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_airspeed_autocal_t, state_y) }, \ - { "state_z", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_airspeed_autocal_t, state_z) }, \ - { "Pax", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_airspeed_autocal_t, Pax) }, \ - { "Pby", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_airspeed_autocal_t, Pby) }, \ - { "Pcz", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_airspeed_autocal_t, Pcz) }, \ - } \ -} - - -/** - * @brief Pack a airspeed_autocal message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param vx GPS velocity north m/s - * @param vy GPS velocity east m/s - * @param vz GPS velocity down m/s - * @param diff_pressure Differential pressure pascals - * @param EAS2TAS Estimated to true airspeed ratio - * @param ratio Airspeed ratio - * @param state_x EKF state x - * @param state_y EKF state y - * @param state_z EKF state z - * @param Pax EKF Pax - * @param Pby EKF Pby - * @param Pcz EKF Pcz - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_airspeed_autocal_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float vx, float vy, float vz, float diff_pressure, float EAS2TAS, float ratio, float state_x, float state_y, float state_z, float Pax, float Pby, float Pcz) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN]; - _mav_put_float(buf, 0, vx); - _mav_put_float(buf, 4, vy); - _mav_put_float(buf, 8, vz); - _mav_put_float(buf, 12, diff_pressure); - _mav_put_float(buf, 16, EAS2TAS); - _mav_put_float(buf, 20, ratio); - _mav_put_float(buf, 24, state_x); - _mav_put_float(buf, 28, state_y); - _mav_put_float(buf, 32, state_z); - _mav_put_float(buf, 36, Pax); - _mav_put_float(buf, 40, Pby); - _mav_put_float(buf, 44, Pcz); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); -#else - mavlink_airspeed_autocal_t packet; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - packet.diff_pressure = diff_pressure; - packet.EAS2TAS = EAS2TAS; - packet.ratio = ratio; - packet.state_x = state_x; - packet.state_y = state_y; - packet.state_z = state_z; - packet.Pax = Pax; - packet.Pby = Pby; - packet.Pcz = Pcz; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AIRSPEED_AUTOCAL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); -#endif -} - -/** - * @brief Pack a airspeed_autocal message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param vx GPS velocity north m/s - * @param vy GPS velocity east m/s - * @param vz GPS velocity down m/s - * @param diff_pressure Differential pressure pascals - * @param EAS2TAS Estimated to true airspeed ratio - * @param ratio Airspeed ratio - * @param state_x EKF state x - * @param state_y EKF state y - * @param state_z EKF state z - * @param Pax EKF Pax - * @param Pby EKF Pby - * @param Pcz EKF Pcz - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_airspeed_autocal_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float vx,float vy,float vz,float diff_pressure,float EAS2TAS,float ratio,float state_x,float state_y,float state_z,float Pax,float Pby,float Pcz) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN]; - _mav_put_float(buf, 0, vx); - _mav_put_float(buf, 4, vy); - _mav_put_float(buf, 8, vz); - _mav_put_float(buf, 12, diff_pressure); - _mav_put_float(buf, 16, EAS2TAS); - _mav_put_float(buf, 20, ratio); - _mav_put_float(buf, 24, state_x); - _mav_put_float(buf, 28, state_y); - _mav_put_float(buf, 32, state_z); - _mav_put_float(buf, 36, Pax); - _mav_put_float(buf, 40, Pby); - _mav_put_float(buf, 44, Pcz); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); -#else - mavlink_airspeed_autocal_t packet; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - packet.diff_pressure = diff_pressure; - packet.EAS2TAS = EAS2TAS; - packet.ratio = ratio; - packet.state_x = state_x; - packet.state_y = state_y; - packet.state_z = state_z; - packet.Pax = Pax; - packet.Pby = Pby; - packet.Pcz = Pcz; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AIRSPEED_AUTOCAL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); -#endif -} - -/** - * @brief Encode a airspeed_autocal struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param airspeed_autocal C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_airspeed_autocal_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_airspeed_autocal_t* airspeed_autocal) -{ - return mavlink_msg_airspeed_autocal_pack(system_id, component_id, msg, airspeed_autocal->vx, airspeed_autocal->vy, airspeed_autocal->vz, airspeed_autocal->diff_pressure, airspeed_autocal->EAS2TAS, airspeed_autocal->ratio, airspeed_autocal->state_x, airspeed_autocal->state_y, airspeed_autocal->state_z, airspeed_autocal->Pax, airspeed_autocal->Pby, airspeed_autocal->Pcz); -} - -/** - * @brief Encode a airspeed_autocal struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param airspeed_autocal C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_airspeed_autocal_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_airspeed_autocal_t* airspeed_autocal) -{ - return mavlink_msg_airspeed_autocal_pack_chan(system_id, component_id, chan, msg, airspeed_autocal->vx, airspeed_autocal->vy, airspeed_autocal->vz, airspeed_autocal->diff_pressure, airspeed_autocal->EAS2TAS, airspeed_autocal->ratio, airspeed_autocal->state_x, airspeed_autocal->state_y, airspeed_autocal->state_z, airspeed_autocal->Pax, airspeed_autocal->Pby, airspeed_autocal->Pcz); -} - -/** - * @brief Send a airspeed_autocal message - * @param chan MAVLink channel to send the message - * - * @param vx GPS velocity north m/s - * @param vy GPS velocity east m/s - * @param vz GPS velocity down m/s - * @param diff_pressure Differential pressure pascals - * @param EAS2TAS Estimated to true airspeed ratio - * @param ratio Airspeed ratio - * @param state_x EKF state x - * @param state_y EKF state y - * @param state_z EKF state z - * @param Pax EKF Pax - * @param Pby EKF Pby - * @param Pcz EKF Pcz - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_airspeed_autocal_send(mavlink_channel_t chan, float vx, float vy, float vz, float diff_pressure, float EAS2TAS, float ratio, float state_x, float state_y, float state_z, float Pax, float Pby, float Pcz) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN]; - _mav_put_float(buf, 0, vx); - _mav_put_float(buf, 4, vy); - _mav_put_float(buf, 8, vz); - _mav_put_float(buf, 12, diff_pressure); - _mav_put_float(buf, 16, EAS2TAS); - _mav_put_float(buf, 20, ratio); - _mav_put_float(buf, 24, state_x); - _mav_put_float(buf, 28, state_y); - _mav_put_float(buf, 32, state_z); - _mav_put_float(buf, 36, Pax); - _mav_put_float(buf, 40, Pby); - _mav_put_float(buf, 44, Pcz); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); -#endif -#else - mavlink_airspeed_autocal_t packet; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - packet.diff_pressure = diff_pressure; - packet.EAS2TAS = EAS2TAS; - packet.ratio = ratio; - packet.state_x = state_x; - packet.state_y = state_y; - packet.state_z = state_z; - packet.Pax = Pax; - packet.Pby = Pby; - packet.Pcz = Pcz; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)&packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)&packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); -#endif -#endif -} - -#endif - -// MESSAGE AIRSPEED_AUTOCAL UNPACKING - - -/** - * @brief Get field vx from airspeed_autocal message - * - * @return GPS velocity north m/s - */ -static inline float mavlink_msg_airspeed_autocal_get_vx(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field vy from airspeed_autocal message - * - * @return GPS velocity east m/s - */ -static inline float mavlink_msg_airspeed_autocal_get_vy(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field vz from airspeed_autocal message - * - * @return GPS velocity down m/s - */ -static inline float mavlink_msg_airspeed_autocal_get_vz(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field diff_pressure from airspeed_autocal message - * - * @return Differential pressure pascals - */ -static inline float mavlink_msg_airspeed_autocal_get_diff_pressure(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field EAS2TAS from airspeed_autocal message - * - * @return Estimated to true airspeed ratio - */ -static inline float mavlink_msg_airspeed_autocal_get_EAS2TAS(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field ratio from airspeed_autocal message - * - * @return Airspeed ratio - */ -static inline float mavlink_msg_airspeed_autocal_get_ratio(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field state_x from airspeed_autocal message - * - * @return EKF state x - */ -static inline float mavlink_msg_airspeed_autocal_get_state_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field state_y from airspeed_autocal message - * - * @return EKF state y - */ -static inline float mavlink_msg_airspeed_autocal_get_state_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Get field state_z from airspeed_autocal message - * - * @return EKF state z - */ -static inline float mavlink_msg_airspeed_autocal_get_state_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 32); -} - -/** - * @brief Get field Pax from airspeed_autocal message - * - * @return EKF Pax - */ -static inline float mavlink_msg_airspeed_autocal_get_Pax(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 36); -} - -/** - * @brief Get field Pby from airspeed_autocal message - * - * @return EKF Pby - */ -static inline float mavlink_msg_airspeed_autocal_get_Pby(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 40); -} - -/** - * @brief Get field Pcz from airspeed_autocal message - * - * @return EKF Pcz - */ -static inline float mavlink_msg_airspeed_autocal_get_Pcz(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 44); -} - -/** - * @brief Decode a airspeed_autocal message into a struct - * - * @param msg The message to decode - * @param airspeed_autocal C-struct to decode the message contents into - */ -static inline void mavlink_msg_airspeed_autocal_decode(const mavlink_message_t* msg, mavlink_airspeed_autocal_t* airspeed_autocal) -{ -#if MAVLINK_NEED_BYTE_SWAP - airspeed_autocal->vx = mavlink_msg_airspeed_autocal_get_vx(msg); - airspeed_autocal->vy = mavlink_msg_airspeed_autocal_get_vy(msg); - airspeed_autocal->vz = mavlink_msg_airspeed_autocal_get_vz(msg); - airspeed_autocal->diff_pressure = mavlink_msg_airspeed_autocal_get_diff_pressure(msg); - airspeed_autocal->EAS2TAS = mavlink_msg_airspeed_autocal_get_EAS2TAS(msg); - airspeed_autocal->ratio = mavlink_msg_airspeed_autocal_get_ratio(msg); - airspeed_autocal->state_x = mavlink_msg_airspeed_autocal_get_state_x(msg); - airspeed_autocal->state_y = mavlink_msg_airspeed_autocal_get_state_y(msg); - airspeed_autocal->state_z = mavlink_msg_airspeed_autocal_get_state_z(msg); - airspeed_autocal->Pax = mavlink_msg_airspeed_autocal_get_Pax(msg); - airspeed_autocal->Pby = mavlink_msg_airspeed_autocal_get_Pby(msg); - airspeed_autocal->Pcz = mavlink_msg_airspeed_autocal_get_Pcz(msg); -#else - memcpy(airspeed_autocal, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h deleted file mode 100644 index 821ce73..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h +++ /dev/null @@ -1,287 +0,0 @@ -// MESSAGE AP_ADC PACKING - -#define MAVLINK_MSG_ID_AP_ADC 153 - -typedef struct __mavlink_ap_adc_t -{ - uint16_t adc1; ///< ADC output 1 - uint16_t adc2; ///< ADC output 2 - uint16_t adc3; ///< ADC output 3 - uint16_t adc4; ///< ADC output 4 - uint16_t adc5; ///< ADC output 5 - uint16_t adc6; ///< ADC output 6 -} mavlink_ap_adc_t; - -#define MAVLINK_MSG_ID_AP_ADC_LEN 12 -#define MAVLINK_MSG_ID_153_LEN 12 - -#define MAVLINK_MSG_ID_AP_ADC_CRC 188 -#define MAVLINK_MSG_ID_153_CRC 188 - - - -#define MAVLINK_MESSAGE_INFO_AP_ADC { \ - "AP_ADC", \ - 6, \ - { { "adc1", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_ap_adc_t, adc1) }, \ - { "adc2", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_ap_adc_t, adc2) }, \ - { "adc3", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_ap_adc_t, adc3) }, \ - { "adc4", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_ap_adc_t, adc4) }, \ - { "adc5", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_ap_adc_t, adc5) }, \ - { "adc6", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_ap_adc_t, adc6) }, \ - } \ -} - - -/** - * @brief Pack a ap_adc message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param adc1 ADC output 1 - * @param adc2 ADC output 2 - * @param adc3 ADC output 3 - * @param adc4 ADC output 4 - * @param adc5 ADC output 5 - * @param adc6 ADC output 6 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ap_adc_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AP_ADC_LEN]; - _mav_put_uint16_t(buf, 0, adc1); - _mav_put_uint16_t(buf, 2, adc2); - _mav_put_uint16_t(buf, 4, adc3); - _mav_put_uint16_t(buf, 6, adc4); - _mav_put_uint16_t(buf, 8, adc5); - _mav_put_uint16_t(buf, 10, adc6); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AP_ADC_LEN); -#else - mavlink_ap_adc_t packet; - packet.adc1 = adc1; - packet.adc2 = adc2; - packet.adc3 = adc3; - packet.adc4 = adc4; - packet.adc5 = adc5; - packet.adc6 = adc6; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AP_ADC_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AP_ADC; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AP_ADC_LEN); -#endif -} - -/** - * @brief Pack a ap_adc message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param adc1 ADC output 1 - * @param adc2 ADC output 2 - * @param adc3 ADC output 3 - * @param adc4 ADC output 4 - * @param adc5 ADC output 5 - * @param adc6 ADC output 6 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ap_adc_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t adc1,uint16_t adc2,uint16_t adc3,uint16_t adc4,uint16_t adc5,uint16_t adc6) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AP_ADC_LEN]; - _mav_put_uint16_t(buf, 0, adc1); - _mav_put_uint16_t(buf, 2, adc2); - _mav_put_uint16_t(buf, 4, adc3); - _mav_put_uint16_t(buf, 6, adc4); - _mav_put_uint16_t(buf, 8, adc5); - _mav_put_uint16_t(buf, 10, adc6); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AP_ADC_LEN); -#else - mavlink_ap_adc_t packet; - packet.adc1 = adc1; - packet.adc2 = adc2; - packet.adc3 = adc3; - packet.adc4 = adc4; - packet.adc5 = adc5; - packet.adc6 = adc6; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AP_ADC_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AP_ADC; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AP_ADC_LEN); -#endif -} - -/** - * @brief Encode a ap_adc struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param ap_adc C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ap_adc_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ap_adc_t* ap_adc) -{ - return mavlink_msg_ap_adc_pack(system_id, component_id, msg, ap_adc->adc1, ap_adc->adc2, ap_adc->adc3, ap_adc->adc4, ap_adc->adc5, ap_adc->adc6); -} - -/** - * @brief Encode a ap_adc struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param ap_adc C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ap_adc_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ap_adc_t* ap_adc) -{ - return mavlink_msg_ap_adc_pack_chan(system_id, component_id, chan, msg, ap_adc->adc1, ap_adc->adc2, ap_adc->adc3, ap_adc->adc4, ap_adc->adc5, ap_adc->adc6); -} - -/** - * @brief Send a ap_adc message - * @param chan MAVLink channel to send the message - * - * @param adc1 ADC output 1 - * @param adc2 ADC output 2 - * @param adc3 ADC output 3 - * @param adc4 ADC output 4 - * @param adc5 ADC output 5 - * @param adc6 ADC output 6 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_ap_adc_send(mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AP_ADC_LEN]; - _mav_put_uint16_t(buf, 0, adc1); - _mav_put_uint16_t(buf, 2, adc2); - _mav_put_uint16_t(buf, 4, adc3); - _mav_put_uint16_t(buf, 6, adc4); - _mav_put_uint16_t(buf, 8, adc5); - _mav_put_uint16_t(buf, 10, adc6); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, MAVLINK_MSG_ID_AP_ADC_LEN); -#endif -#else - mavlink_ap_adc_t packet; - packet.adc1 = adc1; - packet.adc2 = adc2; - packet.adc3 = adc3; - packet.adc4 = adc4; - packet.adc5 = adc5; - packet.adc6 = adc6; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)&packet, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)&packet, MAVLINK_MSG_ID_AP_ADC_LEN); -#endif -#endif -} - -#endif - -// MESSAGE AP_ADC UNPACKING - - -/** - * @brief Get field adc1 from ap_adc message - * - * @return ADC output 1 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field adc2 from ap_adc message - * - * @return ADC output 2 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Get field adc3 from ap_adc message - * - * @return ADC output 3 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field adc4 from ap_adc message - * - * @return ADC output 4 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Get field adc5 from ap_adc message - * - * @return ADC output 5 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} - -/** - * @brief Get field adc6 from ap_adc message - * - * @return ADC output 6 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 10); -} - -/** - * @brief Decode a ap_adc message into a struct - * - * @param msg The message to decode - * @param ap_adc C-struct to decode the message contents into - */ -static inline void mavlink_msg_ap_adc_decode(const mavlink_message_t* msg, mavlink_ap_adc_t* ap_adc) -{ -#if MAVLINK_NEED_BYTE_SWAP - ap_adc->adc1 = mavlink_msg_ap_adc_get_adc1(msg); - ap_adc->adc2 = mavlink_msg_ap_adc_get_adc2(msg); - ap_adc->adc3 = mavlink_msg_ap_adc_get_adc3(msg); - ap_adc->adc4 = mavlink_msg_ap_adc_get_adc4(msg); - ap_adc->adc5 = mavlink_msg_ap_adc_get_adc5(msg); - ap_adc->adc6 = mavlink_msg_ap_adc_get_adc6(msg); -#else - memcpy(ap_adc, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AP_ADC_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h deleted file mode 100644 index 9200eef..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h +++ /dev/null @@ -1,215 +0,0 @@ -// MESSAGE DATA16 PACKING - -#define MAVLINK_MSG_ID_DATA16 169 - -typedef struct __mavlink_data16_t -{ - uint8_t type; ///< data type - uint8_t len; ///< data length - uint8_t data[16]; ///< raw data -} mavlink_data16_t; - -#define MAVLINK_MSG_ID_DATA16_LEN 18 -#define MAVLINK_MSG_ID_169_LEN 18 - -#define MAVLINK_MSG_ID_DATA16_CRC 234 -#define MAVLINK_MSG_ID_169_CRC 234 - -#define MAVLINK_MSG_DATA16_FIELD_DATA_LEN 16 - -#define MAVLINK_MESSAGE_INFO_DATA16 { \ - "DATA16", \ - 3, \ - { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data16_t, type) }, \ - { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data16_t, len) }, \ - { "data", NULL, MAVLINK_TYPE_UINT8_T, 16, 2, offsetof(mavlink_data16_t, data) }, \ - } \ -} - - -/** - * @brief Pack a data16 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param type data type - * @param len data length - * @param data raw data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data16_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t type, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA16_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA16_LEN); -#else - mavlink_data16_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA16_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA16; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA16_LEN); -#endif -} - -/** - * @brief Pack a data16 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param type data type - * @param len data length - * @param data raw data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data16_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t type,uint8_t len,const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA16_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA16_LEN); -#else - mavlink_data16_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA16_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA16; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA16_LEN); -#endif -} - -/** - * @brief Encode a data16 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param data16 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data16_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data16_t* data16) -{ - return mavlink_msg_data16_pack(system_id, component_id, msg, data16->type, data16->len, data16->data); -} - -/** - * @brief Encode a data16 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param data16 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data16_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data16_t* data16) -{ - return mavlink_msg_data16_pack_chan(system_id, component_id, chan, msg, data16->type, data16->len, data16->data); -} - -/** - * @brief Send a data16 message - * @param chan MAVLink channel to send the message - * - * @param type data type - * @param len data length - * @param data raw data - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_data16_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA16_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, MAVLINK_MSG_ID_DATA16_LEN); -#endif -#else - mavlink_data16_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)&packet, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)&packet, MAVLINK_MSG_ID_DATA16_LEN); -#endif -#endif -} - -#endif - -// MESSAGE DATA16 UNPACKING - - -/** - * @brief Get field type from data16 message - * - * @return data type - */ -static inline uint8_t mavlink_msg_data16_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field len from data16 message - * - * @return data length - */ -static inline uint8_t mavlink_msg_data16_get_len(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field data from data16 message - * - * @return raw data - */ -static inline uint16_t mavlink_msg_data16_get_data(const mavlink_message_t* msg, uint8_t *data) -{ - return _MAV_RETURN_uint8_t_array(msg, data, 16, 2); -} - -/** - * @brief Decode a data16 message into a struct - * - * @param msg The message to decode - * @param data16 C-struct to decode the message contents into - */ -static inline void mavlink_msg_data16_decode(const mavlink_message_t* msg, mavlink_data16_t* data16) -{ -#if MAVLINK_NEED_BYTE_SWAP - data16->type = mavlink_msg_data16_get_type(msg); - data16->len = mavlink_msg_data16_get_len(msg); - mavlink_msg_data16_get_data(msg, data16->data); -#else - memcpy(data16, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA16_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h deleted file mode 100644 index 3afedb7..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h +++ /dev/null @@ -1,215 +0,0 @@ -// MESSAGE DATA32 PACKING - -#define MAVLINK_MSG_ID_DATA32 170 - -typedef struct __mavlink_data32_t -{ - uint8_t type; ///< data type - uint8_t len; ///< data length - uint8_t data[32]; ///< raw data -} mavlink_data32_t; - -#define MAVLINK_MSG_ID_DATA32_LEN 34 -#define MAVLINK_MSG_ID_170_LEN 34 - -#define MAVLINK_MSG_ID_DATA32_CRC 73 -#define MAVLINK_MSG_ID_170_CRC 73 - -#define MAVLINK_MSG_DATA32_FIELD_DATA_LEN 32 - -#define MAVLINK_MESSAGE_INFO_DATA32 { \ - "DATA32", \ - 3, \ - { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data32_t, type) }, \ - { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data32_t, len) }, \ - { "data", NULL, MAVLINK_TYPE_UINT8_T, 32, 2, offsetof(mavlink_data32_t, data) }, \ - } \ -} - - -/** - * @brief Pack a data32 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param type data type - * @param len data length - * @param data raw data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data32_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t type, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA32_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA32_LEN); -#else - mavlink_data32_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA32_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA32; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA32_LEN); -#endif -} - -/** - * @brief Pack a data32 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param type data type - * @param len data length - * @param data raw data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data32_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t type,uint8_t len,const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA32_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA32_LEN); -#else - mavlink_data32_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA32_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA32; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA32_LEN); -#endif -} - -/** - * @brief Encode a data32 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param data32 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data32_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data32_t* data32) -{ - return mavlink_msg_data32_pack(system_id, component_id, msg, data32->type, data32->len, data32->data); -} - -/** - * @brief Encode a data32 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param data32 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data32_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data32_t* data32) -{ - return mavlink_msg_data32_pack_chan(system_id, component_id, chan, msg, data32->type, data32->len, data32->data); -} - -/** - * @brief Send a data32 message - * @param chan MAVLink channel to send the message - * - * @param type data type - * @param len data length - * @param data raw data - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_data32_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA32_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 32); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, MAVLINK_MSG_ID_DATA32_LEN); -#endif -#else - mavlink_data32_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*32); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)&packet, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)&packet, MAVLINK_MSG_ID_DATA32_LEN); -#endif -#endif -} - -#endif - -// MESSAGE DATA32 UNPACKING - - -/** - * @brief Get field type from data32 message - * - * @return data type - */ -static inline uint8_t mavlink_msg_data32_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field len from data32 message - * - * @return data length - */ -static inline uint8_t mavlink_msg_data32_get_len(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field data from data32 message - * - * @return raw data - */ -static inline uint16_t mavlink_msg_data32_get_data(const mavlink_message_t* msg, uint8_t *data) -{ - return _MAV_RETURN_uint8_t_array(msg, data, 32, 2); -} - -/** - * @brief Decode a data32 message into a struct - * - * @param msg The message to decode - * @param data32 C-struct to decode the message contents into - */ -static inline void mavlink_msg_data32_decode(const mavlink_message_t* msg, mavlink_data32_t* data32) -{ -#if MAVLINK_NEED_BYTE_SWAP - data32->type = mavlink_msg_data32_get_type(msg); - data32->len = mavlink_msg_data32_get_len(msg); - mavlink_msg_data32_get_data(msg, data32->data); -#else - memcpy(data32, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA32_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h deleted file mode 100644 index 6931ada..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h +++ /dev/null @@ -1,215 +0,0 @@ -// MESSAGE DATA64 PACKING - -#define MAVLINK_MSG_ID_DATA64 171 - -typedef struct __mavlink_data64_t -{ - uint8_t type; ///< data type - uint8_t len; ///< data length - uint8_t data[64]; ///< raw data -} mavlink_data64_t; - -#define MAVLINK_MSG_ID_DATA64_LEN 66 -#define MAVLINK_MSG_ID_171_LEN 66 - -#define MAVLINK_MSG_ID_DATA64_CRC 181 -#define MAVLINK_MSG_ID_171_CRC 181 - -#define MAVLINK_MSG_DATA64_FIELD_DATA_LEN 64 - -#define MAVLINK_MESSAGE_INFO_DATA64 { \ - "DATA64", \ - 3, \ - { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data64_t, type) }, \ - { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data64_t, len) }, \ - { "data", NULL, MAVLINK_TYPE_UINT8_T, 64, 2, offsetof(mavlink_data64_t, data) }, \ - } \ -} - - -/** - * @brief Pack a data64 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param type data type - * @param len data length - * @param data raw data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data64_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t type, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA64_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 64); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA64_LEN); -#else - mavlink_data64_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*64); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA64_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA64; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA64_LEN); -#endif -} - -/** - * @brief Pack a data64 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param type data type - * @param len data length - * @param data raw data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data64_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t type,uint8_t len,const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA64_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 64); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA64_LEN); -#else - mavlink_data64_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*64); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA64_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA64; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA64_LEN); -#endif -} - -/** - * @brief Encode a data64 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param data64 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data64_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data64_t* data64) -{ - return mavlink_msg_data64_pack(system_id, component_id, msg, data64->type, data64->len, data64->data); -} - -/** - * @brief Encode a data64 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param data64 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data64_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data64_t* data64) -{ - return mavlink_msg_data64_pack_chan(system_id, component_id, chan, msg, data64->type, data64->len, data64->data); -} - -/** - * @brief Send a data64 message - * @param chan MAVLink channel to send the message - * - * @param type data type - * @param len data length - * @param data raw data - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_data64_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA64_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 64); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, buf, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, buf, MAVLINK_MSG_ID_DATA64_LEN); -#endif -#else - mavlink_data64_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*64); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)&packet, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)&packet, MAVLINK_MSG_ID_DATA64_LEN); -#endif -#endif -} - -#endif - -// MESSAGE DATA64 UNPACKING - - -/** - * @brief Get field type from data64 message - * - * @return data type - */ -static inline uint8_t mavlink_msg_data64_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field len from data64 message - * - * @return data length - */ -static inline uint8_t mavlink_msg_data64_get_len(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field data from data64 message - * - * @return raw data - */ -static inline uint16_t mavlink_msg_data64_get_data(const mavlink_message_t* msg, uint8_t *data) -{ - return _MAV_RETURN_uint8_t_array(msg, data, 64, 2); -} - -/** - * @brief Decode a data64 message into a struct - * - * @param msg The message to decode - * @param data64 C-struct to decode the message contents into - */ -static inline void mavlink_msg_data64_decode(const mavlink_message_t* msg, mavlink_data64_t* data64) -{ -#if MAVLINK_NEED_BYTE_SWAP - data64->type = mavlink_msg_data64_get_type(msg); - data64->len = mavlink_msg_data64_get_len(msg); - mavlink_msg_data64_get_data(msg, data64->data); -#else - memcpy(data64, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA64_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h deleted file mode 100644 index cffc7d7..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h +++ /dev/null @@ -1,215 +0,0 @@ -// MESSAGE DATA96 PACKING - -#define MAVLINK_MSG_ID_DATA96 172 - -typedef struct __mavlink_data96_t -{ - uint8_t type; ///< data type - uint8_t len; ///< data length - uint8_t data[96]; ///< raw data -} mavlink_data96_t; - -#define MAVLINK_MSG_ID_DATA96_LEN 98 -#define MAVLINK_MSG_ID_172_LEN 98 - -#define MAVLINK_MSG_ID_DATA96_CRC 22 -#define MAVLINK_MSG_ID_172_CRC 22 - -#define MAVLINK_MSG_DATA96_FIELD_DATA_LEN 96 - -#define MAVLINK_MESSAGE_INFO_DATA96 { \ - "DATA96", \ - 3, \ - { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data96_t, type) }, \ - { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data96_t, len) }, \ - { "data", NULL, MAVLINK_TYPE_UINT8_T, 96, 2, offsetof(mavlink_data96_t, data) }, \ - } \ -} - - -/** - * @brief Pack a data96 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param type data type - * @param len data length - * @param data raw data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data96_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t type, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA96_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 96); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA96_LEN); -#else - mavlink_data96_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*96); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA96_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA96; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA96_LEN); -#endif -} - -/** - * @brief Pack a data96 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param type data type - * @param len data length - * @param data raw data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data96_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t type,uint8_t len,const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA96_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 96); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA96_LEN); -#else - mavlink_data96_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*96); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA96_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA96; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA96_LEN); -#endif -} - -/** - * @brief Encode a data96 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param data96 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data96_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data96_t* data96) -{ - return mavlink_msg_data96_pack(system_id, component_id, msg, data96->type, data96->len, data96->data); -} - -/** - * @brief Encode a data96 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param data96 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data96_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data96_t* data96) -{ - return mavlink_msg_data96_pack_chan(system_id, component_id, chan, msg, data96->type, data96->len, data96->data); -} - -/** - * @brief Send a data96 message - * @param chan MAVLink channel to send the message - * - * @param type data type - * @param len data length - * @param data raw data - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_data96_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA96_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 96); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, buf, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, buf, MAVLINK_MSG_ID_DATA96_LEN); -#endif -#else - mavlink_data96_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*96); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)&packet, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)&packet, MAVLINK_MSG_ID_DATA96_LEN); -#endif -#endif -} - -#endif - -// MESSAGE DATA96 UNPACKING - - -/** - * @brief Get field type from data96 message - * - * @return data type - */ -static inline uint8_t mavlink_msg_data96_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field len from data96 message - * - * @return data length - */ -static inline uint8_t mavlink_msg_data96_get_len(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field data from data96 message - * - * @return raw data - */ -static inline uint16_t mavlink_msg_data96_get_data(const mavlink_message_t* msg, uint8_t *data) -{ - return _MAV_RETURN_uint8_t_array(msg, data, 96, 2); -} - -/** - * @brief Decode a data96 message into a struct - * - * @param msg The message to decode - * @param data96 C-struct to decode the message contents into - */ -static inline void mavlink_msg_data96_decode(const mavlink_message_t* msg, mavlink_data96_t* data96) -{ -#if MAVLINK_NEED_BYTE_SWAP - data96->type = mavlink_msg_data96_get_type(msg); - data96->len = mavlink_msg_data96_get_len(msg); - mavlink_msg_data96_get_data(msg, data96->data); -#else - memcpy(data96, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA96_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h deleted file mode 100644 index c6518c4..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h +++ /dev/null @@ -1,397 +0,0 @@ -// MESSAGE DIGICAM_CONFIGURE PACKING - -#define MAVLINK_MSG_ID_DIGICAM_CONFIGURE 154 - -typedef struct __mavlink_digicam_configure_t -{ - float extra_value; ///< Correspondent value to given extra_param - uint16_t shutter_speed; ///< Divisor number //e.g. 1000 means 1/1000 (0 means ignore) - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t mode; ///< Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) - uint8_t aperture; ///< F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) - uint8_t iso; ///< ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) - uint8_t exposure_type; ///< Exposure type enumeration from 1 to N (0 means ignore) - uint8_t command_id; ///< Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once - uint8_t engine_cut_off; ///< Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) - uint8_t extra_param; ///< Extra parameters enumeration (0 means ignore) -} mavlink_digicam_configure_t; - -#define MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN 15 -#define MAVLINK_MSG_ID_154_LEN 15 - -#define MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC 84 -#define MAVLINK_MSG_ID_154_CRC 84 - - - -#define MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE { \ - "DIGICAM_CONFIGURE", \ - 11, \ - { { "extra_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_digicam_configure_t, extra_value) }, \ - { "shutter_speed", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_digicam_configure_t, shutter_speed) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_digicam_configure_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_digicam_configure_t, target_component) }, \ - { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_digicam_configure_t, mode) }, \ - { "aperture", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_digicam_configure_t, aperture) }, \ - { "iso", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_digicam_configure_t, iso) }, \ - { "exposure_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_digicam_configure_t, exposure_type) }, \ - { "command_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_digicam_configure_t, command_id) }, \ - { "engine_cut_off", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_digicam_configure_t, engine_cut_off) }, \ - { "extra_param", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_digicam_configure_t, extra_param) }, \ - } \ -} - - -/** - * @brief Pack a digicam_configure message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param mode Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) - * @param shutter_speed Divisor number //e.g. 1000 means 1/1000 (0 means ignore) - * @param aperture F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) - * @param iso ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) - * @param exposure_type Exposure type enumeration from 1 to N (0 means ignore) - * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once - * @param engine_cut_off Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) - * @param extra_param Extra parameters enumeration (0 means ignore) - * @param extra_value Correspondent value to given extra_param - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_digicam_configure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t mode, uint16_t shutter_speed, uint8_t aperture, uint8_t iso, uint8_t exposure_type, uint8_t command_id, uint8_t engine_cut_off, uint8_t extra_param, float extra_value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN]; - _mav_put_float(buf, 0, extra_value); - _mav_put_uint16_t(buf, 4, shutter_speed); - _mav_put_uint8_t(buf, 6, target_system); - _mav_put_uint8_t(buf, 7, target_component); - _mav_put_uint8_t(buf, 8, mode); - _mav_put_uint8_t(buf, 9, aperture); - _mav_put_uint8_t(buf, 10, iso); - _mav_put_uint8_t(buf, 11, exposure_type); - _mav_put_uint8_t(buf, 12, command_id); - _mav_put_uint8_t(buf, 13, engine_cut_off); - _mav_put_uint8_t(buf, 14, extra_param); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); -#else - mavlink_digicam_configure_t packet; - packet.extra_value = extra_value; - packet.shutter_speed = shutter_speed; - packet.target_system = target_system; - packet.target_component = target_component; - packet.mode = mode; - packet.aperture = aperture; - packet.iso = iso; - packet.exposure_type = exposure_type; - packet.command_id = command_id; - packet.engine_cut_off = engine_cut_off; - packet.extra_param = extra_param; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONFIGURE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); -#endif -} - -/** - * @brief Pack a digicam_configure message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param mode Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) - * @param shutter_speed Divisor number //e.g. 1000 means 1/1000 (0 means ignore) - * @param aperture F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) - * @param iso ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) - * @param exposure_type Exposure type enumeration from 1 to N (0 means ignore) - * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once - * @param engine_cut_off Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) - * @param extra_param Extra parameters enumeration (0 means ignore) - * @param extra_value Correspondent value to given extra_param - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_digicam_configure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t mode,uint16_t shutter_speed,uint8_t aperture,uint8_t iso,uint8_t exposure_type,uint8_t command_id,uint8_t engine_cut_off,uint8_t extra_param,float extra_value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN]; - _mav_put_float(buf, 0, extra_value); - _mav_put_uint16_t(buf, 4, shutter_speed); - _mav_put_uint8_t(buf, 6, target_system); - _mav_put_uint8_t(buf, 7, target_component); - _mav_put_uint8_t(buf, 8, mode); - _mav_put_uint8_t(buf, 9, aperture); - _mav_put_uint8_t(buf, 10, iso); - _mav_put_uint8_t(buf, 11, exposure_type); - _mav_put_uint8_t(buf, 12, command_id); - _mav_put_uint8_t(buf, 13, engine_cut_off); - _mav_put_uint8_t(buf, 14, extra_param); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); -#else - mavlink_digicam_configure_t packet; - packet.extra_value = extra_value; - packet.shutter_speed = shutter_speed; - packet.target_system = target_system; - packet.target_component = target_component; - packet.mode = mode; - packet.aperture = aperture; - packet.iso = iso; - packet.exposure_type = exposure_type; - packet.command_id = command_id; - packet.engine_cut_off = engine_cut_off; - packet.extra_param = extra_param; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONFIGURE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); -#endif -} - -/** - * @brief Encode a digicam_configure struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param digicam_configure C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_digicam_configure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_digicam_configure_t* digicam_configure) -{ - return mavlink_msg_digicam_configure_pack(system_id, component_id, msg, digicam_configure->target_system, digicam_configure->target_component, digicam_configure->mode, digicam_configure->shutter_speed, digicam_configure->aperture, digicam_configure->iso, digicam_configure->exposure_type, digicam_configure->command_id, digicam_configure->engine_cut_off, digicam_configure->extra_param, digicam_configure->extra_value); -} - -/** - * @brief Encode a digicam_configure struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param digicam_configure C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_digicam_configure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_digicam_configure_t* digicam_configure) -{ - return mavlink_msg_digicam_configure_pack_chan(system_id, component_id, chan, msg, digicam_configure->target_system, digicam_configure->target_component, digicam_configure->mode, digicam_configure->shutter_speed, digicam_configure->aperture, digicam_configure->iso, digicam_configure->exposure_type, digicam_configure->command_id, digicam_configure->engine_cut_off, digicam_configure->extra_param, digicam_configure->extra_value); -} - -/** - * @brief Send a digicam_configure message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param mode Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) - * @param shutter_speed Divisor number //e.g. 1000 means 1/1000 (0 means ignore) - * @param aperture F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) - * @param iso ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) - * @param exposure_type Exposure type enumeration from 1 to N (0 means ignore) - * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once - * @param engine_cut_off Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) - * @param extra_param Extra parameters enumeration (0 means ignore) - * @param extra_value Correspondent value to given extra_param - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_digicam_configure_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mode, uint16_t shutter_speed, uint8_t aperture, uint8_t iso, uint8_t exposure_type, uint8_t command_id, uint8_t engine_cut_off, uint8_t extra_param, float extra_value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN]; - _mav_put_float(buf, 0, extra_value); - _mav_put_uint16_t(buf, 4, shutter_speed); - _mav_put_uint8_t(buf, 6, target_system); - _mav_put_uint8_t(buf, 7, target_component); - _mav_put_uint8_t(buf, 8, mode); - _mav_put_uint8_t(buf, 9, aperture); - _mav_put_uint8_t(buf, 10, iso); - _mav_put_uint8_t(buf, 11, exposure_type); - _mav_put_uint8_t(buf, 12, command_id); - _mav_put_uint8_t(buf, 13, engine_cut_off); - _mav_put_uint8_t(buf, 14, extra_param); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); -#endif -#else - mavlink_digicam_configure_t packet; - packet.extra_value = extra_value; - packet.shutter_speed = shutter_speed; - packet.target_system = target_system; - packet.target_component = target_component; - packet.mode = mode; - packet.aperture = aperture; - packet.iso = iso; - packet.exposure_type = exposure_type; - packet.command_id = command_id; - packet.engine_cut_off = engine_cut_off; - packet.extra_param = extra_param; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)&packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)&packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); -#endif -#endif -} - -#endif - -// MESSAGE DIGICAM_CONFIGURE UNPACKING - - -/** - * @brief Get field target_system from digicam_configure message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_digicam_configure_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @brief Get field target_component from digicam_configure message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_digicam_configure_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 7); -} - -/** - * @brief Get field mode from digicam_configure message - * - * @return Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) - */ -static inline uint8_t mavlink_msg_digicam_configure_get_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 8); -} - -/** - * @brief Get field shutter_speed from digicam_configure message - * - * @return Divisor number //e.g. 1000 means 1/1000 (0 means ignore) - */ -static inline uint16_t mavlink_msg_digicam_configure_get_shutter_speed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field aperture from digicam_configure message - * - * @return F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) - */ -static inline uint8_t mavlink_msg_digicam_configure_get_aperture(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 9); -} - -/** - * @brief Get field iso from digicam_configure message - * - * @return ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) - */ -static inline uint8_t mavlink_msg_digicam_configure_get_iso(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 10); -} - -/** - * @brief Get field exposure_type from digicam_configure message - * - * @return Exposure type enumeration from 1 to N (0 means ignore) - */ -static inline uint8_t mavlink_msg_digicam_configure_get_exposure_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 11); -} - -/** - * @brief Get field command_id from digicam_configure message - * - * @return Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once - */ -static inline uint8_t mavlink_msg_digicam_configure_get_command_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 12); -} - -/** - * @brief Get field engine_cut_off from digicam_configure message - * - * @return Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) - */ -static inline uint8_t mavlink_msg_digicam_configure_get_engine_cut_off(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 13); -} - -/** - * @brief Get field extra_param from digicam_configure message - * - * @return Extra parameters enumeration (0 means ignore) - */ -static inline uint8_t mavlink_msg_digicam_configure_get_extra_param(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 14); -} - -/** - * @brief Get field extra_value from digicam_configure message - * - * @return Correspondent value to given extra_param - */ -static inline float mavlink_msg_digicam_configure_get_extra_value(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Decode a digicam_configure message into a struct - * - * @param msg The message to decode - * @param digicam_configure C-struct to decode the message contents into - */ -static inline void mavlink_msg_digicam_configure_decode(const mavlink_message_t* msg, mavlink_digicam_configure_t* digicam_configure) -{ -#if MAVLINK_NEED_BYTE_SWAP - digicam_configure->extra_value = mavlink_msg_digicam_configure_get_extra_value(msg); - digicam_configure->shutter_speed = mavlink_msg_digicam_configure_get_shutter_speed(msg); - digicam_configure->target_system = mavlink_msg_digicam_configure_get_target_system(msg); - digicam_configure->target_component = mavlink_msg_digicam_configure_get_target_component(msg); - digicam_configure->mode = mavlink_msg_digicam_configure_get_mode(msg); - digicam_configure->aperture = mavlink_msg_digicam_configure_get_aperture(msg); - digicam_configure->iso = mavlink_msg_digicam_configure_get_iso(msg); - digicam_configure->exposure_type = mavlink_msg_digicam_configure_get_exposure_type(msg); - digicam_configure->command_id = mavlink_msg_digicam_configure_get_command_id(msg); - digicam_configure->engine_cut_off = mavlink_msg_digicam_configure_get_engine_cut_off(msg); - digicam_configure->extra_param = mavlink_msg_digicam_configure_get_extra_param(msg); -#else - memcpy(digicam_configure, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h deleted file mode 100644 index bfa5414..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h +++ /dev/null @@ -1,375 +0,0 @@ -// MESSAGE DIGICAM_CONTROL PACKING - -#define MAVLINK_MSG_ID_DIGICAM_CONTROL 155 - -typedef struct __mavlink_digicam_control_t -{ - float extra_value; ///< Correspondent value to given extra_param - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t session; ///< 0: stop, 1: start or keep it up //Session control e.g. show/hide lens - uint8_t zoom_pos; ///< 1 to N //Zoom's absolute position (0 means ignore) - int8_t zoom_step; ///< -100 to 100 //Zooming step value to offset zoom from the current position - uint8_t focus_lock; ///< 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus - uint8_t shot; ///< 0: ignore, 1: shot or start filming - uint8_t command_id; ///< Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once - uint8_t extra_param; ///< Extra parameters enumeration (0 means ignore) -} mavlink_digicam_control_t; - -#define MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN 13 -#define MAVLINK_MSG_ID_155_LEN 13 - -#define MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC 22 -#define MAVLINK_MSG_ID_155_CRC 22 - - - -#define MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL { \ - "DIGICAM_CONTROL", \ - 10, \ - { { "extra_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_digicam_control_t, extra_value) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_digicam_control_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_digicam_control_t, target_component) }, \ - { "session", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_digicam_control_t, session) }, \ - { "zoom_pos", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_digicam_control_t, zoom_pos) }, \ - { "zoom_step", NULL, MAVLINK_TYPE_INT8_T, 0, 8, offsetof(mavlink_digicam_control_t, zoom_step) }, \ - { "focus_lock", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_digicam_control_t, focus_lock) }, \ - { "shot", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_digicam_control_t, shot) }, \ - { "command_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_digicam_control_t, command_id) }, \ - { "extra_param", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_digicam_control_t, extra_param) }, \ - } \ -} - - -/** - * @brief Pack a digicam_control message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param session 0: stop, 1: start or keep it up //Session control e.g. show/hide lens - * @param zoom_pos 1 to N //Zoom's absolute position (0 means ignore) - * @param zoom_step -100 to 100 //Zooming step value to offset zoom from the current position - * @param focus_lock 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus - * @param shot 0: ignore, 1: shot or start filming - * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once - * @param extra_param Extra parameters enumeration (0 means ignore) - * @param extra_value Correspondent value to given extra_param - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_digicam_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t session, uint8_t zoom_pos, int8_t zoom_step, uint8_t focus_lock, uint8_t shot, uint8_t command_id, uint8_t extra_param, float extra_value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN]; - _mav_put_float(buf, 0, extra_value); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - _mav_put_uint8_t(buf, 6, session); - _mav_put_uint8_t(buf, 7, zoom_pos); - _mav_put_int8_t(buf, 8, zoom_step); - _mav_put_uint8_t(buf, 9, focus_lock); - _mav_put_uint8_t(buf, 10, shot); - _mav_put_uint8_t(buf, 11, command_id); - _mav_put_uint8_t(buf, 12, extra_param); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); -#else - mavlink_digicam_control_t packet; - packet.extra_value = extra_value; - packet.target_system = target_system; - packet.target_component = target_component; - packet.session = session; - packet.zoom_pos = zoom_pos; - packet.zoom_step = zoom_step; - packet.focus_lock = focus_lock; - packet.shot = shot; - packet.command_id = command_id; - packet.extra_param = extra_param; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); -#endif -} - -/** - * @brief Pack a digicam_control message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param session 0: stop, 1: start or keep it up //Session control e.g. show/hide lens - * @param zoom_pos 1 to N //Zoom's absolute position (0 means ignore) - * @param zoom_step -100 to 100 //Zooming step value to offset zoom from the current position - * @param focus_lock 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus - * @param shot 0: ignore, 1: shot or start filming - * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once - * @param extra_param Extra parameters enumeration (0 means ignore) - * @param extra_value Correspondent value to given extra_param - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_digicam_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t session,uint8_t zoom_pos,int8_t zoom_step,uint8_t focus_lock,uint8_t shot,uint8_t command_id,uint8_t extra_param,float extra_value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN]; - _mav_put_float(buf, 0, extra_value); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - _mav_put_uint8_t(buf, 6, session); - _mav_put_uint8_t(buf, 7, zoom_pos); - _mav_put_int8_t(buf, 8, zoom_step); - _mav_put_uint8_t(buf, 9, focus_lock); - _mav_put_uint8_t(buf, 10, shot); - _mav_put_uint8_t(buf, 11, command_id); - _mav_put_uint8_t(buf, 12, extra_param); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); -#else - mavlink_digicam_control_t packet; - packet.extra_value = extra_value; - packet.target_system = target_system; - packet.target_component = target_component; - packet.session = session; - packet.zoom_pos = zoom_pos; - packet.zoom_step = zoom_step; - packet.focus_lock = focus_lock; - packet.shot = shot; - packet.command_id = command_id; - packet.extra_param = extra_param; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); -#endif -} - -/** - * @brief Encode a digicam_control struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param digicam_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_digicam_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_digicam_control_t* digicam_control) -{ - return mavlink_msg_digicam_control_pack(system_id, component_id, msg, digicam_control->target_system, digicam_control->target_component, digicam_control->session, digicam_control->zoom_pos, digicam_control->zoom_step, digicam_control->focus_lock, digicam_control->shot, digicam_control->command_id, digicam_control->extra_param, digicam_control->extra_value); -} - -/** - * @brief Encode a digicam_control struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param digicam_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_digicam_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_digicam_control_t* digicam_control) -{ - return mavlink_msg_digicam_control_pack_chan(system_id, component_id, chan, msg, digicam_control->target_system, digicam_control->target_component, digicam_control->session, digicam_control->zoom_pos, digicam_control->zoom_step, digicam_control->focus_lock, digicam_control->shot, digicam_control->command_id, digicam_control->extra_param, digicam_control->extra_value); -} - -/** - * @brief Send a digicam_control message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param session 0: stop, 1: start or keep it up //Session control e.g. show/hide lens - * @param zoom_pos 1 to N //Zoom's absolute position (0 means ignore) - * @param zoom_step -100 to 100 //Zooming step value to offset zoom from the current position - * @param focus_lock 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus - * @param shot 0: ignore, 1: shot or start filming - * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once - * @param extra_param Extra parameters enumeration (0 means ignore) - * @param extra_value Correspondent value to given extra_param - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_digicam_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t session, uint8_t zoom_pos, int8_t zoom_step, uint8_t focus_lock, uint8_t shot, uint8_t command_id, uint8_t extra_param, float extra_value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN]; - _mav_put_float(buf, 0, extra_value); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - _mav_put_uint8_t(buf, 6, session); - _mav_put_uint8_t(buf, 7, zoom_pos); - _mav_put_int8_t(buf, 8, zoom_step); - _mav_put_uint8_t(buf, 9, focus_lock); - _mav_put_uint8_t(buf, 10, shot); - _mav_put_uint8_t(buf, 11, command_id); - _mav_put_uint8_t(buf, 12, extra_param); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); -#endif -#else - mavlink_digicam_control_t packet; - packet.extra_value = extra_value; - packet.target_system = target_system; - packet.target_component = target_component; - packet.session = session; - packet.zoom_pos = zoom_pos; - packet.zoom_step = zoom_step; - packet.focus_lock = focus_lock; - packet.shot = shot; - packet.command_id = command_id; - packet.extra_param = extra_param; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); -#endif -#endif -} - -#endif - -// MESSAGE DIGICAM_CONTROL UNPACKING - - -/** - * @brief Get field target_system from digicam_control message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_digicam_control_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field target_component from digicam_control message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_digicam_control_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field session from digicam_control message - * - * @return 0: stop, 1: start or keep it up //Session control e.g. show/hide lens - */ -static inline uint8_t mavlink_msg_digicam_control_get_session(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @brief Get field zoom_pos from digicam_control message - * - * @return 1 to N //Zoom's absolute position (0 means ignore) - */ -static inline uint8_t mavlink_msg_digicam_control_get_zoom_pos(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 7); -} - -/** - * @brief Get field zoom_step from digicam_control message - * - * @return -100 to 100 //Zooming step value to offset zoom from the current position - */ -static inline int8_t mavlink_msg_digicam_control_get_zoom_step(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int8_t(msg, 8); -} - -/** - * @brief Get field focus_lock from digicam_control message - * - * @return 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus - */ -static inline uint8_t mavlink_msg_digicam_control_get_focus_lock(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 9); -} - -/** - * @brief Get field shot from digicam_control message - * - * @return 0: ignore, 1: shot or start filming - */ -static inline uint8_t mavlink_msg_digicam_control_get_shot(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 10); -} - -/** - * @brief Get field command_id from digicam_control message - * - * @return Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once - */ -static inline uint8_t mavlink_msg_digicam_control_get_command_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 11); -} - -/** - * @brief Get field extra_param from digicam_control message - * - * @return Extra parameters enumeration (0 means ignore) - */ -static inline uint8_t mavlink_msg_digicam_control_get_extra_param(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 12); -} - -/** - * @brief Get field extra_value from digicam_control message - * - * @return Correspondent value to given extra_param - */ -static inline float mavlink_msg_digicam_control_get_extra_value(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Decode a digicam_control message into a struct - * - * @param msg The message to decode - * @param digicam_control C-struct to decode the message contents into - */ -static inline void mavlink_msg_digicam_control_decode(const mavlink_message_t* msg, mavlink_digicam_control_t* digicam_control) -{ -#if MAVLINK_NEED_BYTE_SWAP - digicam_control->extra_value = mavlink_msg_digicam_control_get_extra_value(msg); - digicam_control->target_system = mavlink_msg_digicam_control_get_target_system(msg); - digicam_control->target_component = mavlink_msg_digicam_control_get_target_component(msg); - digicam_control->session = mavlink_msg_digicam_control_get_session(msg); - digicam_control->zoom_pos = mavlink_msg_digicam_control_get_zoom_pos(msg); - digicam_control->zoom_step = mavlink_msg_digicam_control_get_zoom_step(msg); - digicam_control->focus_lock = mavlink_msg_digicam_control_get_focus_lock(msg); - digicam_control->shot = mavlink_msg_digicam_control_get_shot(msg); - digicam_control->command_id = mavlink_msg_digicam_control_get_command_id(msg); - digicam_control->extra_param = mavlink_msg_digicam_control_get_extra_param(msg); -#else - memcpy(digicam_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h deleted file mode 100644 index fe3677d..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h +++ /dev/null @@ -1,221 +0,0 @@ -// MESSAGE FENCE_FETCH_POINT PACKING - -#define MAVLINK_MSG_ID_FENCE_FETCH_POINT 161 - -typedef struct __mavlink_fence_fetch_point_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t idx; ///< point index (first point is 1, 0 is for return point) -} mavlink_fence_fetch_point_t; - -#define MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN 3 -#define MAVLINK_MSG_ID_161_LEN 3 - -#define MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC 68 -#define MAVLINK_MSG_ID_161_CRC 68 - - - -#define MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT { \ - "FENCE_FETCH_POINT", \ - 3, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_fence_fetch_point_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_fence_fetch_point_t, target_component) }, \ - { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_fence_fetch_point_t, idx) }, \ - } \ -} - - -/** - * @brief Pack a fence_fetch_point message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 1, 0 is for return point) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_fence_fetch_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t idx) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, idx); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); -#else - mavlink_fence_fetch_point_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FENCE_FETCH_POINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); -#endif -} - -/** - * @brief Pack a fence_fetch_point message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 1, 0 is for return point) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_fence_fetch_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t idx) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, idx); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); -#else - mavlink_fence_fetch_point_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FENCE_FETCH_POINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); -#endif -} - -/** - * @brief Encode a fence_fetch_point struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param fence_fetch_point C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_fence_fetch_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fence_fetch_point_t* fence_fetch_point) -{ - return mavlink_msg_fence_fetch_point_pack(system_id, component_id, msg, fence_fetch_point->target_system, fence_fetch_point->target_component, fence_fetch_point->idx); -} - -/** - * @brief Encode a fence_fetch_point struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param fence_fetch_point C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_fence_fetch_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_fetch_point_t* fence_fetch_point) -{ - return mavlink_msg_fence_fetch_point_pack_chan(system_id, component_id, chan, msg, fence_fetch_point->target_system, fence_fetch_point->target_component, fence_fetch_point->idx); -} - -/** - * @brief Send a fence_fetch_point message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 1, 0 is for return point) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_fence_fetch_point_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, idx); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); -#endif -#else - mavlink_fence_fetch_point_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, (const char *)&packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, (const char *)&packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); -#endif -#endif -} - -#endif - -// MESSAGE FENCE_FETCH_POINT UNPACKING - - -/** - * @brief Get field target_system from fence_fetch_point message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_fence_fetch_point_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from fence_fetch_point message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_fence_fetch_point_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field idx from fence_fetch_point message - * - * @return point index (first point is 1, 0 is for return point) - */ -static inline uint8_t mavlink_msg_fence_fetch_point_get_idx(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Decode a fence_fetch_point message into a struct - * - * @param msg The message to decode - * @param fence_fetch_point C-struct to decode the message contents into - */ -static inline void mavlink_msg_fence_fetch_point_decode(const mavlink_message_t* msg, mavlink_fence_fetch_point_t* fence_fetch_point) -{ -#if MAVLINK_NEED_BYTE_SWAP - fence_fetch_point->target_system = mavlink_msg_fence_fetch_point_get_target_system(msg); - fence_fetch_point->target_component = mavlink_msg_fence_fetch_point_get_target_component(msg); - fence_fetch_point->idx = mavlink_msg_fence_fetch_point_get_idx(msg); -#else - memcpy(fence_fetch_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h deleted file mode 100644 index febda6c..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h +++ /dev/null @@ -1,287 +0,0 @@ -// MESSAGE FENCE_POINT PACKING - -#define MAVLINK_MSG_ID_FENCE_POINT 160 - -typedef struct __mavlink_fence_point_t -{ - float lat; ///< Latitude of point - float lng; ///< Longitude of point - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t idx; ///< point index (first point is 1, 0 is for return point) - uint8_t count; ///< total number of points (for sanity checking) -} mavlink_fence_point_t; - -#define MAVLINK_MSG_ID_FENCE_POINT_LEN 12 -#define MAVLINK_MSG_ID_160_LEN 12 - -#define MAVLINK_MSG_ID_FENCE_POINT_CRC 78 -#define MAVLINK_MSG_ID_160_CRC 78 - - - -#define MAVLINK_MESSAGE_INFO_FENCE_POINT { \ - "FENCE_POINT", \ - 6, \ - { { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_fence_point_t, lat) }, \ - { "lng", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_fence_point_t, lng) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_fence_point_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_fence_point_t, target_component) }, \ - { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_fence_point_t, idx) }, \ - { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_fence_point_t, count) }, \ - } \ -} - - -/** - * @brief Pack a fence_point message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 1, 0 is for return point) - * @param count total number of points (for sanity checking) - * @param lat Latitude of point - * @param lng Longitude of point - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_fence_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, float lat, float lng) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FENCE_POINT_LEN]; - _mav_put_float(buf, 0, lat); - _mav_put_float(buf, 4, lng); - _mav_put_uint8_t(buf, 8, target_system); - _mav_put_uint8_t(buf, 9, target_component); - _mav_put_uint8_t(buf, 10, idx); - _mav_put_uint8_t(buf, 11, count); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_POINT_LEN); -#else - mavlink_fence_point_t packet; - packet.lat = lat; - packet.lng = lng; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - packet.count = count; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_POINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FENCE_POINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_POINT_LEN); -#endif -} - -/** - * @brief Pack a fence_point message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 1, 0 is for return point) - * @param count total number of points (for sanity checking) - * @param lat Latitude of point - * @param lng Longitude of point - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_fence_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t idx,uint8_t count,float lat,float lng) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FENCE_POINT_LEN]; - _mav_put_float(buf, 0, lat); - _mav_put_float(buf, 4, lng); - _mav_put_uint8_t(buf, 8, target_system); - _mav_put_uint8_t(buf, 9, target_component); - _mav_put_uint8_t(buf, 10, idx); - _mav_put_uint8_t(buf, 11, count); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_POINT_LEN); -#else - mavlink_fence_point_t packet; - packet.lat = lat; - packet.lng = lng; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - packet.count = count; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_POINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FENCE_POINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_POINT_LEN); -#endif -} - -/** - * @brief Encode a fence_point struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param fence_point C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_fence_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fence_point_t* fence_point) -{ - return mavlink_msg_fence_point_pack(system_id, component_id, msg, fence_point->target_system, fence_point->target_component, fence_point->idx, fence_point->count, fence_point->lat, fence_point->lng); -} - -/** - * @brief Encode a fence_point struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param fence_point C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_fence_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_point_t* fence_point) -{ - return mavlink_msg_fence_point_pack_chan(system_id, component_id, chan, msg, fence_point->target_system, fence_point->target_component, fence_point->idx, fence_point->count, fence_point->lat, fence_point->lng); -} - -/** - * @brief Send a fence_point message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 1, 0 is for return point) - * @param count total number of points (for sanity checking) - * @param lat Latitude of point - * @param lng Longitude of point - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_fence_point_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, float lat, float lng) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FENCE_POINT_LEN]; - _mav_put_float(buf, 0, lat); - _mav_put_float(buf, 4, lng); - _mav_put_uint8_t(buf, 8, target_system); - _mav_put_uint8_t(buf, 9, target_component); - _mav_put_uint8_t(buf, 10, idx); - _mav_put_uint8_t(buf, 11, count); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, buf, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, buf, MAVLINK_MSG_ID_FENCE_POINT_LEN); -#endif -#else - mavlink_fence_point_t packet; - packet.lat = lat; - packet.lng = lng; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - packet.count = count; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, (const char *)&packet, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, (const char *)&packet, MAVLINK_MSG_ID_FENCE_POINT_LEN); -#endif -#endif -} - -#endif - -// MESSAGE FENCE_POINT UNPACKING - - -/** - * @brief Get field target_system from fence_point message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_fence_point_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 8); -} - -/** - * @brief Get field target_component from fence_point message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_fence_point_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 9); -} - -/** - * @brief Get field idx from fence_point message - * - * @return point index (first point is 1, 0 is for return point) - */ -static inline uint8_t mavlink_msg_fence_point_get_idx(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 10); -} - -/** - * @brief Get field count from fence_point message - * - * @return total number of points (for sanity checking) - */ -static inline uint8_t mavlink_msg_fence_point_get_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 11); -} - -/** - * @brief Get field lat from fence_point message - * - * @return Latitude of point - */ -static inline float mavlink_msg_fence_point_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field lng from fence_point message - * - * @return Longitude of point - */ -static inline float mavlink_msg_fence_point_get_lng(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Decode a fence_point message into a struct - * - * @param msg The message to decode - * @param fence_point C-struct to decode the message contents into - */ -static inline void mavlink_msg_fence_point_decode(const mavlink_message_t* msg, mavlink_fence_point_t* fence_point) -{ -#if MAVLINK_NEED_BYTE_SWAP - fence_point->lat = mavlink_msg_fence_point_get_lat(msg); - fence_point->lng = mavlink_msg_fence_point_get_lng(msg); - fence_point->target_system = mavlink_msg_fence_point_get_target_system(msg); - fence_point->target_component = mavlink_msg_fence_point_get_target_component(msg); - fence_point->idx = mavlink_msg_fence_point_get_idx(msg); - fence_point->count = mavlink_msg_fence_point_get_count(msg); -#else - memcpy(fence_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FENCE_POINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h deleted file mode 100644 index 6120904..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h +++ /dev/null @@ -1,243 +0,0 @@ -// MESSAGE FENCE_STATUS PACKING - -#define MAVLINK_MSG_ID_FENCE_STATUS 162 - -typedef struct __mavlink_fence_status_t -{ - uint32_t breach_time; ///< time of last breach in milliseconds since boot - uint16_t breach_count; ///< number of fence breaches - uint8_t breach_status; ///< 0 if currently inside fence, 1 if outside - uint8_t breach_type; ///< last breach type (see FENCE_BREACH_* enum) -} mavlink_fence_status_t; - -#define MAVLINK_MSG_ID_FENCE_STATUS_LEN 8 -#define MAVLINK_MSG_ID_162_LEN 8 - -#define MAVLINK_MSG_ID_FENCE_STATUS_CRC 189 -#define MAVLINK_MSG_ID_162_CRC 189 - - - -#define MAVLINK_MESSAGE_INFO_FENCE_STATUS { \ - "FENCE_STATUS", \ - 4, \ - { { "breach_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_fence_status_t, breach_time) }, \ - { "breach_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_fence_status_t, breach_count) }, \ - { "breach_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_fence_status_t, breach_status) }, \ - { "breach_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_fence_status_t, breach_type) }, \ - } \ -} - - -/** - * @brief Pack a fence_status message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param breach_status 0 if currently inside fence, 1 if outside - * @param breach_count number of fence breaches - * @param breach_type last breach type (see FENCE_BREACH_* enum) - * @param breach_time time of last breach in milliseconds since boot - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_fence_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FENCE_STATUS_LEN]; - _mav_put_uint32_t(buf, 0, breach_time); - _mav_put_uint16_t(buf, 4, breach_count); - _mav_put_uint8_t(buf, 6, breach_status); - _mav_put_uint8_t(buf, 7, breach_type); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN); -#else - mavlink_fence_status_t packet; - packet.breach_time = breach_time; - packet.breach_count = breach_count; - packet.breach_status = breach_status; - packet.breach_type = breach_type; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FENCE_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_STATUS_LEN); -#endif -} - -/** - * @brief Pack a fence_status message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param breach_status 0 if currently inside fence, 1 if outside - * @param breach_count number of fence breaches - * @param breach_type last breach type (see FENCE_BREACH_* enum) - * @param breach_time time of last breach in milliseconds since boot - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_fence_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t breach_status,uint16_t breach_count,uint8_t breach_type,uint32_t breach_time) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FENCE_STATUS_LEN]; - _mav_put_uint32_t(buf, 0, breach_time); - _mav_put_uint16_t(buf, 4, breach_count); - _mav_put_uint8_t(buf, 6, breach_status); - _mav_put_uint8_t(buf, 7, breach_type); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN); -#else - mavlink_fence_status_t packet; - packet.breach_time = breach_time; - packet.breach_count = breach_count; - packet.breach_status = breach_status; - packet.breach_type = breach_type; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FENCE_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_STATUS_LEN); -#endif -} - -/** - * @brief Encode a fence_status struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param fence_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_fence_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fence_status_t* fence_status) -{ - return mavlink_msg_fence_status_pack(system_id, component_id, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time); -} - -/** - * @brief Encode a fence_status struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param fence_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_fence_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_status_t* fence_status) -{ - return mavlink_msg_fence_status_pack_chan(system_id, component_id, chan, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time); -} - -/** - * @brief Send a fence_status message - * @param chan MAVLink channel to send the message - * - * @param breach_status 0 if currently inside fence, 1 if outside - * @param breach_count number of fence breaches - * @param breach_type last breach type (see FENCE_BREACH_* enum) - * @param breach_time time of last breach in milliseconds since boot - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_fence_status_send(mavlink_channel_t chan, uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FENCE_STATUS_LEN]; - _mav_put_uint32_t(buf, 0, breach_time); - _mav_put_uint16_t(buf, 4, breach_count); - _mav_put_uint8_t(buf, 6, breach_status); - _mav_put_uint8_t(buf, 7, breach_type); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN); -#endif -#else - mavlink_fence_status_t packet; - packet.breach_time = breach_time; - packet.breach_count = breach_count; - packet.breach_status = breach_status; - packet.breach_type = breach_type; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN); -#endif -#endif -} - -#endif - -// MESSAGE FENCE_STATUS UNPACKING - - -/** - * @brief Get field breach_status from fence_status message - * - * @return 0 if currently inside fence, 1 if outside - */ -static inline uint8_t mavlink_msg_fence_status_get_breach_status(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @brief Get field breach_count from fence_status message - * - * @return number of fence breaches - */ -static inline uint16_t mavlink_msg_fence_status_get_breach_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field breach_type from fence_status message - * - * @return last breach type (see FENCE_BREACH_* enum) - */ -static inline uint8_t mavlink_msg_fence_status_get_breach_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 7); -} - -/** - * @brief Get field breach_time from fence_status message - * - * @return time of last breach in milliseconds since boot - */ -static inline uint32_t mavlink_msg_fence_status_get_breach_time(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Decode a fence_status message into a struct - * - * @param msg The message to decode - * @param fence_status C-struct to decode the message contents into - */ -static inline void mavlink_msg_fence_status_decode(const mavlink_message_t* msg, mavlink_fence_status_t* fence_status) -{ -#if MAVLINK_NEED_BYTE_SWAP - fence_status->breach_time = mavlink_msg_fence_status_get_breach_time(msg); - fence_status->breach_count = mavlink_msg_fence_status_get_breach_count(msg); - fence_status->breach_status = mavlink_msg_fence_status_get_breach_status(msg); - fence_status->breach_type = mavlink_msg_fence_status_get_breach_type(msg); -#else - memcpy(fence_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FENCE_STATUS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h deleted file mode 100644 index 2f5dea5..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h +++ /dev/null @@ -1,199 +0,0 @@ -// MESSAGE HWSTATUS PACKING - -#define MAVLINK_MSG_ID_HWSTATUS 165 - -typedef struct __mavlink_hwstatus_t -{ - uint16_t Vcc; ///< board voltage (mV) - uint8_t I2Cerr; ///< I2C error count -} mavlink_hwstatus_t; - -#define MAVLINK_MSG_ID_HWSTATUS_LEN 3 -#define MAVLINK_MSG_ID_165_LEN 3 - -#define MAVLINK_MSG_ID_HWSTATUS_CRC 21 -#define MAVLINK_MSG_ID_165_CRC 21 - - - -#define MAVLINK_MESSAGE_INFO_HWSTATUS { \ - "HWSTATUS", \ - 2, \ - { { "Vcc", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_hwstatus_t, Vcc) }, \ - { "I2Cerr", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_hwstatus_t, I2Cerr) }, \ - } \ -} - - -/** - * @brief Pack a hwstatus message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param Vcc board voltage (mV) - * @param I2Cerr I2C error count - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_hwstatus_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t Vcc, uint8_t I2Cerr) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HWSTATUS_LEN]; - _mav_put_uint16_t(buf, 0, Vcc); - _mav_put_uint8_t(buf, 2, I2Cerr); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HWSTATUS_LEN); -#else - mavlink_hwstatus_t packet; - packet.Vcc = Vcc; - packet.I2Cerr = I2Cerr; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HWSTATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HWSTATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HWSTATUS_LEN); -#endif -} - -/** - * @brief Pack a hwstatus message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param Vcc board voltage (mV) - * @param I2Cerr I2C error count - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_hwstatus_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t Vcc,uint8_t I2Cerr) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HWSTATUS_LEN]; - _mav_put_uint16_t(buf, 0, Vcc); - _mav_put_uint8_t(buf, 2, I2Cerr); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HWSTATUS_LEN); -#else - mavlink_hwstatus_t packet; - packet.Vcc = Vcc; - packet.I2Cerr = I2Cerr; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HWSTATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HWSTATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HWSTATUS_LEN); -#endif -} - -/** - * @brief Encode a hwstatus struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param hwstatus C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_hwstatus_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hwstatus_t* hwstatus) -{ - return mavlink_msg_hwstatus_pack(system_id, component_id, msg, hwstatus->Vcc, hwstatus->I2Cerr); -} - -/** - * @brief Encode a hwstatus struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param hwstatus C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_hwstatus_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hwstatus_t* hwstatus) -{ - return mavlink_msg_hwstatus_pack_chan(system_id, component_id, chan, msg, hwstatus->Vcc, hwstatus->I2Cerr); -} - -/** - * @brief Send a hwstatus message - * @param chan MAVLink channel to send the message - * - * @param Vcc board voltage (mV) - * @param I2Cerr I2C error count - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_hwstatus_send(mavlink_channel_t chan, uint16_t Vcc, uint8_t I2Cerr) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HWSTATUS_LEN]; - _mav_put_uint16_t(buf, 0, Vcc); - _mav_put_uint8_t(buf, 2, I2Cerr); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_LEN); -#endif -#else - mavlink_hwstatus_t packet; - packet.Vcc = Vcc; - packet.I2Cerr = I2Cerr; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)&packet, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)&packet, MAVLINK_MSG_ID_HWSTATUS_LEN); -#endif -#endif -} - -#endif - -// MESSAGE HWSTATUS UNPACKING - - -/** - * @brief Get field Vcc from hwstatus message - * - * @return board voltage (mV) - */ -static inline uint16_t mavlink_msg_hwstatus_get_Vcc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field I2Cerr from hwstatus message - * - * @return I2C error count - */ -static inline uint8_t mavlink_msg_hwstatus_get_I2Cerr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Decode a hwstatus message into a struct - * - * @param msg The message to decode - * @param hwstatus C-struct to decode the message contents into - */ -static inline void mavlink_msg_hwstatus_decode(const mavlink_message_t* msg, mavlink_hwstatus_t* hwstatus) -{ -#if MAVLINK_NEED_BYTE_SWAP - hwstatus->Vcc = mavlink_msg_hwstatus_get_Vcc(msg); - hwstatus->I2Cerr = mavlink_msg_hwstatus_get_I2Cerr(msg); -#else - memcpy(hwstatus, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HWSTATUS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h deleted file mode 100644 index 34743fd..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE LIMITS_STATUS PACKING - -#define MAVLINK_MSG_ID_LIMITS_STATUS 167 - -typedef struct __mavlink_limits_status_t -{ - uint32_t last_trigger; ///< time of last breach in milliseconds since boot - uint32_t last_action; ///< time of last recovery action in milliseconds since boot - uint32_t last_recovery; ///< time of last successful recovery in milliseconds since boot - uint32_t last_clear; ///< time of last all-clear in milliseconds since boot - uint16_t breach_count; ///< number of fence breaches - uint8_t limits_state; ///< state of AP_Limits, (see enum LimitState, LIMITS_STATE) - uint8_t mods_enabled; ///< AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) - uint8_t mods_required; ///< AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) - uint8_t mods_triggered; ///< AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) -} mavlink_limits_status_t; - -#define MAVLINK_MSG_ID_LIMITS_STATUS_LEN 22 -#define MAVLINK_MSG_ID_167_LEN 22 - -#define MAVLINK_MSG_ID_LIMITS_STATUS_CRC 144 -#define MAVLINK_MSG_ID_167_CRC 144 - - - -#define MAVLINK_MESSAGE_INFO_LIMITS_STATUS { \ - "LIMITS_STATUS", \ - 9, \ - { { "last_trigger", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_limits_status_t, last_trigger) }, \ - { "last_action", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_limits_status_t, last_action) }, \ - { "last_recovery", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_limits_status_t, last_recovery) }, \ - { "last_clear", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_limits_status_t, last_clear) }, \ - { "breach_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_limits_status_t, breach_count) }, \ - { "limits_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_limits_status_t, limits_state) }, \ - { "mods_enabled", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_limits_status_t, mods_enabled) }, \ - { "mods_required", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_limits_status_t, mods_required) }, \ - { "mods_triggered", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_limits_status_t, mods_triggered) }, \ - } \ -} - - -/** - * @brief Pack a limits_status message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE) - * @param last_trigger time of last breach in milliseconds since boot - * @param last_action time of last recovery action in milliseconds since boot - * @param last_recovery time of last successful recovery in milliseconds since boot - * @param last_clear time of last all-clear in milliseconds since boot - * @param breach_count number of fence breaches - * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) - * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) - * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_limits_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t limits_state, uint32_t last_trigger, uint32_t last_action, uint32_t last_recovery, uint32_t last_clear, uint16_t breach_count, uint8_t mods_enabled, uint8_t mods_required, uint8_t mods_triggered) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LIMITS_STATUS_LEN]; - _mav_put_uint32_t(buf, 0, last_trigger); - _mav_put_uint32_t(buf, 4, last_action); - _mav_put_uint32_t(buf, 8, last_recovery); - _mav_put_uint32_t(buf, 12, last_clear); - _mav_put_uint16_t(buf, 16, breach_count); - _mav_put_uint8_t(buf, 18, limits_state); - _mav_put_uint8_t(buf, 19, mods_enabled); - _mav_put_uint8_t(buf, 20, mods_required); - _mav_put_uint8_t(buf, 21, mods_triggered); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); -#else - mavlink_limits_status_t packet; - packet.last_trigger = last_trigger; - packet.last_action = last_action; - packet.last_recovery = last_recovery; - packet.last_clear = last_clear; - packet.breach_count = breach_count; - packet.limits_state = limits_state; - packet.mods_enabled = mods_enabled; - packet.mods_required = mods_required; - packet.mods_triggered = mods_triggered; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LIMITS_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); -#endif -} - -/** - * @brief Pack a limits_status message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE) - * @param last_trigger time of last breach in milliseconds since boot - * @param last_action time of last recovery action in milliseconds since boot - * @param last_recovery time of last successful recovery in milliseconds since boot - * @param last_clear time of last all-clear in milliseconds since boot - * @param breach_count number of fence breaches - * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) - * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) - * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_limits_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t limits_state,uint32_t last_trigger,uint32_t last_action,uint32_t last_recovery,uint32_t last_clear,uint16_t breach_count,uint8_t mods_enabled,uint8_t mods_required,uint8_t mods_triggered) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LIMITS_STATUS_LEN]; - _mav_put_uint32_t(buf, 0, last_trigger); - _mav_put_uint32_t(buf, 4, last_action); - _mav_put_uint32_t(buf, 8, last_recovery); - _mav_put_uint32_t(buf, 12, last_clear); - _mav_put_uint16_t(buf, 16, breach_count); - _mav_put_uint8_t(buf, 18, limits_state); - _mav_put_uint8_t(buf, 19, mods_enabled); - _mav_put_uint8_t(buf, 20, mods_required); - _mav_put_uint8_t(buf, 21, mods_triggered); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); -#else - mavlink_limits_status_t packet; - packet.last_trigger = last_trigger; - packet.last_action = last_action; - packet.last_recovery = last_recovery; - packet.last_clear = last_clear; - packet.breach_count = breach_count; - packet.limits_state = limits_state; - packet.mods_enabled = mods_enabled; - packet.mods_required = mods_required; - packet.mods_triggered = mods_triggered; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LIMITS_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); -#endif -} - -/** - * @brief Encode a limits_status struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param limits_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_limits_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_limits_status_t* limits_status) -{ - return mavlink_msg_limits_status_pack(system_id, component_id, msg, limits_status->limits_state, limits_status->last_trigger, limits_status->last_action, limits_status->last_recovery, limits_status->last_clear, limits_status->breach_count, limits_status->mods_enabled, limits_status->mods_required, limits_status->mods_triggered); -} - -/** - * @brief Encode a limits_status struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param limits_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_limits_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_limits_status_t* limits_status) -{ - return mavlink_msg_limits_status_pack_chan(system_id, component_id, chan, msg, limits_status->limits_state, limits_status->last_trigger, limits_status->last_action, limits_status->last_recovery, limits_status->last_clear, limits_status->breach_count, limits_status->mods_enabled, limits_status->mods_required, limits_status->mods_triggered); -} - -/** - * @brief Send a limits_status message - * @param chan MAVLink channel to send the message - * - * @param limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE) - * @param last_trigger time of last breach in milliseconds since boot - * @param last_action time of last recovery action in milliseconds since boot - * @param last_recovery time of last successful recovery in milliseconds since boot - * @param last_clear time of last all-clear in milliseconds since boot - * @param breach_count number of fence breaches - * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) - * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) - * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_limits_status_send(mavlink_channel_t chan, uint8_t limits_state, uint32_t last_trigger, uint32_t last_action, uint32_t last_recovery, uint32_t last_clear, uint16_t breach_count, uint8_t mods_enabled, uint8_t mods_required, uint8_t mods_triggered) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LIMITS_STATUS_LEN]; - _mav_put_uint32_t(buf, 0, last_trigger); - _mav_put_uint32_t(buf, 4, last_action); - _mav_put_uint32_t(buf, 8, last_recovery); - _mav_put_uint32_t(buf, 12, last_clear); - _mav_put_uint16_t(buf, 16, breach_count); - _mav_put_uint8_t(buf, 18, limits_state); - _mav_put_uint8_t(buf, 19, mods_enabled); - _mav_put_uint8_t(buf, 20, mods_required); - _mav_put_uint8_t(buf, 21, mods_triggered); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); -#endif -#else - mavlink_limits_status_t packet; - packet.last_trigger = last_trigger; - packet.last_action = last_action; - packet.last_recovery = last_recovery; - packet.last_clear = last_clear; - packet.breach_count = breach_count; - packet.limits_state = limits_state; - packet.mods_enabled = mods_enabled; - packet.mods_required = mods_required; - packet.mods_triggered = mods_triggered; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); -#endif -#endif -} - -#endif - -// MESSAGE LIMITS_STATUS UNPACKING - - -/** - * @brief Get field limits_state from limits_status message - * - * @return state of AP_Limits, (see enum LimitState, LIMITS_STATE) - */ -static inline uint8_t mavlink_msg_limits_status_get_limits_state(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 18); -} - -/** - * @brief Get field last_trigger from limits_status message - * - * @return time of last breach in milliseconds since boot - */ -static inline uint32_t mavlink_msg_limits_status_get_last_trigger(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field last_action from limits_status message - * - * @return time of last recovery action in milliseconds since boot - */ -static inline uint32_t mavlink_msg_limits_status_get_last_action(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 4); -} - -/** - * @brief Get field last_recovery from limits_status message - * - * @return time of last successful recovery in milliseconds since boot - */ -static inline uint32_t mavlink_msg_limits_status_get_last_recovery(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 8); -} - -/** - * @brief Get field last_clear from limits_status message - * - * @return time of last all-clear in milliseconds since boot - */ -static inline uint32_t mavlink_msg_limits_status_get_last_clear(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 12); -} - -/** - * @brief Get field breach_count from limits_status message - * - * @return number of fence breaches - */ -static inline uint16_t mavlink_msg_limits_status_get_breach_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 16); -} - -/** - * @brief Get field mods_enabled from limits_status message - * - * @return AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) - */ -static inline uint8_t mavlink_msg_limits_status_get_mods_enabled(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 19); -} - -/** - * @brief Get field mods_required from limits_status message - * - * @return AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) - */ -static inline uint8_t mavlink_msg_limits_status_get_mods_required(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 20); -} - -/** - * @brief Get field mods_triggered from limits_status message - * - * @return AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) - */ -static inline uint8_t mavlink_msg_limits_status_get_mods_triggered(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 21); -} - -/** - * @brief Decode a limits_status message into a struct - * - * @param msg The message to decode - * @param limits_status C-struct to decode the message contents into - */ -static inline void mavlink_msg_limits_status_decode(const mavlink_message_t* msg, mavlink_limits_status_t* limits_status) -{ -#if MAVLINK_NEED_BYTE_SWAP - limits_status->last_trigger = mavlink_msg_limits_status_get_last_trigger(msg); - limits_status->last_action = mavlink_msg_limits_status_get_last_action(msg); - limits_status->last_recovery = mavlink_msg_limits_status_get_last_recovery(msg); - limits_status->last_clear = mavlink_msg_limits_status_get_last_clear(msg); - limits_status->breach_count = mavlink_msg_limits_status_get_breach_count(msg); - limits_status->limits_state = mavlink_msg_limits_status_get_limits_state(msg); - limits_status->mods_enabled = mavlink_msg_limits_status_get_mods_enabled(msg); - limits_status->mods_required = mavlink_msg_limits_status_get_mods_required(msg); - limits_status->mods_triggered = mavlink_msg_limits_status_get_mods_triggered(msg); -#else - memcpy(limits_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LIMITS_STATUS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h deleted file mode 100644 index 55f772b..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h +++ /dev/null @@ -1,199 +0,0 @@ -// MESSAGE MEMINFO PACKING - -#define MAVLINK_MSG_ID_MEMINFO 152 - -typedef struct __mavlink_meminfo_t -{ - uint16_t brkval; ///< heap top - uint16_t freemem; ///< free memory -} mavlink_meminfo_t; - -#define MAVLINK_MSG_ID_MEMINFO_LEN 4 -#define MAVLINK_MSG_ID_152_LEN 4 - -#define MAVLINK_MSG_ID_MEMINFO_CRC 208 -#define MAVLINK_MSG_ID_152_CRC 208 - - - -#define MAVLINK_MESSAGE_INFO_MEMINFO { \ - "MEMINFO", \ - 2, \ - { { "brkval", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_meminfo_t, brkval) }, \ - { "freemem", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_meminfo_t, freemem) }, \ - } \ -} - - -/** - * @brief Pack a meminfo message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param brkval heap top - * @param freemem free memory - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_meminfo_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t brkval, uint16_t freemem) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MEMINFO_LEN]; - _mav_put_uint16_t(buf, 0, brkval); - _mav_put_uint16_t(buf, 2, freemem); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MEMINFO_LEN); -#else - mavlink_meminfo_t packet; - packet.brkval = brkval; - packet.freemem = freemem; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMINFO_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MEMINFO; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMINFO_LEN); -#endif -} - -/** - * @brief Pack a meminfo message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param brkval heap top - * @param freemem free memory - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_meminfo_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t brkval,uint16_t freemem) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MEMINFO_LEN]; - _mav_put_uint16_t(buf, 0, brkval); - _mav_put_uint16_t(buf, 2, freemem); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MEMINFO_LEN); -#else - mavlink_meminfo_t packet; - packet.brkval = brkval; - packet.freemem = freemem; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMINFO_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MEMINFO; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MEMINFO_LEN); -#endif -} - -/** - * @brief Encode a meminfo struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param meminfo C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_meminfo_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_meminfo_t* meminfo) -{ - return mavlink_msg_meminfo_pack(system_id, component_id, msg, meminfo->brkval, meminfo->freemem); -} - -/** - * @brief Encode a meminfo struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param meminfo C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_meminfo_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_meminfo_t* meminfo) -{ - return mavlink_msg_meminfo_pack_chan(system_id, component_id, chan, msg, meminfo->brkval, meminfo->freemem); -} - -/** - * @brief Send a meminfo message - * @param chan MAVLink channel to send the message - * - * @param brkval heap top - * @param freemem free memory - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_meminfo_send(mavlink_channel_t chan, uint16_t brkval, uint16_t freemem) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MEMINFO_LEN]; - _mav_put_uint16_t(buf, 0, brkval); - _mav_put_uint16_t(buf, 2, freemem); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, buf, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, buf, MAVLINK_MSG_ID_MEMINFO_LEN); -#endif -#else - mavlink_meminfo_t packet; - packet.brkval = brkval; - packet.freemem = freemem; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, (const char *)&packet, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, (const char *)&packet, MAVLINK_MSG_ID_MEMINFO_LEN); -#endif -#endif -} - -#endif - -// MESSAGE MEMINFO UNPACKING - - -/** - * @brief Get field brkval from meminfo message - * - * @return heap top - */ -static inline uint16_t mavlink_msg_meminfo_get_brkval(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field freemem from meminfo message - * - * @return free memory - */ -static inline uint16_t mavlink_msg_meminfo_get_freemem(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Decode a meminfo message into a struct - * - * @param msg The message to decode - * @param meminfo C-struct to decode the message contents into - */ -static inline void mavlink_msg_meminfo_decode(const mavlink_message_t* msg, mavlink_meminfo_t* meminfo) -{ -#if MAVLINK_NEED_BYTE_SWAP - meminfo->brkval = mavlink_msg_meminfo_get_brkval(msg); - meminfo->freemem = mavlink_msg_meminfo_get_freemem(msg); -#else - memcpy(meminfo, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MEMINFO_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h deleted file mode 100644 index de717df..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h +++ /dev/null @@ -1,287 +0,0 @@ -// MESSAGE MOUNT_CONFIGURE PACKING - -#define MAVLINK_MSG_ID_MOUNT_CONFIGURE 156 - -typedef struct __mavlink_mount_configure_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t mount_mode; ///< mount operating mode (see MAV_MOUNT_MODE enum) - uint8_t stab_roll; ///< (1 = yes, 0 = no) - uint8_t stab_pitch; ///< (1 = yes, 0 = no) - uint8_t stab_yaw; ///< (1 = yes, 0 = no) -} mavlink_mount_configure_t; - -#define MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN 6 -#define MAVLINK_MSG_ID_156_LEN 6 - -#define MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC 19 -#define MAVLINK_MSG_ID_156_CRC 19 - - - -#define MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE { \ - "MOUNT_CONFIGURE", \ - 6, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mount_configure_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mount_configure_t, target_component) }, \ - { "mount_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mount_configure_t, mount_mode) }, \ - { "stab_roll", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mount_configure_t, stab_roll) }, \ - { "stab_pitch", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mount_configure_t, stab_pitch) }, \ - { "stab_yaw", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mount_configure_t, stab_yaw) }, \ - } \ -} - - -/** - * @brief Pack a mount_configure message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param mount_mode mount operating mode (see MAV_MOUNT_MODE enum) - * @param stab_roll (1 = yes, 0 = no) - * @param stab_pitch (1 = yes, 0 = no) - * @param stab_yaw (1 = yes, 0 = no) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mount_configure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t mount_mode, uint8_t stab_roll, uint8_t stab_pitch, uint8_t stab_yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, mount_mode); - _mav_put_uint8_t(buf, 3, stab_roll); - _mav_put_uint8_t(buf, 4, stab_pitch); - _mav_put_uint8_t(buf, 5, stab_yaw); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); -#else - mavlink_mount_configure_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.mount_mode = mount_mode; - packet.stab_roll = stab_roll; - packet.stab_pitch = stab_pitch; - packet.stab_yaw = stab_yaw; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MOUNT_CONFIGURE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); -#endif -} - -/** - * @brief Pack a mount_configure message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param mount_mode mount operating mode (see MAV_MOUNT_MODE enum) - * @param stab_roll (1 = yes, 0 = no) - * @param stab_pitch (1 = yes, 0 = no) - * @param stab_yaw (1 = yes, 0 = no) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mount_configure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t mount_mode,uint8_t stab_roll,uint8_t stab_pitch,uint8_t stab_yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, mount_mode); - _mav_put_uint8_t(buf, 3, stab_roll); - _mav_put_uint8_t(buf, 4, stab_pitch); - _mav_put_uint8_t(buf, 5, stab_yaw); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); -#else - mavlink_mount_configure_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.mount_mode = mount_mode; - packet.stab_roll = stab_roll; - packet.stab_pitch = stab_pitch; - packet.stab_yaw = stab_yaw; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MOUNT_CONFIGURE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); -#endif -} - -/** - * @brief Encode a mount_configure struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param mount_configure C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mount_configure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mount_configure_t* mount_configure) -{ - return mavlink_msg_mount_configure_pack(system_id, component_id, msg, mount_configure->target_system, mount_configure->target_component, mount_configure->mount_mode, mount_configure->stab_roll, mount_configure->stab_pitch, mount_configure->stab_yaw); -} - -/** - * @brief Encode a mount_configure struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param mount_configure C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mount_configure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_configure_t* mount_configure) -{ - return mavlink_msg_mount_configure_pack_chan(system_id, component_id, chan, msg, mount_configure->target_system, mount_configure->target_component, mount_configure->mount_mode, mount_configure->stab_roll, mount_configure->stab_pitch, mount_configure->stab_yaw); -} - -/** - * @brief Send a mount_configure message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param mount_mode mount operating mode (see MAV_MOUNT_MODE enum) - * @param stab_roll (1 = yes, 0 = no) - * @param stab_pitch (1 = yes, 0 = no) - * @param stab_yaw (1 = yes, 0 = no) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_mount_configure_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mount_mode, uint8_t stab_roll, uint8_t stab_pitch, uint8_t stab_yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, mount_mode); - _mav_put_uint8_t(buf, 3, stab_roll); - _mav_put_uint8_t(buf, 4, stab_pitch); - _mav_put_uint8_t(buf, 5, stab_yaw); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); -#endif -#else - mavlink_mount_configure_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.mount_mode = mount_mode; - packet.stab_roll = stab_roll; - packet.stab_pitch = stab_pitch; - packet.stab_yaw = stab_yaw; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); -#endif -#endif -} - -#endif - -// MESSAGE MOUNT_CONFIGURE UNPACKING - - -/** - * @brief Get field target_system from mount_configure message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_mount_configure_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from mount_configure message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_mount_configure_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field mount_mode from mount_configure message - * - * @return mount operating mode (see MAV_MOUNT_MODE enum) - */ -static inline uint8_t mavlink_msg_mount_configure_get_mount_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field stab_roll from mount_configure message - * - * @return (1 = yes, 0 = no) - */ -static inline uint8_t mavlink_msg_mount_configure_get_stab_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field stab_pitch from mount_configure message - * - * @return (1 = yes, 0 = no) - */ -static inline uint8_t mavlink_msg_mount_configure_get_stab_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field stab_yaw from mount_configure message - * - * @return (1 = yes, 0 = no) - */ -static inline uint8_t mavlink_msg_mount_configure_get_stab_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Decode a mount_configure message into a struct - * - * @param msg The message to decode - * @param mount_configure C-struct to decode the message contents into - */ -static inline void mavlink_msg_mount_configure_decode(const mavlink_message_t* msg, mavlink_mount_configure_t* mount_configure) -{ -#if MAVLINK_NEED_BYTE_SWAP - mount_configure->target_system = mavlink_msg_mount_configure_get_target_system(msg); - mount_configure->target_component = mavlink_msg_mount_configure_get_target_component(msg); - mount_configure->mount_mode = mavlink_msg_mount_configure_get_mount_mode(msg); - mount_configure->stab_roll = mavlink_msg_mount_configure_get_stab_roll(msg); - mount_configure->stab_pitch = mavlink_msg_mount_configure_get_stab_pitch(msg); - mount_configure->stab_yaw = mavlink_msg_mount_configure_get_stab_yaw(msg); -#else - memcpy(mount_configure, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h deleted file mode 100644 index 4441635..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h +++ /dev/null @@ -1,287 +0,0 @@ -// MESSAGE MOUNT_CONTROL PACKING - -#define MAVLINK_MSG_ID_MOUNT_CONTROL 157 - -typedef struct __mavlink_mount_control_t -{ - int32_t input_a; ///< pitch(deg*100) or lat, depending on mount mode - int32_t input_b; ///< roll(deg*100) or lon depending on mount mode - int32_t input_c; ///< yaw(deg*100) or alt (in cm) depending on mount mode - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t save_position; ///< if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) -} mavlink_mount_control_t; - -#define MAVLINK_MSG_ID_MOUNT_CONTROL_LEN 15 -#define MAVLINK_MSG_ID_157_LEN 15 - -#define MAVLINK_MSG_ID_MOUNT_CONTROL_CRC 21 -#define MAVLINK_MSG_ID_157_CRC 21 - - - -#define MAVLINK_MESSAGE_INFO_MOUNT_CONTROL { \ - "MOUNT_CONTROL", \ - 6, \ - { { "input_a", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_mount_control_t, input_a) }, \ - { "input_b", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_mount_control_t, input_b) }, \ - { "input_c", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_mount_control_t, input_c) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_mount_control_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_mount_control_t, target_component) }, \ - { "save_position", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_mount_control_t, save_position) }, \ - } \ -} - - -/** - * @brief Pack a mount_control message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param input_a pitch(deg*100) or lat, depending on mount mode - * @param input_b roll(deg*100) or lon depending on mount mode - * @param input_c yaw(deg*100) or alt (in cm) depending on mount mode - * @param save_position if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mount_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, int32_t input_a, int32_t input_b, int32_t input_c, uint8_t save_position) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MOUNT_CONTROL_LEN]; - _mav_put_int32_t(buf, 0, input_a); - _mav_put_int32_t(buf, 4, input_b); - _mav_put_int32_t(buf, 8, input_c); - _mav_put_uint8_t(buf, 12, target_system); - _mav_put_uint8_t(buf, 13, target_component); - _mav_put_uint8_t(buf, 14, save_position); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); -#else - mavlink_mount_control_t packet; - packet.input_a = input_a; - packet.input_b = input_b; - packet.input_c = input_c; - packet.target_system = target_system; - packet.target_component = target_component; - packet.save_position = save_position; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MOUNT_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); -#endif -} - -/** - * @brief Pack a mount_control message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param input_a pitch(deg*100) or lat, depending on mount mode - * @param input_b roll(deg*100) or lon depending on mount mode - * @param input_c yaw(deg*100) or alt (in cm) depending on mount mode - * @param save_position if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mount_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,int32_t input_a,int32_t input_b,int32_t input_c,uint8_t save_position) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MOUNT_CONTROL_LEN]; - _mav_put_int32_t(buf, 0, input_a); - _mav_put_int32_t(buf, 4, input_b); - _mav_put_int32_t(buf, 8, input_c); - _mav_put_uint8_t(buf, 12, target_system); - _mav_put_uint8_t(buf, 13, target_component); - _mav_put_uint8_t(buf, 14, save_position); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); -#else - mavlink_mount_control_t packet; - packet.input_a = input_a; - packet.input_b = input_b; - packet.input_c = input_c; - packet.target_system = target_system; - packet.target_component = target_component; - packet.save_position = save_position; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MOUNT_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); -#endif -} - -/** - * @brief Encode a mount_control struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param mount_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mount_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mount_control_t* mount_control) -{ - return mavlink_msg_mount_control_pack(system_id, component_id, msg, mount_control->target_system, mount_control->target_component, mount_control->input_a, mount_control->input_b, mount_control->input_c, mount_control->save_position); -} - -/** - * @brief Encode a mount_control struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param mount_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mount_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_control_t* mount_control) -{ - return mavlink_msg_mount_control_pack_chan(system_id, component_id, chan, msg, mount_control->target_system, mount_control->target_component, mount_control->input_a, mount_control->input_b, mount_control->input_c, mount_control->save_position); -} - -/** - * @brief Send a mount_control message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param input_a pitch(deg*100) or lat, depending on mount mode - * @param input_b roll(deg*100) or lon depending on mount mode - * @param input_c yaw(deg*100) or alt (in cm) depending on mount mode - * @param save_position if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_mount_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t input_a, int32_t input_b, int32_t input_c, uint8_t save_position) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MOUNT_CONTROL_LEN]; - _mav_put_int32_t(buf, 0, input_a); - _mav_put_int32_t(buf, 4, input_b); - _mav_put_int32_t(buf, 8, input_c); - _mav_put_uint8_t(buf, 12, target_system); - _mav_put_uint8_t(buf, 13, target_component); - _mav_put_uint8_t(buf, 14, save_position); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); -#endif -#else - mavlink_mount_control_t packet; - packet.input_a = input_a; - packet.input_b = input_b; - packet.input_c = input_c; - packet.target_system = target_system; - packet.target_component = target_component; - packet.save_position = save_position; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); -#endif -#endif -} - -#endif - -// MESSAGE MOUNT_CONTROL UNPACKING - - -/** - * @brief Get field target_system from mount_control message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_mount_control_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 12); -} - -/** - * @brief Get field target_component from mount_control message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_mount_control_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 13); -} - -/** - * @brief Get field input_a from mount_control message - * - * @return pitch(deg*100) or lat, depending on mount mode - */ -static inline int32_t mavlink_msg_mount_control_get_input_a(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field input_b from mount_control message - * - * @return roll(deg*100) or lon depending on mount mode - */ -static inline int32_t mavlink_msg_mount_control_get_input_b(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field input_c from mount_control message - * - * @return yaw(deg*100) or alt (in cm) depending on mount mode - */ -static inline int32_t mavlink_msg_mount_control_get_input_c(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Get field save_position from mount_control message - * - * @return if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) - */ -static inline uint8_t mavlink_msg_mount_control_get_save_position(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 14); -} - -/** - * @brief Decode a mount_control message into a struct - * - * @param msg The message to decode - * @param mount_control C-struct to decode the message contents into - */ -static inline void mavlink_msg_mount_control_decode(const mavlink_message_t* msg, mavlink_mount_control_t* mount_control) -{ -#if MAVLINK_NEED_BYTE_SWAP - mount_control->input_a = mavlink_msg_mount_control_get_input_a(msg); - mount_control->input_b = mavlink_msg_mount_control_get_input_b(msg); - mount_control->input_c = mavlink_msg_mount_control_get_input_c(msg); - mount_control->target_system = mavlink_msg_mount_control_get_target_system(msg); - mount_control->target_component = mavlink_msg_mount_control_get_target_component(msg); - mount_control->save_position = mavlink_msg_mount_control_get_save_position(msg); -#else - memcpy(mount_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h deleted file mode 100644 index 4905905..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h +++ /dev/null @@ -1,265 +0,0 @@ -// MESSAGE MOUNT_STATUS PACKING - -#define MAVLINK_MSG_ID_MOUNT_STATUS 158 - -typedef struct __mavlink_mount_status_t -{ - int32_t pointing_a; ///< pitch(deg*100) or lat, depending on mount mode - int32_t pointing_b; ///< roll(deg*100) or lon depending on mount mode - int32_t pointing_c; ///< yaw(deg*100) or alt (in cm) depending on mount mode - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_mount_status_t; - -#define MAVLINK_MSG_ID_MOUNT_STATUS_LEN 14 -#define MAVLINK_MSG_ID_158_LEN 14 - -#define MAVLINK_MSG_ID_MOUNT_STATUS_CRC 134 -#define MAVLINK_MSG_ID_158_CRC 134 - - - -#define MAVLINK_MESSAGE_INFO_MOUNT_STATUS { \ - "MOUNT_STATUS", \ - 5, \ - { { "pointing_a", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_mount_status_t, pointing_a) }, \ - { "pointing_b", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_mount_status_t, pointing_b) }, \ - { "pointing_c", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_mount_status_t, pointing_c) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_mount_status_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_mount_status_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a mount_status message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param pointing_a pitch(deg*100) or lat, depending on mount mode - * @param pointing_b roll(deg*100) or lon depending on mount mode - * @param pointing_c yaw(deg*100) or alt (in cm) depending on mount mode - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mount_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, int32_t pointing_a, int32_t pointing_b, int32_t pointing_c) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MOUNT_STATUS_LEN]; - _mav_put_int32_t(buf, 0, pointing_a); - _mav_put_int32_t(buf, 4, pointing_b); - _mav_put_int32_t(buf, 8, pointing_c); - _mav_put_uint8_t(buf, 12, target_system); - _mav_put_uint8_t(buf, 13, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); -#else - mavlink_mount_status_t packet; - packet.pointing_a = pointing_a; - packet.pointing_b = pointing_b; - packet.pointing_c = pointing_c; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MOUNT_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); -#endif -} - -/** - * @brief Pack a mount_status message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param pointing_a pitch(deg*100) or lat, depending on mount mode - * @param pointing_b roll(deg*100) or lon depending on mount mode - * @param pointing_c yaw(deg*100) or alt (in cm) depending on mount mode - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mount_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,int32_t pointing_a,int32_t pointing_b,int32_t pointing_c) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MOUNT_STATUS_LEN]; - _mav_put_int32_t(buf, 0, pointing_a); - _mav_put_int32_t(buf, 4, pointing_b); - _mav_put_int32_t(buf, 8, pointing_c); - _mav_put_uint8_t(buf, 12, target_system); - _mav_put_uint8_t(buf, 13, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); -#else - mavlink_mount_status_t packet; - packet.pointing_a = pointing_a; - packet.pointing_b = pointing_b; - packet.pointing_c = pointing_c; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MOUNT_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); -#endif -} - -/** - * @brief Encode a mount_status struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param mount_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mount_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mount_status_t* mount_status) -{ - return mavlink_msg_mount_status_pack(system_id, component_id, msg, mount_status->target_system, mount_status->target_component, mount_status->pointing_a, mount_status->pointing_b, mount_status->pointing_c); -} - -/** - * @brief Encode a mount_status struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param mount_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mount_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_status_t* mount_status) -{ - return mavlink_msg_mount_status_pack_chan(system_id, component_id, chan, msg, mount_status->target_system, mount_status->target_component, mount_status->pointing_a, mount_status->pointing_b, mount_status->pointing_c); -} - -/** - * @brief Send a mount_status message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param pointing_a pitch(deg*100) or lat, depending on mount mode - * @param pointing_b roll(deg*100) or lon depending on mount mode - * @param pointing_c yaw(deg*100) or alt (in cm) depending on mount mode - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_mount_status_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t pointing_a, int32_t pointing_b, int32_t pointing_c) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MOUNT_STATUS_LEN]; - _mav_put_int32_t(buf, 0, pointing_a); - _mav_put_int32_t(buf, 4, pointing_b); - _mav_put_int32_t(buf, 8, pointing_c); - _mav_put_uint8_t(buf, 12, target_system); - _mav_put_uint8_t(buf, 13, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); -#endif -#else - mavlink_mount_status_t packet; - packet.pointing_a = pointing_a; - packet.pointing_b = pointing_b; - packet.pointing_c = pointing_c; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); -#endif -#endif -} - -#endif - -// MESSAGE MOUNT_STATUS UNPACKING - - -/** - * @brief Get field target_system from mount_status message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_mount_status_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 12); -} - -/** - * @brief Get field target_component from mount_status message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_mount_status_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 13); -} - -/** - * @brief Get field pointing_a from mount_status message - * - * @return pitch(deg*100) or lat, depending on mount mode - */ -static inline int32_t mavlink_msg_mount_status_get_pointing_a(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field pointing_b from mount_status message - * - * @return roll(deg*100) or lon depending on mount mode - */ -static inline int32_t mavlink_msg_mount_status_get_pointing_b(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field pointing_c from mount_status message - * - * @return yaw(deg*100) or alt (in cm) depending on mount mode - */ -static inline int32_t mavlink_msg_mount_status_get_pointing_c(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Decode a mount_status message into a struct - * - * @param msg The message to decode - * @param mount_status C-struct to decode the message contents into - */ -static inline void mavlink_msg_mount_status_decode(const mavlink_message_t* msg, mavlink_mount_status_t* mount_status) -{ -#if MAVLINK_NEED_BYTE_SWAP - mount_status->pointing_a = mavlink_msg_mount_status_get_pointing_a(msg); - mount_status->pointing_b = mavlink_msg_mount_status_get_pointing_b(msg); - mount_status->pointing_c = mavlink_msg_mount_status_get_pointing_c(msg); - mount_status->target_system = mavlink_msg_mount_status_get_target_system(msg); - mount_status->target_component = mavlink_msg_mount_status_get_target_component(msg); -#else - memcpy(mount_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MOUNT_STATUS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h deleted file mode 100644 index 8e9740e..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h +++ /dev/null @@ -1,309 +0,0 @@ -// MESSAGE RADIO PACKING - -#define MAVLINK_MSG_ID_RADIO 166 - -typedef struct __mavlink_radio_t -{ - uint16_t rxerrors; ///< receive errors - uint16_t fixed; ///< count of error corrected packets - uint8_t rssi; ///< local signal strength - uint8_t remrssi; ///< remote signal strength - uint8_t txbuf; ///< how full the tx buffer is as a percentage - uint8_t noise; ///< background noise level - uint8_t remnoise; ///< remote background noise level -} mavlink_radio_t; - -#define MAVLINK_MSG_ID_RADIO_LEN 9 -#define MAVLINK_MSG_ID_166_LEN 9 - -#define MAVLINK_MSG_ID_RADIO_CRC 21 -#define MAVLINK_MSG_ID_166_CRC 21 - - - -#define MAVLINK_MESSAGE_INFO_RADIO { \ - "RADIO", \ - 7, \ - { { "rxerrors", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_radio_t, rxerrors) }, \ - { "fixed", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_radio_t, fixed) }, \ - { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_radio_t, rssi) }, \ - { "remrssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_radio_t, remrssi) }, \ - { "txbuf", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_radio_t, txbuf) }, \ - { "noise", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_radio_t, noise) }, \ - { "remnoise", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_radio_t, remnoise) }, \ - } \ -} - - -/** - * @brief Pack a radio message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param rssi local signal strength - * @param remrssi remote signal strength - * @param txbuf how full the tx buffer is as a percentage - * @param noise background noise level - * @param remnoise remote background noise level - * @param rxerrors receive errors - * @param fixed count of error corrected packets - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_radio_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RADIO_LEN]; - _mav_put_uint16_t(buf, 0, rxerrors); - _mav_put_uint16_t(buf, 2, fixed); - _mav_put_uint8_t(buf, 4, rssi); - _mav_put_uint8_t(buf, 5, remrssi); - _mav_put_uint8_t(buf, 6, txbuf); - _mav_put_uint8_t(buf, 7, noise); - _mav_put_uint8_t(buf, 8, remnoise); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RADIO_LEN); -#else - mavlink_radio_t packet; - packet.rxerrors = rxerrors; - packet.fixed = fixed; - packet.rssi = rssi; - packet.remrssi = remrssi; - packet.txbuf = txbuf; - packet.noise = noise; - packet.remnoise = remnoise; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RADIO_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RADIO; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_LEN); -#endif -} - -/** - * @brief Pack a radio message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param rssi local signal strength - * @param remrssi remote signal strength - * @param txbuf how full the tx buffer is as a percentage - * @param noise background noise level - * @param remnoise remote background noise level - * @param rxerrors receive errors - * @param fixed count of error corrected packets - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_radio_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t rssi,uint8_t remrssi,uint8_t txbuf,uint8_t noise,uint8_t remnoise,uint16_t rxerrors,uint16_t fixed) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RADIO_LEN]; - _mav_put_uint16_t(buf, 0, rxerrors); - _mav_put_uint16_t(buf, 2, fixed); - _mav_put_uint8_t(buf, 4, rssi); - _mav_put_uint8_t(buf, 5, remrssi); - _mav_put_uint8_t(buf, 6, txbuf); - _mav_put_uint8_t(buf, 7, noise); - _mav_put_uint8_t(buf, 8, remnoise); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RADIO_LEN); -#else - mavlink_radio_t packet; - packet.rxerrors = rxerrors; - packet.fixed = fixed; - packet.rssi = rssi; - packet.remrssi = remrssi; - packet.txbuf = txbuf; - packet.noise = noise; - packet.remnoise = remnoise; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RADIO_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RADIO; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_LEN); -#endif -} - -/** - * @brief Encode a radio struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param radio C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_radio_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_radio_t* radio) -{ - return mavlink_msg_radio_pack(system_id, component_id, msg, radio->rssi, radio->remrssi, radio->txbuf, radio->noise, radio->remnoise, radio->rxerrors, radio->fixed); -} - -/** - * @brief Encode a radio struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param radio C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_radio_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_radio_t* radio) -{ - return mavlink_msg_radio_pack_chan(system_id, component_id, chan, msg, radio->rssi, radio->remrssi, radio->txbuf, radio->noise, radio->remnoise, radio->rxerrors, radio->fixed); -} - -/** - * @brief Send a radio message - * @param chan MAVLink channel to send the message - * - * @param rssi local signal strength - * @param remrssi remote signal strength - * @param txbuf how full the tx buffer is as a percentage - * @param noise background noise level - * @param remnoise remote background noise level - * @param rxerrors receive errors - * @param fixed count of error corrected packets - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_radio_send(mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RADIO_LEN]; - _mav_put_uint16_t(buf, 0, rxerrors); - _mav_put_uint16_t(buf, 2, fixed); - _mav_put_uint8_t(buf, 4, rssi); - _mav_put_uint8_t(buf, 5, remrssi); - _mav_put_uint8_t(buf, 6, txbuf); - _mav_put_uint8_t(buf, 7, noise); - _mav_put_uint8_t(buf, 8, remnoise); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, MAVLINK_MSG_ID_RADIO_LEN); -#endif -#else - mavlink_radio_t packet; - packet.rxerrors = rxerrors; - packet.fixed = fixed; - packet.rssi = rssi; - packet.remrssi = remrssi; - packet.txbuf = txbuf; - packet.noise = noise; - packet.remnoise = remnoise; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)&packet, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)&packet, MAVLINK_MSG_ID_RADIO_LEN); -#endif -#endif -} - -#endif - -// MESSAGE RADIO UNPACKING - - -/** - * @brief Get field rssi from radio message - * - * @return local signal strength - */ -static inline uint8_t mavlink_msg_radio_get_rssi(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field remrssi from radio message - * - * @return remote signal strength - */ -static inline uint8_t mavlink_msg_radio_get_remrssi(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field txbuf from radio message - * - * @return how full the tx buffer is as a percentage - */ -static inline uint8_t mavlink_msg_radio_get_txbuf(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @brief Get field noise from radio message - * - * @return background noise level - */ -static inline uint8_t mavlink_msg_radio_get_noise(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 7); -} - -/** - * @brief Get field remnoise from radio message - * - * @return remote background noise level - */ -static inline uint8_t mavlink_msg_radio_get_remnoise(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 8); -} - -/** - * @brief Get field rxerrors from radio message - * - * @return receive errors - */ -static inline uint16_t mavlink_msg_radio_get_rxerrors(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field fixed from radio message - * - * @return count of error corrected packets - */ -static inline uint16_t mavlink_msg_radio_get_fixed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Decode a radio message into a struct - * - * @param msg The message to decode - * @param radio C-struct to decode the message contents into - */ -static inline void mavlink_msg_radio_decode(const mavlink_message_t* msg, mavlink_radio_t* radio) -{ -#if MAVLINK_NEED_BYTE_SWAP - radio->rxerrors = mavlink_msg_radio_get_rxerrors(msg); - radio->fixed = mavlink_msg_radio_get_fixed(msg); - radio->rssi = mavlink_msg_radio_get_rssi(msg); - radio->remrssi = mavlink_msg_radio_get_remrssi(msg); - radio->txbuf = mavlink_msg_radio_get_txbuf(msg); - radio->noise = mavlink_msg_radio_get_noise(msg); - radio->remnoise = mavlink_msg_radio_get_remnoise(msg); -#else - memcpy(radio, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RADIO_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h deleted file mode 100644 index f057e3c..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h +++ /dev/null @@ -1,221 +0,0 @@ -// MESSAGE RALLY_FETCH_POINT PACKING - -#define MAVLINK_MSG_ID_RALLY_FETCH_POINT 176 - -typedef struct __mavlink_rally_fetch_point_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t idx; ///< point index (first point is 0) -} mavlink_rally_fetch_point_t; - -#define MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN 3 -#define MAVLINK_MSG_ID_176_LEN 3 - -#define MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC 234 -#define MAVLINK_MSG_ID_176_CRC 234 - - - -#define MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT { \ - "RALLY_FETCH_POINT", \ - 3, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_rally_fetch_point_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_rally_fetch_point_t, target_component) }, \ - { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_rally_fetch_point_t, idx) }, \ - } \ -} - - -/** - * @brief Pack a rally_fetch_point message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 0) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_rally_fetch_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t idx) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, idx); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); -#else - mavlink_rally_fetch_point_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RALLY_FETCH_POINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); -#endif -} - -/** - * @brief Pack a rally_fetch_point message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 0) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_rally_fetch_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t idx) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, idx); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); -#else - mavlink_rally_fetch_point_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RALLY_FETCH_POINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); -#endif -} - -/** - * @brief Encode a rally_fetch_point struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param rally_fetch_point C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_rally_fetch_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rally_fetch_point_t* rally_fetch_point) -{ - return mavlink_msg_rally_fetch_point_pack(system_id, component_id, msg, rally_fetch_point->target_system, rally_fetch_point->target_component, rally_fetch_point->idx); -} - -/** - * @brief Encode a rally_fetch_point struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param rally_fetch_point C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_rally_fetch_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rally_fetch_point_t* rally_fetch_point) -{ - return mavlink_msg_rally_fetch_point_pack_chan(system_id, component_id, chan, msg, rally_fetch_point->target_system, rally_fetch_point->target_component, rally_fetch_point->idx); -} - -/** - * @brief Send a rally_fetch_point message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 0) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_rally_fetch_point_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, idx); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); -#endif -#else - mavlink_rally_fetch_point_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, (const char *)&packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, (const char *)&packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); -#endif -#endif -} - -#endif - -// MESSAGE RALLY_FETCH_POINT UNPACKING - - -/** - * @brief Get field target_system from rally_fetch_point message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_rally_fetch_point_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from rally_fetch_point message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_rally_fetch_point_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field idx from rally_fetch_point message - * - * @return point index (first point is 0) - */ -static inline uint8_t mavlink_msg_rally_fetch_point_get_idx(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Decode a rally_fetch_point message into a struct - * - * @param msg The message to decode - * @param rally_fetch_point C-struct to decode the message contents into - */ -static inline void mavlink_msg_rally_fetch_point_decode(const mavlink_message_t* msg, mavlink_rally_fetch_point_t* rally_fetch_point) -{ -#if MAVLINK_NEED_BYTE_SWAP - rally_fetch_point->target_system = mavlink_msg_rally_fetch_point_get_target_system(msg); - rally_fetch_point->target_component = mavlink_msg_rally_fetch_point_get_target_component(msg); - rally_fetch_point->idx = mavlink_msg_rally_fetch_point_get_idx(msg); -#else - memcpy(rally_fetch_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h deleted file mode 100644 index 2c21db4..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h +++ /dev/null @@ -1,375 +0,0 @@ -// MESSAGE RALLY_POINT PACKING - -#define MAVLINK_MSG_ID_RALLY_POINT 175 - -typedef struct __mavlink_rally_point_t -{ - int32_t lat; ///< Latitude of point in degrees * 1E7 - int32_t lng; ///< Longitude of point in degrees * 1E7 - int16_t alt; ///< Transit / loiter altitude in meters relative to home - int16_t break_alt; ///< Break altitude in meters relative to home - uint16_t land_dir; ///< Heading to aim for when landing. In centi-degrees. - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t idx; ///< point index (first point is 0) - uint8_t count; ///< total number of points (for sanity checking) - uint8_t flags; ///< See RALLY_FLAGS enum for definition of the bitmask. -} mavlink_rally_point_t; - -#define MAVLINK_MSG_ID_RALLY_POINT_LEN 19 -#define MAVLINK_MSG_ID_175_LEN 19 - -#define MAVLINK_MSG_ID_RALLY_POINT_CRC 138 -#define MAVLINK_MSG_ID_175_CRC 138 - - - -#define MAVLINK_MESSAGE_INFO_RALLY_POINT { \ - "RALLY_POINT", \ - 10, \ - { { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_rally_point_t, lat) }, \ - { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_rally_point_t, lng) }, \ - { "alt", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_rally_point_t, alt) }, \ - { "break_alt", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_rally_point_t, break_alt) }, \ - { "land_dir", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rally_point_t, land_dir) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_rally_point_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_rally_point_t, target_component) }, \ - { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_rally_point_t, idx) }, \ - { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_rally_point_t, count) }, \ - { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_rally_point_t, flags) }, \ - } \ -} - - -/** - * @brief Pack a rally_point message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 0) - * @param count total number of points (for sanity checking) - * @param lat Latitude of point in degrees * 1E7 - * @param lng Longitude of point in degrees * 1E7 - * @param alt Transit / loiter altitude in meters relative to home - * @param break_alt Break altitude in meters relative to home - * @param land_dir Heading to aim for when landing. In centi-degrees. - * @param flags See RALLY_FLAGS enum for definition of the bitmask. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_rally_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, int32_t lat, int32_t lng, int16_t alt, int16_t break_alt, uint16_t land_dir, uint8_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RALLY_POINT_LEN]; - _mav_put_int32_t(buf, 0, lat); - _mav_put_int32_t(buf, 4, lng); - _mav_put_int16_t(buf, 8, alt); - _mav_put_int16_t(buf, 10, break_alt); - _mav_put_uint16_t(buf, 12, land_dir); - _mav_put_uint8_t(buf, 14, target_system); - _mav_put_uint8_t(buf, 15, target_component); - _mav_put_uint8_t(buf, 16, idx); - _mav_put_uint8_t(buf, 17, count); - _mav_put_uint8_t(buf, 18, flags); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_POINT_LEN); -#else - mavlink_rally_point_t packet; - packet.lat = lat; - packet.lng = lng; - packet.alt = alt; - packet.break_alt = break_alt; - packet.land_dir = land_dir; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - packet.count = count; - packet.flags = flags; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_POINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RALLY_POINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_POINT_LEN); -#endif -} - -/** - * @brief Pack a rally_point message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 0) - * @param count total number of points (for sanity checking) - * @param lat Latitude of point in degrees * 1E7 - * @param lng Longitude of point in degrees * 1E7 - * @param alt Transit / loiter altitude in meters relative to home - * @param break_alt Break altitude in meters relative to home - * @param land_dir Heading to aim for when landing. In centi-degrees. - * @param flags See RALLY_FLAGS enum for definition of the bitmask. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_rally_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t idx,uint8_t count,int32_t lat,int32_t lng,int16_t alt,int16_t break_alt,uint16_t land_dir,uint8_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RALLY_POINT_LEN]; - _mav_put_int32_t(buf, 0, lat); - _mav_put_int32_t(buf, 4, lng); - _mav_put_int16_t(buf, 8, alt); - _mav_put_int16_t(buf, 10, break_alt); - _mav_put_uint16_t(buf, 12, land_dir); - _mav_put_uint8_t(buf, 14, target_system); - _mav_put_uint8_t(buf, 15, target_component); - _mav_put_uint8_t(buf, 16, idx); - _mav_put_uint8_t(buf, 17, count); - _mav_put_uint8_t(buf, 18, flags); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_POINT_LEN); -#else - mavlink_rally_point_t packet; - packet.lat = lat; - packet.lng = lng; - packet.alt = alt; - packet.break_alt = break_alt; - packet.land_dir = land_dir; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - packet.count = count; - packet.flags = flags; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_POINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RALLY_POINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_POINT_LEN); -#endif -} - -/** - * @brief Encode a rally_point struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param rally_point C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_rally_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rally_point_t* rally_point) -{ - return mavlink_msg_rally_point_pack(system_id, component_id, msg, rally_point->target_system, rally_point->target_component, rally_point->idx, rally_point->count, rally_point->lat, rally_point->lng, rally_point->alt, rally_point->break_alt, rally_point->land_dir, rally_point->flags); -} - -/** - * @brief Encode a rally_point struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param rally_point C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_rally_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rally_point_t* rally_point) -{ - return mavlink_msg_rally_point_pack_chan(system_id, component_id, chan, msg, rally_point->target_system, rally_point->target_component, rally_point->idx, rally_point->count, rally_point->lat, rally_point->lng, rally_point->alt, rally_point->break_alt, rally_point->land_dir, rally_point->flags); -} - -/** - * @brief Send a rally_point message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 0) - * @param count total number of points (for sanity checking) - * @param lat Latitude of point in degrees * 1E7 - * @param lng Longitude of point in degrees * 1E7 - * @param alt Transit / loiter altitude in meters relative to home - * @param break_alt Break altitude in meters relative to home - * @param land_dir Heading to aim for when landing. In centi-degrees. - * @param flags See RALLY_FLAGS enum for definition of the bitmask. - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_rally_point_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, int32_t lat, int32_t lng, int16_t alt, int16_t break_alt, uint16_t land_dir, uint8_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RALLY_POINT_LEN]; - _mav_put_int32_t(buf, 0, lat); - _mav_put_int32_t(buf, 4, lng); - _mav_put_int16_t(buf, 8, alt); - _mav_put_int16_t(buf, 10, break_alt); - _mav_put_uint16_t(buf, 12, land_dir); - _mav_put_uint8_t(buf, 14, target_system); - _mav_put_uint8_t(buf, 15, target_component); - _mav_put_uint8_t(buf, 16, idx); - _mav_put_uint8_t(buf, 17, count); - _mav_put_uint8_t(buf, 18, flags); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, buf, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, buf, MAVLINK_MSG_ID_RALLY_POINT_LEN); -#endif -#else - mavlink_rally_point_t packet; - packet.lat = lat; - packet.lng = lng; - packet.alt = alt; - packet.break_alt = break_alt; - packet.land_dir = land_dir; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - packet.count = count; - packet.flags = flags; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, (const char *)&packet, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, (const char *)&packet, MAVLINK_MSG_ID_RALLY_POINT_LEN); -#endif -#endif -} - -#endif - -// MESSAGE RALLY_POINT UNPACKING - - -/** - * @brief Get field target_system from rally_point message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_rally_point_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 14); -} - -/** - * @brief Get field target_component from rally_point message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_rally_point_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 15); -} - -/** - * @brief Get field idx from rally_point message - * - * @return point index (first point is 0) - */ -static inline uint8_t mavlink_msg_rally_point_get_idx(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Get field count from rally_point message - * - * @return total number of points (for sanity checking) - */ -static inline uint8_t mavlink_msg_rally_point_get_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 17); -} - -/** - * @brief Get field lat from rally_point message - * - * @return Latitude of point in degrees * 1E7 - */ -static inline int32_t mavlink_msg_rally_point_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field lng from rally_point message - * - * @return Longitude of point in degrees * 1E7 - */ -static inline int32_t mavlink_msg_rally_point_get_lng(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field alt from rally_point message - * - * @return Transit / loiter altitude in meters relative to home - */ -static inline int16_t mavlink_msg_rally_point_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 8); -} - -/** - * @brief Get field break_alt from rally_point message - * - * @return Break altitude in meters relative to home - */ -static inline int16_t mavlink_msg_rally_point_get_break_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 10); -} - -/** - * @brief Get field land_dir from rally_point message - * - * @return Heading to aim for when landing. In centi-degrees. - */ -static inline uint16_t mavlink_msg_rally_point_get_land_dir(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 12); -} - -/** - * @brief Get field flags from rally_point message - * - * @return See RALLY_FLAGS enum for definition of the bitmask. - */ -static inline uint8_t mavlink_msg_rally_point_get_flags(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 18); -} - -/** - * @brief Decode a rally_point message into a struct - * - * @param msg The message to decode - * @param rally_point C-struct to decode the message contents into - */ -static inline void mavlink_msg_rally_point_decode(const mavlink_message_t* msg, mavlink_rally_point_t* rally_point) -{ -#if MAVLINK_NEED_BYTE_SWAP - rally_point->lat = mavlink_msg_rally_point_get_lat(msg); - rally_point->lng = mavlink_msg_rally_point_get_lng(msg); - rally_point->alt = mavlink_msg_rally_point_get_alt(msg); - rally_point->break_alt = mavlink_msg_rally_point_get_break_alt(msg); - rally_point->land_dir = mavlink_msg_rally_point_get_land_dir(msg); - rally_point->target_system = mavlink_msg_rally_point_get_target_system(msg); - rally_point->target_component = mavlink_msg_rally_point_get_target_component(msg); - rally_point->idx = mavlink_msg_rally_point_get_idx(msg); - rally_point->count = mavlink_msg_rally_point_get_count(msg); - rally_point->flags = mavlink_msg_rally_point_get_flags(msg); -#else - memcpy(rally_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RALLY_POINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h deleted file mode 100644 index c476447..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h +++ /dev/null @@ -1,199 +0,0 @@ -// MESSAGE RANGEFINDER PACKING - -#define MAVLINK_MSG_ID_RANGEFINDER 173 - -typedef struct __mavlink_rangefinder_t -{ - float distance; ///< distance in meters - float voltage; ///< raw voltage if available, zero otherwise -} mavlink_rangefinder_t; - -#define MAVLINK_MSG_ID_RANGEFINDER_LEN 8 -#define MAVLINK_MSG_ID_173_LEN 8 - -#define MAVLINK_MSG_ID_RANGEFINDER_CRC 83 -#define MAVLINK_MSG_ID_173_CRC 83 - - - -#define MAVLINK_MESSAGE_INFO_RANGEFINDER { \ - "RANGEFINDER", \ - 2, \ - { { "distance", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_rangefinder_t, distance) }, \ - { "voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_rangefinder_t, voltage) }, \ - } \ -} - - -/** - * @brief Pack a rangefinder message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param distance distance in meters - * @param voltage raw voltage if available, zero otherwise - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_rangefinder_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float distance, float voltage) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RANGEFINDER_LEN]; - _mav_put_float(buf, 0, distance); - _mav_put_float(buf, 4, voltage); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RANGEFINDER_LEN); -#else - mavlink_rangefinder_t packet; - packet.distance = distance; - packet.voltage = voltage; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RANGEFINDER_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RANGEFINDER; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RANGEFINDER_LEN); -#endif -} - -/** - * @brief Pack a rangefinder message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param distance distance in meters - * @param voltage raw voltage if available, zero otherwise - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_rangefinder_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float distance,float voltage) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RANGEFINDER_LEN]; - _mav_put_float(buf, 0, distance); - _mav_put_float(buf, 4, voltage); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RANGEFINDER_LEN); -#else - mavlink_rangefinder_t packet; - packet.distance = distance; - packet.voltage = voltage; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RANGEFINDER_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RANGEFINDER; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RANGEFINDER_LEN); -#endif -} - -/** - * @brief Encode a rangefinder struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param rangefinder C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_rangefinder_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rangefinder_t* rangefinder) -{ - return mavlink_msg_rangefinder_pack(system_id, component_id, msg, rangefinder->distance, rangefinder->voltage); -} - -/** - * @brief Encode a rangefinder struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param rangefinder C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_rangefinder_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rangefinder_t* rangefinder) -{ - return mavlink_msg_rangefinder_pack_chan(system_id, component_id, chan, msg, rangefinder->distance, rangefinder->voltage); -} - -/** - * @brief Send a rangefinder message - * @param chan MAVLink channel to send the message - * - * @param distance distance in meters - * @param voltage raw voltage if available, zero otherwise - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_rangefinder_send(mavlink_channel_t chan, float distance, float voltage) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RANGEFINDER_LEN]; - _mav_put_float(buf, 0, distance); - _mav_put_float(buf, 4, voltage); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, buf, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, buf, MAVLINK_MSG_ID_RANGEFINDER_LEN); -#endif -#else - mavlink_rangefinder_t packet; - packet.distance = distance; - packet.voltage = voltage; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, (const char *)&packet, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, (const char *)&packet, MAVLINK_MSG_ID_RANGEFINDER_LEN); -#endif -#endif -} - -#endif - -// MESSAGE RANGEFINDER UNPACKING - - -/** - * @brief Get field distance from rangefinder message - * - * @return distance in meters - */ -static inline float mavlink_msg_rangefinder_get_distance(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field voltage from rangefinder message - * - * @return raw voltage if available, zero otherwise - */ -static inline float mavlink_msg_rangefinder_get_voltage(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Decode a rangefinder message into a struct - * - * @param msg The message to decode - * @param rangefinder C-struct to decode the message contents into - */ -static inline void mavlink_msg_rangefinder_decode(const mavlink_message_t* msg, mavlink_rangefinder_t* rangefinder) -{ -#if MAVLINK_NEED_BYTE_SWAP - rangefinder->distance = mavlink_msg_rangefinder_get_distance(msg); - rangefinder->voltage = mavlink_msg_rangefinder_get_voltage(msg); -#else - memcpy(rangefinder, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RANGEFINDER_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h deleted file mode 100644 index 31b7d98..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h +++ /dev/null @@ -1,419 +0,0 @@ -// MESSAGE SENSOR_OFFSETS PACKING - -#define MAVLINK_MSG_ID_SENSOR_OFFSETS 150 - -typedef struct __mavlink_sensor_offsets_t -{ - float mag_declination; ///< magnetic declination (radians) - int32_t raw_press; ///< raw pressure from barometer - int32_t raw_temp; ///< raw temperature from barometer - float gyro_cal_x; ///< gyro X calibration - float gyro_cal_y; ///< gyro Y calibration - float gyro_cal_z; ///< gyro Z calibration - float accel_cal_x; ///< accel X calibration - float accel_cal_y; ///< accel Y calibration - float accel_cal_z; ///< accel Z calibration - int16_t mag_ofs_x; ///< magnetometer X offset - int16_t mag_ofs_y; ///< magnetometer Y offset - int16_t mag_ofs_z; ///< magnetometer Z offset -} mavlink_sensor_offsets_t; - -#define MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN 42 -#define MAVLINK_MSG_ID_150_LEN 42 - -#define MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC 134 -#define MAVLINK_MSG_ID_150_CRC 134 - - - -#define MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS { \ - "SENSOR_OFFSETS", \ - 12, \ - { { "mag_declination", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_sensor_offsets_t, mag_declination) }, \ - { "raw_press", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_sensor_offsets_t, raw_press) }, \ - { "raw_temp", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_sensor_offsets_t, raw_temp) }, \ - { "gyro_cal_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_sensor_offsets_t, gyro_cal_x) }, \ - { "gyro_cal_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_sensor_offsets_t, gyro_cal_y) }, \ - { "gyro_cal_z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_sensor_offsets_t, gyro_cal_z) }, \ - { "accel_cal_x", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_sensor_offsets_t, accel_cal_x) }, \ - { "accel_cal_y", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_sensor_offsets_t, accel_cal_y) }, \ - { "accel_cal_z", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_sensor_offsets_t, accel_cal_z) }, \ - { "mag_ofs_x", NULL, MAVLINK_TYPE_INT16_T, 0, 36, offsetof(mavlink_sensor_offsets_t, mag_ofs_x) }, \ - { "mag_ofs_y", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_sensor_offsets_t, mag_ofs_y) }, \ - { "mag_ofs_z", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_sensor_offsets_t, mag_ofs_z) }, \ - } \ -} - - -/** - * @brief Pack a sensor_offsets message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param mag_ofs_x magnetometer X offset - * @param mag_ofs_y magnetometer Y offset - * @param mag_ofs_z magnetometer Z offset - * @param mag_declination magnetic declination (radians) - * @param raw_press raw pressure from barometer - * @param raw_temp raw temperature from barometer - * @param gyro_cal_x gyro X calibration - * @param gyro_cal_y gyro Y calibration - * @param gyro_cal_z gyro Z calibration - * @param accel_cal_x accel X calibration - * @param accel_cal_y accel Y calibration - * @param accel_cal_z accel Z calibration - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_sensor_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN]; - _mav_put_float(buf, 0, mag_declination); - _mav_put_int32_t(buf, 4, raw_press); - _mav_put_int32_t(buf, 8, raw_temp); - _mav_put_float(buf, 12, gyro_cal_x); - _mav_put_float(buf, 16, gyro_cal_y); - _mav_put_float(buf, 20, gyro_cal_z); - _mav_put_float(buf, 24, accel_cal_x); - _mav_put_float(buf, 28, accel_cal_y); - _mav_put_float(buf, 32, accel_cal_z); - _mav_put_int16_t(buf, 36, mag_ofs_x); - _mav_put_int16_t(buf, 38, mag_ofs_y); - _mav_put_int16_t(buf, 40, mag_ofs_z); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); -#else - mavlink_sensor_offsets_t packet; - packet.mag_declination = mag_declination; - packet.raw_press = raw_press; - packet.raw_temp = raw_temp; - packet.gyro_cal_x = gyro_cal_x; - packet.gyro_cal_y = gyro_cal_y; - packet.gyro_cal_z = gyro_cal_z; - packet.accel_cal_x = accel_cal_x; - packet.accel_cal_y = accel_cal_y; - packet.accel_cal_z = accel_cal_z; - packet.mag_ofs_x = mag_ofs_x; - packet.mag_ofs_y = mag_ofs_y; - packet.mag_ofs_z = mag_ofs_z; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); -#endif -} - -/** - * @brief Pack a sensor_offsets message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param mag_ofs_x magnetometer X offset - * @param mag_ofs_y magnetometer Y offset - * @param mag_ofs_z magnetometer Z offset - * @param mag_declination magnetic declination (radians) - * @param raw_press raw pressure from barometer - * @param raw_temp raw temperature from barometer - * @param gyro_cal_x gyro X calibration - * @param gyro_cal_y gyro Y calibration - * @param gyro_cal_z gyro Z calibration - * @param accel_cal_x accel X calibration - * @param accel_cal_y accel Y calibration - * @param accel_cal_z accel Z calibration - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_sensor_offsets_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - int16_t mag_ofs_x,int16_t mag_ofs_y,int16_t mag_ofs_z,float mag_declination,int32_t raw_press,int32_t raw_temp,float gyro_cal_x,float gyro_cal_y,float gyro_cal_z,float accel_cal_x,float accel_cal_y,float accel_cal_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN]; - _mav_put_float(buf, 0, mag_declination); - _mav_put_int32_t(buf, 4, raw_press); - _mav_put_int32_t(buf, 8, raw_temp); - _mav_put_float(buf, 12, gyro_cal_x); - _mav_put_float(buf, 16, gyro_cal_y); - _mav_put_float(buf, 20, gyro_cal_z); - _mav_put_float(buf, 24, accel_cal_x); - _mav_put_float(buf, 28, accel_cal_y); - _mav_put_float(buf, 32, accel_cal_z); - _mav_put_int16_t(buf, 36, mag_ofs_x); - _mav_put_int16_t(buf, 38, mag_ofs_y); - _mav_put_int16_t(buf, 40, mag_ofs_z); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); -#else - mavlink_sensor_offsets_t packet; - packet.mag_declination = mag_declination; - packet.raw_press = raw_press; - packet.raw_temp = raw_temp; - packet.gyro_cal_x = gyro_cal_x; - packet.gyro_cal_y = gyro_cal_y; - packet.gyro_cal_z = gyro_cal_z; - packet.accel_cal_x = accel_cal_x; - packet.accel_cal_y = accel_cal_y; - packet.accel_cal_z = accel_cal_z; - packet.mag_ofs_x = mag_ofs_x; - packet.mag_ofs_y = mag_ofs_y; - packet.mag_ofs_z = mag_ofs_z; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); -#endif -} - -/** - * @brief Encode a sensor_offsets struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param sensor_offsets C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_sensor_offsets_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sensor_offsets_t* sensor_offsets) -{ - return mavlink_msg_sensor_offsets_pack(system_id, component_id, msg, sensor_offsets->mag_ofs_x, sensor_offsets->mag_ofs_y, sensor_offsets->mag_ofs_z, sensor_offsets->mag_declination, sensor_offsets->raw_press, sensor_offsets->raw_temp, sensor_offsets->gyro_cal_x, sensor_offsets->gyro_cal_y, sensor_offsets->gyro_cal_z, sensor_offsets->accel_cal_x, sensor_offsets->accel_cal_y, sensor_offsets->accel_cal_z); -} - -/** - * @brief Encode a sensor_offsets struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sensor_offsets C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_sensor_offsets_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sensor_offsets_t* sensor_offsets) -{ - return mavlink_msg_sensor_offsets_pack_chan(system_id, component_id, chan, msg, sensor_offsets->mag_ofs_x, sensor_offsets->mag_ofs_y, sensor_offsets->mag_ofs_z, sensor_offsets->mag_declination, sensor_offsets->raw_press, sensor_offsets->raw_temp, sensor_offsets->gyro_cal_x, sensor_offsets->gyro_cal_y, sensor_offsets->gyro_cal_z, sensor_offsets->accel_cal_x, sensor_offsets->accel_cal_y, sensor_offsets->accel_cal_z); -} - -/** - * @brief Send a sensor_offsets message - * @param chan MAVLink channel to send the message - * - * @param mag_ofs_x magnetometer X offset - * @param mag_ofs_y magnetometer Y offset - * @param mag_ofs_z magnetometer Z offset - * @param mag_declination magnetic declination (radians) - * @param raw_press raw pressure from barometer - * @param raw_temp raw temperature from barometer - * @param gyro_cal_x gyro X calibration - * @param gyro_cal_y gyro Y calibration - * @param gyro_cal_z gyro Z calibration - * @param accel_cal_x accel X calibration - * @param accel_cal_y accel Y calibration - * @param accel_cal_z accel Z calibration - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_sensor_offsets_send(mavlink_channel_t chan, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN]; - _mav_put_float(buf, 0, mag_declination); - _mav_put_int32_t(buf, 4, raw_press); - _mav_put_int32_t(buf, 8, raw_temp); - _mav_put_float(buf, 12, gyro_cal_x); - _mav_put_float(buf, 16, gyro_cal_y); - _mav_put_float(buf, 20, gyro_cal_z); - _mav_put_float(buf, 24, accel_cal_x); - _mav_put_float(buf, 28, accel_cal_y); - _mav_put_float(buf, 32, accel_cal_z); - _mav_put_int16_t(buf, 36, mag_ofs_x); - _mav_put_int16_t(buf, 38, mag_ofs_y); - _mav_put_int16_t(buf, 40, mag_ofs_z); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); -#endif -#else - mavlink_sensor_offsets_t packet; - packet.mag_declination = mag_declination; - packet.raw_press = raw_press; - packet.raw_temp = raw_temp; - packet.gyro_cal_x = gyro_cal_x; - packet.gyro_cal_y = gyro_cal_y; - packet.gyro_cal_z = gyro_cal_z; - packet.accel_cal_x = accel_cal_x; - packet.accel_cal_y = accel_cal_y; - packet.accel_cal_z = accel_cal_z; - packet.mag_ofs_x = mag_ofs_x; - packet.mag_ofs_y = mag_ofs_y; - packet.mag_ofs_z = mag_ofs_z; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, (const char *)&packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, (const char *)&packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SENSOR_OFFSETS UNPACKING - - -/** - * @brief Get field mag_ofs_x from sensor_offsets message - * - * @return magnetometer X offset - */ -static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 36); -} - -/** - * @brief Get field mag_ofs_y from sensor_offsets message - * - * @return magnetometer Y offset - */ -static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 38); -} - -/** - * @brief Get field mag_ofs_z from sensor_offsets message - * - * @return magnetometer Z offset - */ -static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 40); -} - -/** - * @brief Get field mag_declination from sensor_offsets message - * - * @return magnetic declination (radians) - */ -static inline float mavlink_msg_sensor_offsets_get_mag_declination(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field raw_press from sensor_offsets message - * - * @return raw pressure from barometer - */ -static inline int32_t mavlink_msg_sensor_offsets_get_raw_press(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field raw_temp from sensor_offsets message - * - * @return raw temperature from barometer - */ -static inline int32_t mavlink_msg_sensor_offsets_get_raw_temp(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Get field gyro_cal_x from sensor_offsets message - * - * @return gyro X calibration - */ -static inline float mavlink_msg_sensor_offsets_get_gyro_cal_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field gyro_cal_y from sensor_offsets message - * - * @return gyro Y calibration - */ -static inline float mavlink_msg_sensor_offsets_get_gyro_cal_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field gyro_cal_z from sensor_offsets message - * - * @return gyro Z calibration - */ -static inline float mavlink_msg_sensor_offsets_get_gyro_cal_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field accel_cal_x from sensor_offsets message - * - * @return accel X calibration - */ -static inline float mavlink_msg_sensor_offsets_get_accel_cal_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field accel_cal_y from sensor_offsets message - * - * @return accel Y calibration - */ -static inline float mavlink_msg_sensor_offsets_get_accel_cal_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Get field accel_cal_z from sensor_offsets message - * - * @return accel Z calibration - */ -static inline float mavlink_msg_sensor_offsets_get_accel_cal_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 32); -} - -/** - * @brief Decode a sensor_offsets message into a struct - * - * @param msg The message to decode - * @param sensor_offsets C-struct to decode the message contents into - */ -static inline void mavlink_msg_sensor_offsets_decode(const mavlink_message_t* msg, mavlink_sensor_offsets_t* sensor_offsets) -{ -#if MAVLINK_NEED_BYTE_SWAP - sensor_offsets->mag_declination = mavlink_msg_sensor_offsets_get_mag_declination(msg); - sensor_offsets->raw_press = mavlink_msg_sensor_offsets_get_raw_press(msg); - sensor_offsets->raw_temp = mavlink_msg_sensor_offsets_get_raw_temp(msg); - sensor_offsets->gyro_cal_x = mavlink_msg_sensor_offsets_get_gyro_cal_x(msg); - sensor_offsets->gyro_cal_y = mavlink_msg_sensor_offsets_get_gyro_cal_y(msg); - sensor_offsets->gyro_cal_z = mavlink_msg_sensor_offsets_get_gyro_cal_z(msg); - sensor_offsets->accel_cal_x = mavlink_msg_sensor_offsets_get_accel_cal_x(msg); - sensor_offsets->accel_cal_y = mavlink_msg_sensor_offsets_get_accel_cal_y(msg); - sensor_offsets->accel_cal_z = mavlink_msg_sensor_offsets_get_accel_cal_z(msg); - sensor_offsets->mag_ofs_x = mavlink_msg_sensor_offsets_get_mag_ofs_x(msg); - sensor_offsets->mag_ofs_y = mavlink_msg_sensor_offsets_get_mag_ofs_y(msg); - sensor_offsets->mag_ofs_z = mavlink_msg_sensor_offsets_get_mag_ofs_z(msg); -#else - memcpy(sensor_offsets, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h deleted file mode 100644 index b2c629c..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h +++ /dev/null @@ -1,265 +0,0 @@ -// MESSAGE SET_MAG_OFFSETS PACKING - -#define MAVLINK_MSG_ID_SET_MAG_OFFSETS 151 - -typedef struct __mavlink_set_mag_offsets_t -{ - int16_t mag_ofs_x; ///< magnetometer X offset - int16_t mag_ofs_y; ///< magnetometer Y offset - int16_t mag_ofs_z; ///< magnetometer Z offset - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_set_mag_offsets_t; - -#define MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN 8 -#define MAVLINK_MSG_ID_151_LEN 8 - -#define MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC 219 -#define MAVLINK_MSG_ID_151_CRC 219 - - - -#define MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS { \ - "SET_MAG_OFFSETS", \ - 5, \ - { { "mag_ofs_x", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_set_mag_offsets_t, mag_ofs_x) }, \ - { "mag_ofs_y", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_set_mag_offsets_t, mag_ofs_y) }, \ - { "mag_ofs_z", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_set_mag_offsets_t, mag_ofs_z) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_set_mag_offsets_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_set_mag_offsets_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a set_mag_offsets message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param mag_ofs_x magnetometer X offset - * @param mag_ofs_y magnetometer Y offset - * @param mag_ofs_z magnetometer Z offset - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_mag_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN]; - _mav_put_int16_t(buf, 0, mag_ofs_x); - _mav_put_int16_t(buf, 2, mag_ofs_y); - _mav_put_int16_t(buf, 4, mag_ofs_z); - _mav_put_uint8_t(buf, 6, target_system); - _mav_put_uint8_t(buf, 7, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); -#else - mavlink_set_mag_offsets_t packet; - packet.mag_ofs_x = mag_ofs_x; - packet.mag_ofs_y = mag_ofs_y; - packet.mag_ofs_z = mag_ofs_z; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); -#endif -} - -/** - * @brief Pack a set_mag_offsets message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param mag_ofs_x magnetometer X offset - * @param mag_ofs_y magnetometer Y offset - * @param mag_ofs_z magnetometer Z offset - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_mag_offsets_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,int16_t mag_ofs_x,int16_t mag_ofs_y,int16_t mag_ofs_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN]; - _mav_put_int16_t(buf, 0, mag_ofs_x); - _mav_put_int16_t(buf, 2, mag_ofs_y); - _mav_put_int16_t(buf, 4, mag_ofs_z); - _mav_put_uint8_t(buf, 6, target_system); - _mav_put_uint8_t(buf, 7, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); -#else - mavlink_set_mag_offsets_t packet; - packet.mag_ofs_x = mag_ofs_x; - packet.mag_ofs_y = mag_ofs_y; - packet.mag_ofs_z = mag_ofs_z; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); -#endif -} - -/** - * @brief Encode a set_mag_offsets struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_mag_offsets C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_mag_offsets_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_mag_offsets_t* set_mag_offsets) -{ - return mavlink_msg_set_mag_offsets_pack(system_id, component_id, msg, set_mag_offsets->target_system, set_mag_offsets->target_component, set_mag_offsets->mag_ofs_x, set_mag_offsets->mag_ofs_y, set_mag_offsets->mag_ofs_z); -} - -/** - * @brief Encode a set_mag_offsets struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_mag_offsets C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_mag_offsets_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_mag_offsets_t* set_mag_offsets) -{ - return mavlink_msg_set_mag_offsets_pack_chan(system_id, component_id, chan, msg, set_mag_offsets->target_system, set_mag_offsets->target_component, set_mag_offsets->mag_ofs_x, set_mag_offsets->mag_ofs_y, set_mag_offsets->mag_ofs_z); -} - -/** - * @brief Send a set_mag_offsets message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param mag_ofs_x magnetometer X offset - * @param mag_ofs_y magnetometer Y offset - * @param mag_ofs_z magnetometer Z offset - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_mag_offsets_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN]; - _mav_put_int16_t(buf, 0, mag_ofs_x); - _mav_put_int16_t(buf, 2, mag_ofs_y); - _mav_put_int16_t(buf, 4, mag_ofs_z); - _mav_put_uint8_t(buf, 6, target_system); - _mav_put_uint8_t(buf, 7, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); -#endif -#else - mavlink_set_mag_offsets_t packet; - packet.mag_ofs_x = mag_ofs_x; - packet.mag_ofs_y = mag_ofs_y; - packet.mag_ofs_z = mag_ofs_z; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, (const char *)&packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, (const char *)&packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SET_MAG_OFFSETS UNPACKING - - -/** - * @brief Get field target_system from set_mag_offsets message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_set_mag_offsets_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @brief Get field target_component from set_mag_offsets message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_set_mag_offsets_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 7); -} - -/** - * @brief Get field mag_ofs_x from set_mag_offsets message - * - * @return magnetometer X offset - */ -static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 0); -} - -/** - * @brief Get field mag_ofs_y from set_mag_offsets message - * - * @return magnetometer Y offset - */ -static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 2); -} - -/** - * @brief Get field mag_ofs_z from set_mag_offsets message - * - * @return magnetometer Z offset - */ -static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 4); -} - -/** - * @brief Decode a set_mag_offsets message into a struct - * - * @param msg The message to decode - * @param set_mag_offsets C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_mag_offsets_decode(const mavlink_message_t* msg, mavlink_set_mag_offsets_t* set_mag_offsets) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_mag_offsets->mag_ofs_x = mavlink_msg_set_mag_offsets_get_mag_ofs_x(msg); - set_mag_offsets->mag_ofs_y = mavlink_msg_set_mag_offsets_get_mag_ofs_y(msg); - set_mag_offsets->mag_ofs_z = mavlink_msg_set_mag_offsets_get_mag_ofs_z(msg); - set_mag_offsets->target_system = mavlink_msg_set_mag_offsets_get_target_system(msg); - set_mag_offsets->target_component = mavlink_msg_set_mag_offsets_get_target_component(msg); -#else - memcpy(set_mag_offsets, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h deleted file mode 100644 index 8ef44f7..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h +++ /dev/null @@ -1,397 +0,0 @@ -// MESSAGE SIMSTATE PACKING - -#define MAVLINK_MSG_ID_SIMSTATE 164 - -typedef struct __mavlink_simstate_t -{ - float roll; ///< Roll angle (rad) - float pitch; ///< Pitch angle (rad) - float yaw; ///< Yaw angle (rad) - float xacc; ///< X acceleration m/s/s - float yacc; ///< Y acceleration m/s/s - float zacc; ///< Z acceleration m/s/s - float xgyro; ///< Angular speed around X axis rad/s - float ygyro; ///< Angular speed around Y axis rad/s - float zgyro; ///< Angular speed around Z axis rad/s - int32_t lat; ///< Latitude in degrees * 1E7 - int32_t lng; ///< Longitude in degrees * 1E7 -} mavlink_simstate_t; - -#define MAVLINK_MSG_ID_SIMSTATE_LEN 44 -#define MAVLINK_MSG_ID_164_LEN 44 - -#define MAVLINK_MSG_ID_SIMSTATE_CRC 154 -#define MAVLINK_MSG_ID_164_CRC 154 - - - -#define MAVLINK_MESSAGE_INFO_SIMSTATE { \ - "SIMSTATE", \ - 11, \ - { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_simstate_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_simstate_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_simstate_t, yaw) }, \ - { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_simstate_t, xacc) }, \ - { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_simstate_t, yacc) }, \ - { "zacc", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_simstate_t, zacc) }, \ - { "xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_simstate_t, xgyro) }, \ - { "ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_simstate_t, ygyro) }, \ - { "zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_simstate_t, zgyro) }, \ - { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_simstate_t, lat) }, \ - { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 40, offsetof(mavlink_simstate_t, lng) }, \ - } \ -} - - -/** - * @brief Pack a simstate message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param xacc X acceleration m/s/s - * @param yacc Y acceleration m/s/s - * @param zacc Z acceleration m/s/s - * @param xgyro Angular speed around X axis rad/s - * @param ygyro Angular speed around Y axis rad/s - * @param zgyro Angular speed around Z axis rad/s - * @param lat Latitude in degrees * 1E7 - * @param lng Longitude in degrees * 1E7 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_simstate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, int32_t lat, int32_t lng) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SIMSTATE_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, xacc); - _mav_put_float(buf, 16, yacc); - _mav_put_float(buf, 20, zacc); - _mav_put_float(buf, 24, xgyro); - _mav_put_float(buf, 28, ygyro); - _mav_put_float(buf, 32, zgyro); - _mav_put_int32_t(buf, 36, lat); - _mav_put_int32_t(buf, 40, lng); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIMSTATE_LEN); -#else - mavlink_simstate_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.lat = lat; - packet.lng = lng; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIMSTATE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SIMSTATE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SIMSTATE_LEN); -#endif -} - -/** - * @brief Pack a simstate message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param xacc X acceleration m/s/s - * @param yacc Y acceleration m/s/s - * @param zacc Z acceleration m/s/s - * @param xgyro Angular speed around X axis rad/s - * @param ygyro Angular speed around Y axis rad/s - * @param zgyro Angular speed around Z axis rad/s - * @param lat Latitude in degrees * 1E7 - * @param lng Longitude in degrees * 1E7 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_simstate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float roll,float pitch,float yaw,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,int32_t lat,int32_t lng) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SIMSTATE_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, xacc); - _mav_put_float(buf, 16, yacc); - _mav_put_float(buf, 20, zacc); - _mav_put_float(buf, 24, xgyro); - _mav_put_float(buf, 28, ygyro); - _mav_put_float(buf, 32, zgyro); - _mav_put_int32_t(buf, 36, lat); - _mav_put_int32_t(buf, 40, lng); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIMSTATE_LEN); -#else - mavlink_simstate_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.lat = lat; - packet.lng = lng; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIMSTATE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SIMSTATE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SIMSTATE_LEN); -#endif -} - -/** - * @brief Encode a simstate struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param simstate C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_simstate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_simstate_t* simstate) -{ - return mavlink_msg_simstate_pack(system_id, component_id, msg, simstate->roll, simstate->pitch, simstate->yaw, simstate->xacc, simstate->yacc, simstate->zacc, simstate->xgyro, simstate->ygyro, simstate->zgyro, simstate->lat, simstate->lng); -} - -/** - * @brief Encode a simstate struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param simstate C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_simstate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_simstate_t* simstate) -{ - return mavlink_msg_simstate_pack_chan(system_id, component_id, chan, msg, simstate->roll, simstate->pitch, simstate->yaw, simstate->xacc, simstate->yacc, simstate->zacc, simstate->xgyro, simstate->ygyro, simstate->zgyro, simstate->lat, simstate->lng); -} - -/** - * @brief Send a simstate message - * @param chan MAVLink channel to send the message - * - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param xacc X acceleration m/s/s - * @param yacc Y acceleration m/s/s - * @param zacc Z acceleration m/s/s - * @param xgyro Angular speed around X axis rad/s - * @param ygyro Angular speed around Y axis rad/s - * @param zgyro Angular speed around Z axis rad/s - * @param lat Latitude in degrees * 1E7 - * @param lng Longitude in degrees * 1E7 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_simstate_send(mavlink_channel_t chan, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, int32_t lat, int32_t lng) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SIMSTATE_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, xacc); - _mav_put_float(buf, 16, yacc); - _mav_put_float(buf, 20, zacc); - _mav_put_float(buf, 24, xgyro); - _mav_put_float(buf, 28, ygyro); - _mav_put_float(buf, 32, zgyro); - _mav_put_int32_t(buf, 36, lat); - _mav_put_int32_t(buf, 40, lng); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, MAVLINK_MSG_ID_SIMSTATE_LEN); -#endif -#else - mavlink_simstate_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.lat = lat; - packet.lng = lng; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, (const char *)&packet, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, (const char *)&packet, MAVLINK_MSG_ID_SIMSTATE_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SIMSTATE UNPACKING - - -/** - * @brief Get field roll from simstate message - * - * @return Roll angle (rad) - */ -static inline float mavlink_msg_simstate_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field pitch from simstate message - * - * @return Pitch angle (rad) - */ -static inline float mavlink_msg_simstate_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field yaw from simstate message - * - * @return Yaw angle (rad) - */ -static inline float mavlink_msg_simstate_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field xacc from simstate message - * - * @return X acceleration m/s/s - */ -static inline float mavlink_msg_simstate_get_xacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field yacc from simstate message - * - * @return Y acceleration m/s/s - */ -static inline float mavlink_msg_simstate_get_yacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field zacc from simstate message - * - * @return Z acceleration m/s/s - */ -static inline float mavlink_msg_simstate_get_zacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field xgyro from simstate message - * - * @return Angular speed around X axis rad/s - */ -static inline float mavlink_msg_simstate_get_xgyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field ygyro from simstate message - * - * @return Angular speed around Y axis rad/s - */ -static inline float mavlink_msg_simstate_get_ygyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Get field zgyro from simstate message - * - * @return Angular speed around Z axis rad/s - */ -static inline float mavlink_msg_simstate_get_zgyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 32); -} - -/** - * @brief Get field lat from simstate message - * - * @return Latitude in degrees * 1E7 - */ -static inline int32_t mavlink_msg_simstate_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 36); -} - -/** - * @brief Get field lng from simstate message - * - * @return Longitude in degrees * 1E7 - */ -static inline int32_t mavlink_msg_simstate_get_lng(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 40); -} - -/** - * @brief Decode a simstate message into a struct - * - * @param msg The message to decode - * @param simstate C-struct to decode the message contents into - */ -static inline void mavlink_msg_simstate_decode(const mavlink_message_t* msg, mavlink_simstate_t* simstate) -{ -#if MAVLINK_NEED_BYTE_SWAP - simstate->roll = mavlink_msg_simstate_get_roll(msg); - simstate->pitch = mavlink_msg_simstate_get_pitch(msg); - simstate->yaw = mavlink_msg_simstate_get_yaw(msg); - simstate->xacc = mavlink_msg_simstate_get_xacc(msg); - simstate->yacc = mavlink_msg_simstate_get_yacc(msg); - simstate->zacc = mavlink_msg_simstate_get_zacc(msg); - simstate->xgyro = mavlink_msg_simstate_get_xgyro(msg); - simstate->ygyro = mavlink_msg_simstate_get_ygyro(msg); - simstate->zgyro = mavlink_msg_simstate_get_zgyro(msg); - simstate->lat = mavlink_msg_simstate_get_lat(msg); - simstate->lng = mavlink_msg_simstate_get_lng(msg); -#else - memcpy(simstate, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SIMSTATE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h deleted file mode 100644 index 7608a7b..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h +++ /dev/null @@ -1,221 +0,0 @@ -// MESSAGE WIND PACKING - -#define MAVLINK_MSG_ID_WIND 168 - -typedef struct __mavlink_wind_t -{ - float direction; ///< wind direction that wind is coming from (degrees) - float speed; ///< wind speed in ground plane (m/s) - float speed_z; ///< vertical wind speed (m/s) -} mavlink_wind_t; - -#define MAVLINK_MSG_ID_WIND_LEN 12 -#define MAVLINK_MSG_ID_168_LEN 12 - -#define MAVLINK_MSG_ID_WIND_CRC 1 -#define MAVLINK_MSG_ID_168_CRC 1 - - - -#define MAVLINK_MESSAGE_INFO_WIND { \ - "WIND", \ - 3, \ - { { "direction", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_wind_t, direction) }, \ - { "speed", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_wind_t, speed) }, \ - { "speed_z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_wind_t, speed_z) }, \ - } \ -} - - -/** - * @brief Pack a wind message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param direction wind direction that wind is coming from (degrees) - * @param speed wind speed in ground plane (m/s) - * @param speed_z vertical wind speed (m/s) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_wind_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float direction, float speed, float speed_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WIND_LEN]; - _mav_put_float(buf, 0, direction); - _mav_put_float(buf, 4, speed); - _mav_put_float(buf, 8, speed_z); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WIND_LEN); -#else - mavlink_wind_t packet; - packet.direction = direction; - packet.speed = speed; - packet.speed_z = speed_z; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WIND_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_WIND; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WIND_LEN); -#endif -} - -/** - * @brief Pack a wind message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param direction wind direction that wind is coming from (degrees) - * @param speed wind speed in ground plane (m/s) - * @param speed_z vertical wind speed (m/s) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_wind_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float direction,float speed,float speed_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WIND_LEN]; - _mav_put_float(buf, 0, direction); - _mav_put_float(buf, 4, speed); - _mav_put_float(buf, 8, speed_z); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WIND_LEN); -#else - mavlink_wind_t packet; - packet.direction = direction; - packet.speed = speed; - packet.speed_z = speed_z; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WIND_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_WIND; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WIND_LEN); -#endif -} - -/** - * @brief Encode a wind struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param wind C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_wind_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_wind_t* wind) -{ - return mavlink_msg_wind_pack(system_id, component_id, msg, wind->direction, wind->speed, wind->speed_z); -} - -/** - * @brief Encode a wind struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param wind C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_wind_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_wind_t* wind) -{ - return mavlink_msg_wind_pack_chan(system_id, component_id, chan, msg, wind->direction, wind->speed, wind->speed_z); -} - -/** - * @brief Send a wind message - * @param chan MAVLink channel to send the message - * - * @param direction wind direction that wind is coming from (degrees) - * @param speed wind speed in ground plane (m/s) - * @param speed_z vertical wind speed (m/s) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_wind_send(mavlink_channel_t chan, float direction, float speed, float speed_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WIND_LEN]; - _mav_put_float(buf, 0, direction); - _mav_put_float(buf, 4, speed); - _mav_put_float(buf, 8, speed_z); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, buf, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, buf, MAVLINK_MSG_ID_WIND_LEN); -#endif -#else - mavlink_wind_t packet; - packet.direction = direction; - packet.speed = speed; - packet.speed_z = speed_z; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, (const char *)&packet, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, (const char *)&packet, MAVLINK_MSG_ID_WIND_LEN); -#endif -#endif -} - -#endif - -// MESSAGE WIND UNPACKING - - -/** - * @brief Get field direction from wind message - * - * @return wind direction that wind is coming from (degrees) - */ -static inline float mavlink_msg_wind_get_direction(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field speed from wind message - * - * @return wind speed in ground plane (m/s) - */ -static inline float mavlink_msg_wind_get_speed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field speed_z from wind message - * - * @return vertical wind speed (m/s) - */ -static inline float mavlink_msg_wind_get_speed_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Decode a wind message into a struct - * - * @param msg The message to decode - * @param wind C-struct to decode the message contents into - */ -static inline void mavlink_msg_wind_decode(const mavlink_message_t* msg, mavlink_wind_t* wind) -{ -#if MAVLINK_NEED_BYTE_SWAP - wind->direction = mavlink_msg_wind_get_direction(msg); - wind->speed = mavlink_msg_wind_get_speed(msg); - wind->speed_z = mavlink_msg_wind_get_speed_z(msg); -#else - memcpy(wind, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_WIND_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h deleted file mode 100644 index b249633..0000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h +++ /dev/null @@ -1,1488 +0,0 @@ -/** @file - * @brief MAVLink comm protocol testsuite generated from ardupilotmega.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef ARDUPILOTMEGA_TESTSUITE_H -#define ARDUPILOTMEGA_TESTSUITE_H - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef MAVLINK_TEST_ALL -#define MAVLINK_TEST_ALL -static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); -static void mavlink_test_ardupilotmega(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_common(system_id, component_id, last_msg); - mavlink_test_ardupilotmega(system_id, component_id, last_msg); -} -#endif - -#include "../common/testsuite.h" - - -static void mavlink_test_sensor_offsets(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_sensor_offsets_t packet_in = { - 17.0, - }963497672, - }963497880, - }101.0, - }129.0, - }157.0, - }185.0, - }213.0, - }241.0, - }19107, - }19211, - }19315, - }; - mavlink_sensor_offsets_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.mag_declination = packet_in.mag_declination; - packet1.raw_press = packet_in.raw_press; - packet1.raw_temp = packet_in.raw_temp; - packet1.gyro_cal_x = packet_in.gyro_cal_x; - packet1.gyro_cal_y = packet_in.gyro_cal_y; - packet1.gyro_cal_z = packet_in.gyro_cal_z; - packet1.accel_cal_x = packet_in.accel_cal_x; - packet1.accel_cal_y = packet_in.accel_cal_y; - packet1.accel_cal_z = packet_in.accel_cal_z; - packet1.mag_ofs_x = packet_in.mag_ofs_x; - packet1.mag_ofs_y = packet_in.mag_ofs_y; - packet1.mag_ofs_z = packet_in.mag_ofs_z; - - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_sensor_offsets_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_sensor_offsets_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_sensor_offsets_pack(system_id, component_id, &msg , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z , packet1.mag_declination , packet1.raw_press , packet1.raw_temp , packet1.gyro_cal_x , packet1.gyro_cal_y , packet1.gyro_cal_z , packet1.accel_cal_x , packet1.accel_cal_y , packet1.accel_cal_z ); - mavlink_msg_sensor_offsets_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_sensor_offsets_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z , packet1.mag_declination , packet1.raw_press , packet1.raw_temp , packet1.gyro_cal_x , packet1.gyro_cal_y , packet1.gyro_cal_z , packet1.accel_cal_x , packet1.accel_cal_y , packet1.accel_cal_z ); - mavlink_msg_sensor_offsets_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 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ - MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ - MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ - MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ - MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ - MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ - MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ - MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - MAV_CMD_ENUM_END=501, /* | */ -}; -#endif - -#include "../common/common.h" - -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 3 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 3 -#endif - -// MESSAGE DEFINITIONS -#include "./mavlink_msg_aq_telemetry_f.h" - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // AUTOQUAD_H diff --git a/mavlink/include/mavlink/v1.0/autoquad/mavlink.h b/mavlink/include/mavlink/v1.0/autoquad/mavlink.h deleted file mode 100644 index 3f80c9a..0000000 --- a/mavlink/include/mavlink/v1.0/autoquad/mavlink.h +++ /dev/null @@ -1,27 +0,0 @@ -/** @file - * @brief MAVLink comm protocol built from autoquad.xml - * @see http://pixhawk.ethz.ch/software/mavlink - */ -#ifndef MAVLINK_H -#define MAVLINK_H - -#ifndef MAVLINK_STX -#define MAVLINK_STX 254 -#endif - -#ifndef MAVLINK_ENDIAN -#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN -#endif - -#ifndef MAVLINK_ALIGNED_FIELDS -#define MAVLINK_ALIGNED_FIELDS 1 -#endif - -#ifndef MAVLINK_CRC_EXTRA -#define MAVLINK_CRC_EXTRA 1 -#endif - -#include "version.h" -#include "autoquad.h" - -#endif // MAVLINK_H diff --git a/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h b/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h deleted file mode 100644 index ed7c86b..0000000 --- a/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h +++ /dev/null @@ -1,617 +0,0 @@ -// MESSAGE AQ_TELEMETRY_F PACKING - -#define MAVLINK_MSG_ID_AQ_TELEMETRY_F 150 - -typedef struct __mavlink_aq_telemetry_f_t -{ - float value1; ///< value1 - float value2; ///< value2 - float value3; ///< value3 - float value4; ///< value4 - float value5; ///< value5 - float value6; ///< value6 - float value7; ///< value7 - float value8; ///< value8 - float value9; ///< value9 - float value10; ///< value10 - float value11; ///< value11 - float value12; ///< value12 - float value13; ///< value13 - float value14; ///< value14 - float value15; ///< value15 - float value16; ///< value16 - float value17; ///< value17 - float value18; ///< value18 - float value19; ///< value19 - float value20; ///< value20 - uint16_t Index; ///< Index of message -} mavlink_aq_telemetry_f_t; - -#define MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN 82 -#define MAVLINK_MSG_ID_150_LEN 82 - -#define MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC 241 -#define MAVLINK_MSG_ID_150_CRC 241 - - - -#define MAVLINK_MESSAGE_INFO_AQ_TELEMETRY_F { \ - "AQ_TELEMETRY_F", \ - 21, \ - { { "value1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_aq_telemetry_f_t, value1) }, \ - { "value2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_aq_telemetry_f_t, value2) }, \ - { "value3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_aq_telemetry_f_t, value3) }, \ - { "value4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_aq_telemetry_f_t, value4) }, \ - { "value5", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_aq_telemetry_f_t, value5) }, \ - { "value6", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_aq_telemetry_f_t, value6) }, \ - { "value7", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_aq_telemetry_f_t, value7) }, \ - { "value8", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_aq_telemetry_f_t, value8) }, \ - { "value9", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_aq_telemetry_f_t, value9) }, \ - { "value10", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_aq_telemetry_f_t, value10) }, \ - { "value11", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_aq_telemetry_f_t, value11) }, \ - { "value12", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_aq_telemetry_f_t, value12) }, \ - { "value13", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_aq_telemetry_f_t, value13) }, \ - { "value14", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_aq_telemetry_f_t, value14) }, \ - { "value15", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_aq_telemetry_f_t, value15) }, \ - { "value16", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_aq_telemetry_f_t, value16) }, \ - { "value17", NULL, MAVLINK_TYPE_FLOAT, 0, 64, offsetof(mavlink_aq_telemetry_f_t, value17) }, \ - { "value18", NULL, MAVLINK_TYPE_FLOAT, 0, 68, offsetof(mavlink_aq_telemetry_f_t, value18) }, \ - { "value19", NULL, MAVLINK_TYPE_FLOAT, 0, 72, offsetof(mavlink_aq_telemetry_f_t, value19) }, \ - { "value20", NULL, MAVLINK_TYPE_FLOAT, 0, 76, offsetof(mavlink_aq_telemetry_f_t, value20) }, \ - { "Index", NULL, MAVLINK_TYPE_UINT16_T, 0, 80, offsetof(mavlink_aq_telemetry_f_t, Index) }, \ - } \ -} - - -/** - * @brief Pack a aq_telemetry_f message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param Index Index of message - * @param value1 value1 - * @param value2 value2 - * @param value3 value3 - * @param value4 value4 - * @param value5 value5 - * @param value6 value6 - * @param value7 value7 - * @param value8 value8 - * @param value9 value9 - * @param value10 value10 - * @param value11 value11 - * @param value12 value12 - * @param value13 value13 - * @param value14 value14 - * @param value15 value15 - * @param value16 value16 - * @param value17 value17 - * @param value18 value18 - * @param value19 value19 - * @param value20 value20 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_aq_telemetry_f_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t Index, float value1, float value2, float value3, float value4, float value5, float value6, float value7, float value8, float value9, float value10, float value11, float value12, float value13, float value14, float value15, float value16, float value17, float value18, float value19, float value20) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN]; - _mav_put_float(buf, 0, value1); - _mav_put_float(buf, 4, value2); - _mav_put_float(buf, 8, value3); - _mav_put_float(buf, 12, value4); - _mav_put_float(buf, 16, value5); - _mav_put_float(buf, 20, value6); - _mav_put_float(buf, 24, value7); - _mav_put_float(buf, 28, value8); - _mav_put_float(buf, 32, value9); - _mav_put_float(buf, 36, value10); - _mav_put_float(buf, 40, value11); - _mav_put_float(buf, 44, value12); - _mav_put_float(buf, 48, value13); - _mav_put_float(buf, 52, value14); - _mav_put_float(buf, 56, value15); - _mav_put_float(buf, 60, value16); - _mav_put_float(buf, 64, value17); - _mav_put_float(buf, 68, value18); - _mav_put_float(buf, 72, value19); - _mav_put_float(buf, 76, value20); - _mav_put_uint16_t(buf, 80, Index); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#else - mavlink_aq_telemetry_f_t packet; - packet.value1 = value1; - packet.value2 = value2; - packet.value3 = value3; - packet.value4 = value4; - packet.value5 = value5; - packet.value6 = value6; - packet.value7 = value7; - packet.value8 = value8; - packet.value9 = value9; - packet.value10 = value10; - packet.value11 = value11; - packet.value12 = value12; - packet.value13 = value13; - packet.value14 = value14; - packet.value15 = value15; - packet.value16 = value16; - packet.value17 = value17; - packet.value18 = value18; - packet.value19 = value19; - packet.value20 = value20; - packet.Index = Index; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AQ_TELEMETRY_F; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif -} - -/** - * @brief Pack a aq_telemetry_f message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param Index Index of message - * @param value1 value1 - * @param value2 value2 - * @param value3 value3 - * @param value4 value4 - * @param value5 value5 - * @param value6 value6 - * @param value7 value7 - * @param value8 value8 - * @param value9 value9 - * @param value10 value10 - * @param value11 value11 - * @param value12 value12 - * @param value13 value13 - * @param value14 value14 - * @param value15 value15 - * @param value16 value16 - * @param value17 value17 - * @param value18 value18 - * @param value19 value19 - * @param value20 value20 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_aq_telemetry_f_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t Index,float value1,float value2,float value3,float value4,float value5,float value6,float value7,float value8,float value9,float value10,float value11,float value12,float value13,float value14,float value15,float value16,float value17,float value18,float value19,float value20) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN]; - _mav_put_float(buf, 0, value1); - _mav_put_float(buf, 4, value2); - _mav_put_float(buf, 8, value3); - _mav_put_float(buf, 12, value4); - _mav_put_float(buf, 16, value5); - _mav_put_float(buf, 20, value6); - _mav_put_float(buf, 24, value7); - _mav_put_float(buf, 28, value8); - _mav_put_float(buf, 32, value9); - _mav_put_float(buf, 36, value10); - _mav_put_float(buf, 40, value11); - _mav_put_float(buf, 44, value12); - _mav_put_float(buf, 48, value13); - _mav_put_float(buf, 52, value14); - _mav_put_float(buf, 56, value15); - _mav_put_float(buf, 60, value16); - _mav_put_float(buf, 64, value17); - _mav_put_float(buf, 68, value18); - _mav_put_float(buf, 72, value19); - _mav_put_float(buf, 76, value20); - _mav_put_uint16_t(buf, 80, Index); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#else - mavlink_aq_telemetry_f_t packet; - packet.value1 = value1; - packet.value2 = value2; - packet.value3 = value3; - packet.value4 = value4; - packet.value5 = value5; - packet.value6 = value6; - packet.value7 = value7; - packet.value8 = value8; - packet.value9 = value9; - packet.value10 = value10; - packet.value11 = value11; - packet.value12 = value12; - packet.value13 = value13; - packet.value14 = value14; - packet.value15 = value15; - packet.value16 = value16; - packet.value17 = value17; - packet.value18 = value18; - packet.value19 = value19; - packet.value20 = value20; - packet.Index = Index; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AQ_TELEMETRY_F; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif -} - -/** - * @brief Encode a aq_telemetry_f struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param aq_telemetry_f C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_aq_telemetry_f_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_aq_telemetry_f_t* aq_telemetry_f) -{ - return mavlink_msg_aq_telemetry_f_pack(system_id, component_id, msg, aq_telemetry_f->Index, aq_telemetry_f->value1, aq_telemetry_f->value2, aq_telemetry_f->value3, aq_telemetry_f->value4, aq_telemetry_f->value5, aq_telemetry_f->value6, aq_telemetry_f->value7, aq_telemetry_f->value8, aq_telemetry_f->value9, aq_telemetry_f->value10, aq_telemetry_f->value11, aq_telemetry_f->value12, aq_telemetry_f->value13, aq_telemetry_f->value14, aq_telemetry_f->value15, aq_telemetry_f->value16, aq_telemetry_f->value17, aq_telemetry_f->value18, aq_telemetry_f->value19, aq_telemetry_f->value20); -} - -/** - * @brief Encode a aq_telemetry_f struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param aq_telemetry_f C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_aq_telemetry_f_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_aq_telemetry_f_t* aq_telemetry_f) -{ - return mavlink_msg_aq_telemetry_f_pack_chan(system_id, component_id, chan, msg, aq_telemetry_f->Index, aq_telemetry_f->value1, aq_telemetry_f->value2, aq_telemetry_f->value3, aq_telemetry_f->value4, aq_telemetry_f->value5, aq_telemetry_f->value6, aq_telemetry_f->value7, aq_telemetry_f->value8, aq_telemetry_f->value9, aq_telemetry_f->value10, aq_telemetry_f->value11, aq_telemetry_f->value12, aq_telemetry_f->value13, aq_telemetry_f->value14, aq_telemetry_f->value15, aq_telemetry_f->value16, aq_telemetry_f->value17, aq_telemetry_f->value18, aq_telemetry_f->value19, aq_telemetry_f->value20); -} - -/** - * @brief Send a aq_telemetry_f message - * @param chan MAVLink channel to send the message - * - * @param Index Index of message - * @param value1 value1 - * @param value2 value2 - * @param value3 value3 - * @param value4 value4 - * @param value5 value5 - * @param value6 value6 - * @param value7 value7 - * @param value8 value8 - * @param value9 value9 - * @param value10 value10 - * @param value11 value11 - * @param value12 value12 - * @param value13 value13 - * @param value14 value14 - * @param value15 value15 - * @param value16 value16 - * @param value17 value17 - * @param value18 value18 - * @param value19 value19 - * @param value20 value20 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_aq_telemetry_f_send(mavlink_channel_t chan, uint16_t Index, float value1, float value2, float value3, float value4, float value5, float value6, float value7, float value8, float value9, float value10, float value11, float value12, float value13, float value14, float value15, float value16, float value17, float value18, float value19, float value20) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN]; - _mav_put_float(buf, 0, value1); - _mav_put_float(buf, 4, value2); - _mav_put_float(buf, 8, value3); - _mav_put_float(buf, 12, value4); - _mav_put_float(buf, 16, value5); - _mav_put_float(buf, 20, value6); - _mav_put_float(buf, 24, value7); - _mav_put_float(buf, 28, value8); - _mav_put_float(buf, 32, value9); - _mav_put_float(buf, 36, value10); - _mav_put_float(buf, 40, value11); - _mav_put_float(buf, 44, value12); - _mav_put_float(buf, 48, value13); - _mav_put_float(buf, 52, value14); - _mav_put_float(buf, 56, value15); - _mav_put_float(buf, 60, value16); - _mav_put_float(buf, 64, value17); - _mav_put_float(buf, 68, value18); - _mav_put_float(buf, 72, value19); - _mav_put_float(buf, 76, value20); - _mav_put_uint16_t(buf, 80, Index); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif -#else - mavlink_aq_telemetry_f_t packet; - packet.value1 = value1; - packet.value2 = value2; - packet.value3 = value3; - packet.value4 = value4; - packet.value5 = value5; - packet.value6 = value6; - packet.value7 = value7; - packet.value8 = value8; - packet.value9 = value9; - packet.value10 = value10; - packet.value11 = value11; - packet.value12 = value12; - packet.value13 = value13; - packet.value14 = value14; - packet.value15 = value15; - packet.value16 = value16; - packet.value17 = value17; - packet.value18 = value18; - packet.value19 = value19; - packet.value20 = value20; - packet.Index = Index; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, (const char *)&packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, (const char *)&packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif -#endif -} - -#endif - -// MESSAGE AQ_TELEMETRY_F UNPACKING - - -/** - * @brief Get field Index from aq_telemetry_f message - * - * @return Index of message - */ -static inline uint16_t mavlink_msg_aq_telemetry_f_get_Index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 80); -} - -/** - * @brief Get field value1 from aq_telemetry_f message - * - * @return value1 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field value2 from aq_telemetry_f message - * - * @return value2 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field value3 from aq_telemetry_f message - * - * @return value3 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field value4 from aq_telemetry_f message - * - * @return value4 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field value5 from aq_telemetry_f message - * - * @return value5 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field value6 from aq_telemetry_f message - * - * @return value6 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field value7 from aq_telemetry_f message - * - * @return value7 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value7(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field value8 from aq_telemetry_f message - * - * @return value8 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value8(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Get field value9 from aq_telemetry_f message - * - * @return value9 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value9(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 32); -} - -/** - * @brief Get field value10 from aq_telemetry_f message - * - * @return value10 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value10(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 36); -} - -/** - * @brief Get field value11 from aq_telemetry_f message - * - * @return value11 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value11(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 40); -} - -/** - * @brief Get field value12 from aq_telemetry_f message - * - * @return value12 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value12(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 44); -} - -/** - * @brief Get field value13 from aq_telemetry_f message - * - * @return value13 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value13(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 48); -} - -/** - * @brief Get field value14 from aq_telemetry_f message - * - * @return value14 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value14(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 52); -} - -/** - * @brief Get field value15 from aq_telemetry_f message - * - * @return value15 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value15(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 56); -} - -/** - * @brief Get field value16 from aq_telemetry_f message - * - * @return value16 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value16(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 60); -} - -/** - * @brief Get field value17 from aq_telemetry_f message - * - * @return value17 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value17(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 64); -} - -/** - * @brief Get field value18 from aq_telemetry_f message - * - * @return value18 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value18(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 68); -} - -/** - * @brief Get field value19 from aq_telemetry_f message - * - * @return value19 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value19(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 72); -} - -/** - * @brief Get field value20 from aq_telemetry_f message - * - * @return value20 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value20(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 76); -} - -/** - * @brief Decode a aq_telemetry_f message into a struct - * - * @param msg The message to decode - * @param aq_telemetry_f C-struct to decode the message contents into - */ -static inline void mavlink_msg_aq_telemetry_f_decode(const mavlink_message_t* msg, mavlink_aq_telemetry_f_t* aq_telemetry_f) -{ -#if MAVLINK_NEED_BYTE_SWAP - aq_telemetry_f->value1 = mavlink_msg_aq_telemetry_f_get_value1(msg); - aq_telemetry_f->value2 = mavlink_msg_aq_telemetry_f_get_value2(msg); - aq_telemetry_f->value3 = mavlink_msg_aq_telemetry_f_get_value3(msg); - aq_telemetry_f->value4 = mavlink_msg_aq_telemetry_f_get_value4(msg); - aq_telemetry_f->value5 = mavlink_msg_aq_telemetry_f_get_value5(msg); - aq_telemetry_f->value6 = mavlink_msg_aq_telemetry_f_get_value6(msg); - aq_telemetry_f->value7 = mavlink_msg_aq_telemetry_f_get_value7(msg); - aq_telemetry_f->value8 = mavlink_msg_aq_telemetry_f_get_value8(msg); - aq_telemetry_f->value9 = mavlink_msg_aq_telemetry_f_get_value9(msg); - aq_telemetry_f->value10 = mavlink_msg_aq_telemetry_f_get_value10(msg); - aq_telemetry_f->value11 = mavlink_msg_aq_telemetry_f_get_value11(msg); - aq_telemetry_f->value12 = mavlink_msg_aq_telemetry_f_get_value12(msg); - aq_telemetry_f->value13 = mavlink_msg_aq_telemetry_f_get_value13(msg); - aq_telemetry_f->value14 = mavlink_msg_aq_telemetry_f_get_value14(msg); - aq_telemetry_f->value15 = mavlink_msg_aq_telemetry_f_get_value15(msg); - aq_telemetry_f->value16 = mavlink_msg_aq_telemetry_f_get_value16(msg); - aq_telemetry_f->value17 = mavlink_msg_aq_telemetry_f_get_value17(msg); - aq_telemetry_f->value18 = mavlink_msg_aq_telemetry_f_get_value18(msg); - aq_telemetry_f->value19 = mavlink_msg_aq_telemetry_f_get_value19(msg); - aq_telemetry_f->value20 = mavlink_msg_aq_telemetry_f_get_value20(msg); - aq_telemetry_f->Index = mavlink_msg_aq_telemetry_f_get_Index(msg); -#else - memcpy(aq_telemetry_f, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/autoquad/testsuite.h b/mavlink/include/mavlink/v1.0/autoquad/testsuite.h deleted file mode 100644 index 4eafe7b..0000000 --- a/mavlink/include/mavlink/v1.0/autoquad/testsuite.h +++ /dev/null @@ -1,118 +0,0 @@ -/** @file - * @brief MAVLink comm protocol testsuite generated from autoquad.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef AUTOQUAD_TESTSUITE_H -#define AUTOQUAD_TESTSUITE_H - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef MAVLINK_TEST_ALL -#define MAVLINK_TEST_ALL -static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); -static void mavlink_test_autoquad(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_common(system_id, component_id, last_msg); - mavlink_test_autoquad(system_id, component_id, last_msg); -} -#endif - -#include "../common/testsuite.h" - - -static void mavlink_test_aq_telemetry_f(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_aq_telemetry_f_t packet_in = { - 17.0, - }45.0, - }73.0, - }101.0, - }129.0, - }157.0, - }185.0, - }213.0, - }241.0, - }269.0, - }297.0, - }325.0, - }353.0, - }381.0, - }409.0, - }437.0, - }465.0, - }493.0, - }521.0, - }549.0, - }21395, - }; - mavlink_aq_telemetry_f_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.value1 = packet_in.value1; - packet1.value2 = packet_in.value2; - packet1.value3 = packet_in.value3; - packet1.value4 = packet_in.value4; - packet1.value5 = packet_in.value5; - packet1.value6 = packet_in.value6; - packet1.value7 = packet_in.value7; - packet1.value8 = packet_in.value8; - packet1.value9 = packet_in.value9; - packet1.value10 = packet_in.value10; - packet1.value11 = packet_in.value11; - packet1.value12 = packet_in.value12; - packet1.value13 = packet_in.value13; - packet1.value14 = packet_in.value14; - packet1.value15 = packet_in.value15; - packet1.value16 = packet_in.value16; - packet1.value17 = packet_in.value17; - packet1.value18 = packet_in.value18; - packet1.value19 = packet_in.value19; - packet1.value20 = packet_in.value20; - packet1.Index = packet_in.Index; - - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_aq_telemetry_f_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_aq_telemetry_f_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_aq_telemetry_f_pack(system_id, component_id, &msg , packet1.Index , packet1.value1 , packet1.value2 , packet1.value3 , packet1.value4 , packet1.value5 , packet1.value6 , packet1.value7 , packet1.value8 , packet1.value9 , packet1.value10 , packet1.value11 , packet1.value12 , packet1.value13 , packet1.value14 , packet1.value15 , packet1.value16 , packet1.value17 , packet1.value18 , packet1.value19 , packet1.value20 ); - mavlink_msg_aq_telemetry_f_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_aq_telemetry_f_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.Index , packet1.value1 , packet1.value2 , packet1.value3 , packet1.value4 , packet1.value5 , packet1.value6 , packet1.value7 , packet1.value8 , packet1.value9 , packet1.value10 , packet1.value11 , packet1.value12 , packet1.value13 , packet1.value14 , packet1.value15 , packet1.value16 , packet1.value17 , packet1.value18 , packet1.value19 , packet1.value20 ); - mavlink_msg_aq_telemetry_f_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 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ @@ -275,13 +329,15 @@ enum MAV_CMD MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + MAV_CMD_NAV_SPLINE_WAYPOINT=82, /* Navigate to MISSION using a spline path. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Empty| Empty| Empty| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + MAV_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Custom mode - this is system specific, please refer to the individual autopilot specifications for details.| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ @@ -290,19 +346,41 @@ enum MAV_CMD MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ + MAV_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_LAND_START=189, /* Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0/0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty| */ + MAV_CMD_DO_RALLY_LAND=190, /* Mission command to perform a landing from a rally point. |Break altitude (meters)| Landing speed (m/s)| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_GO_AROUND=191, /* Mission command to safely abort an autonmous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ + MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ + MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| */ + MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_INVERTED_FLIGHT=210, /* Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)| q2 - quaternion param #2, x (0 in null-rotation)| q3 - quaternion param #3, y (0 in null-rotation)| q4 - quaternion param #4, z (0 in null-rotation)| Empty| Empty| Empty| */ + MAV_CMD_DO_GUIDED_MASTER=221, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_GUIDED_LIMITS=222, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, WGS84) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, WGS84) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ - MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ + MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty| */ + MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow, 5: second magnetometer| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - MAV_CMD_ENUM_END=501, /* | */ -}; + MAV_CMD_IMAGE_START_CAPTURE=2000, /* Start image capture sequence |Duration between two consecutive pictures (in seconds)| Number of images to capture total - 0 for unlimited capture| Resolution in megapixels (0.3 for 640x480, 1.3 for 1280x720, etc)| */ + MAV_CMD_IMAGE_STOP_CAPTURE=2001, /* Stop image capture sequence |Reserved| Reserved| */ + MAV_CMD_VIDEO_START_CAPTURE=2500, /* Starts video capture |Camera ID (0 for all cameras), 1 for first, 2 for second, etc.| Frames per second| Resolution in megapixels (0.3 for 640x480, 1.3 for 1280x720, etc)| */ + MAV_CMD_VIDEO_STOP_CAPTURE=2501, /* Stop the current video capture |Reserved| Reserved| */ + MAV_CMD_PANORAMA_CREATE=2800, /* Create a panorama at the current position |Viewing angle horizontal of the panorama (in degrees, +- 0.5 the total angle)| Viewing angle vertical of panorama (in degrees)| Speed of the horizontal rotation (in degrees per second)| Speed of the vertical rotation (in degrees per second)| */ + MAV_CMD_PAYLOAD_PREPARE_DEPLOY=30001, /* Deploy payload on a Lat / Lon / Alt position. This includes the navigation to reach the required release position and velocity. |Operation mode. 0: prepare single payload deploy (overwriting previous requests), but do not execute it. 1: execute payload deploy immediately (rejecting further deploy commands during execution, but allowing abort). 2: add payload deploy to existing deployment list.| Desired approach vector in degrees compass heading (0..360). A negative value indicates the system can define the approach vector at will.| Desired ground speed at release time. This can be overriden by the airframe in case it needs to meet minimum airspeed. A negative value indicates the system can define the ground speed at will.| Minimum altitude clearance to the release position in meters. A negative value indicates the system can define the clearance at will.| Latitude unscaled for MISSION_ITEM or in 1e7 degrees for MISSION_ITEM_INT| Longitude unscaled for MISSION_ITEM or in 1e7 degrees for MISSION_ITEM_INT| Altitude, in meters WGS84| */ + MAV_CMD_PAYLOAD_CONTROL_DEPLOY=30002, /* Control the payload deployment. |Operation mode. 0: Abort deployment, continue normal mission. 1: switch to payload deploment mode. 100: delete first payload deployment request. 101: delete all payload deployment requests.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ + MAV_CMD_ENUM_END=30003, /* | */ +} MAV_CMD; #endif /** @brief Data stream IDs. A data stream is not a fixed set of messages, but rather a @@ -310,7 +388,7 @@ enum MAV_CMD the recommended messages. */ #ifndef HAVE_ENUM_MAV_DATA_STREAM #define HAVE_ENUM_MAV_DATA_STREAM -enum MAV_DATA_STREAM +typedef enum MAV_DATA_STREAM { MAV_DATA_STREAM_ALL=0, /* Enable all data streams | */ MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. | */ @@ -322,7 +400,7 @@ enum MAV_DATA_STREAM MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot | */ MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot | */ MAV_DATA_STREAM_ENUM_END=13, /* | */ -}; +} MAV_DATA_STREAM; #endif /** @brief The ROI (region of interest) for the vehicle. This can be @@ -330,7 +408,7 @@ enum MAV_DATA_STREAM MAV_CMD_NAV_ROI). */ #ifndef HAVE_ENUM_MAV_ROI #define HAVE_ENUM_MAV_ROI -enum MAV_ROI +typedef enum MAV_ROI { MAV_ROI_NONE=0, /* No region of interest. | */ MAV_ROI_WPNEXT=1, /* Point toward next MISSION. | */ @@ -338,13 +416,13 @@ enum MAV_ROI MAV_ROI_LOCATION=3, /* Point toward fixed location. | */ MAV_ROI_TARGET=4, /* Point toward of given id. | */ MAV_ROI_ENUM_END=5, /* | */ -}; +} MAV_ROI; #endif /** @brief ACK / NACK / ERROR values as a result of MAV_CMDs and for mission item transmission. */ #ifndef HAVE_ENUM_MAV_CMD_ACK #define HAVE_ENUM_MAV_CMD_ACK -enum MAV_CMD_ACK +typedef enum MAV_CMD_ACK { MAV_CMD_ACK_OK=1, /* Command / mission item is ok. | */ MAV_CMD_ACK_ERR_FAIL=2, /* Generic error message if none of the other reasons fails or if no detailed error reporting is implemented. | */ @@ -356,13 +434,13 @@ enum MAV_CMD_ACK MAV_CMD_ACK_ERR_Y_LON_OUT_OF_RANGE=8, /* The Y or longitude value is out of range. | */ MAV_CMD_ACK_ERR_Z_ALT_OUT_OF_RANGE=9, /* The Z or altitude value is out of range. | */ MAV_CMD_ACK_ENUM_END=10, /* | */ -}; +} MAV_CMD_ACK; #endif /** @brief Specifies the datatype of a MAVLink parameter. */ #ifndef HAVE_ENUM_MAV_PARAM_TYPE #define HAVE_ENUM_MAV_PARAM_TYPE -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 | */ @@ -375,13 +453,13 @@ enum MAV_PARAM_TYPE 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 /** @brief result from a mavlink command */ #ifndef HAVE_ENUM_MAV_RESULT #define HAVE_ENUM_MAV_RESULT -enum MAV_RESULT +typedef enum MAV_RESULT { MAV_RESULT_ACCEPTED=0, /* Command ACCEPTED and EXECUTED | */ MAV_RESULT_TEMPORARILY_REJECTED=1, /* Command TEMPORARY REJECTED/DENIED | */ @@ -389,13 +467,13 @@ enum MAV_RESULT MAV_RESULT_UNSUPPORTED=3, /* Command UNKNOWN/UNSUPPORTED | */ MAV_RESULT_FAILED=4, /* Command executed, but failed | */ MAV_RESULT_ENUM_END=5, /* | */ -}; +} MAV_RESULT; #endif /** @brief result in a mavlink mission ack */ #ifndef HAVE_ENUM_MAV_MISSION_RESULT #define HAVE_ENUM_MAV_MISSION_RESULT -enum MAV_MISSION_RESULT +typedef enum MAV_MISSION_RESULT { MAV_MISSION_ACCEPTED=0, /* mission accepted OK | */ MAV_MISSION_ERROR=1, /* generic error / not accepting mission commands at all right now | */ @@ -413,13 +491,13 @@ enum MAV_MISSION_RESULT MAV_MISSION_INVALID_SEQUENCE=13, /* received waypoint out of sequence | */ MAV_MISSION_DENIED=14, /* not accepting any mission commands from this communication partner | */ MAV_MISSION_RESULT_ENUM_END=15, /* | */ -}; +} MAV_MISSION_RESULT; #endif /** @brief Indicates the severity level, generally used for status messages to indicate their relative urgency. Based on RFC-5424 using expanded definitions at: http://www.kiwisyslog.com/kb/info:-syslog-message-levels/. */ #ifndef HAVE_ENUM_MAV_SEVERITY #define HAVE_ENUM_MAV_SEVERITY -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. | */ @@ -430,7 +508,121 @@ enum MAV_SEVERITY 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 + +/** @brief Power supply status flags (bitmask) */ +#ifndef HAVE_ENUM_MAV_POWER_STATUS +#define HAVE_ENUM_MAV_POWER_STATUS +typedef enum MAV_POWER_STATUS +{ + MAV_POWER_STATUS_BRICK_VALID=1, /* main brick power supply valid | */ + MAV_POWER_STATUS_SERVO_VALID=2, /* main servo power supply valid for FMU | */ + MAV_POWER_STATUS_USB_CONNECTED=4, /* USB power is connected | */ + MAV_POWER_STATUS_PERIPH_OVERCURRENT=8, /* peripheral supply is in over-current state | */ + MAV_POWER_STATUS_PERIPH_HIPOWER_OVERCURRENT=16, /* hi-power peripheral supply is in over-current state | */ + MAV_POWER_STATUS_CHANGED=32, /* Power status has changed since boot | */ + MAV_POWER_STATUS_ENUM_END=33, /* | */ +} MAV_POWER_STATUS; +#endif + +/** @brief SERIAL_CONTROL device types */ +#ifndef HAVE_ENUM_SERIAL_CONTROL_DEV +#define HAVE_ENUM_SERIAL_CONTROL_DEV +typedef enum SERIAL_CONTROL_DEV +{ + SERIAL_CONTROL_DEV_TELEM1=0, /* First telemetry port | */ + SERIAL_CONTROL_DEV_TELEM2=1, /* Second telemetry port | */ + SERIAL_CONTROL_DEV_GPS1=2, /* First GPS port | */ + SERIAL_CONTROL_DEV_GPS2=3, /* Second GPS port | */ + SERIAL_CONTROL_DEV_ENUM_END=4, /* | */ +} SERIAL_CONTROL_DEV; +#endif + +/** @brief SERIAL_CONTROL flags (bitmask) */ +#ifndef HAVE_ENUM_SERIAL_CONTROL_FLAG +#define HAVE_ENUM_SERIAL_CONTROL_FLAG +typedef enum SERIAL_CONTROL_FLAG +{ + SERIAL_CONTROL_FLAG_REPLY=1, /* Set if this is a reply | */ + SERIAL_CONTROL_FLAG_RESPOND=2, /* Set if the sender wants the receiver to send a response as another SERIAL_CONTROL message | */ + SERIAL_CONTROL_FLAG_EXCLUSIVE=4, /* Set if access to the serial port should be removed from whatever driver is currently using it, giving exclusive access to the SERIAL_CONTROL protocol. The port can be handed back by sending a request without this flag set | */ + SERIAL_CONTROL_FLAG_BLOCKING=8, /* Block on writes to the serial port | */ + SERIAL_CONTROL_FLAG_MULTI=16, /* Send multiple replies until port is drained | */ + SERIAL_CONTROL_FLAG_ENUM_END=17, /* | */ +} SERIAL_CONTROL_FLAG; +#endif + +/** @brief Enumeration of distance sensor types */ +#ifndef HAVE_ENUM_MAV_DISTANCE_SENSOR +#define HAVE_ENUM_MAV_DISTANCE_SENSOR +typedef enum MAV_DISTANCE_SENSOR +{ + MAV_DISTANCE_SENSOR_LASER=0, /* Laser altimeter, e.g. LightWare SF02/F or PulsedLight units | */ + MAV_DISTANCE_SENSOR_ULTRASOUND=1, /* Ultrasound altimeter, e.g. MaxBotix units | */ + MAV_DISTANCE_SENSOR_ENUM_END=2, /* | */ +} MAV_DISTANCE_SENSOR; +#endif + +/** @brief Bitmask of (optional) autopilot capabilities (64 bit). If a bit is set, the autopilot supports this capability. */ +#ifndef HAVE_ENUM_MAV_PROTOCOL_CAPABILITY +#define HAVE_ENUM_MAV_PROTOCOL_CAPABILITY +typedef enum MAV_PROTOCOL_CAPABILITY +{ + MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT=1, /* Autopilot supports MISSION float message type. | */ + MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT=2, /* Autopilot supports the new param float message type. | */ + MAV_PROTOCOL_CAPABILITY_MISSION_INT=4, /* Autopilot supports MISSION_INT scaled integer message type. | */ + MAV_PROTOCOL_CAPABILITY_COMMAND_INT=8, /* Autopilot supports COMMAND_INT scaled integer message type. | */ + MAV_PROTOCOL_CAPABILITY_PARAM_UNION=16, /* Autopilot supports the new param union message type. | */ + MAV_PROTOCOL_CAPABILITY_FTP=32, /* Autopilot supports the new param union message type. | */ + MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET=64, /* Autopilot supports commanding attitude offboard. | */ + MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED=128, /* Autopilot supports commanding position and velocity targets in local NED frame. | */ + MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT=256, /* Autopilot supports commanding position and velocity targets in global scaled integers. | */ + MAV_PROTOCOL_CAPABILITY_TERRAIN=512, /* Autopilot supports terrain protocol / data handling. | */ + MAV_PROTOCOL_CAPABILITY_ENUM_END=513, /* | */ +} MAV_PROTOCOL_CAPABILITY; +#endif + +/** @brief Enumeration of estimator types */ +#ifndef HAVE_ENUM_MAV_ESTIMATOR_TYPE +#define HAVE_ENUM_MAV_ESTIMATOR_TYPE +typedef enum MAV_ESTIMATOR_TYPE +{ + MAV_ESTIMATOR_TYPE_NAIVE=1, /* This is a naive estimator without any real covariance feedback. | */ + MAV_ESTIMATOR_TYPE_VISION=2, /* Computer vision based estimate. Might be up to scale. | */ + MAV_ESTIMATOR_TYPE_VIO=3, /* Visual-inertial estimate. | */ + MAV_ESTIMATOR_TYPE_GPS=4, /* Plain GPS estimate. | */ + MAV_ESTIMATOR_TYPE_GPS_INS=5, /* Estimator integrating GPS and inertial sensing. | */ + MAV_ESTIMATOR_TYPE_ENUM_END=6, /* | */ +} MAV_ESTIMATOR_TYPE; +#endif + +/** @brief Enumeration of battery types */ +#ifndef HAVE_ENUM_MAV_BATTERY_TYPE +#define HAVE_ENUM_MAV_BATTERY_TYPE +typedef enum MAV_BATTERY_TYPE +{ + MAV_BATTERY_TYPE_UNKNOWN=0, /* Not specified. | */ + MAV_BATTERY_TYPE_LIPO=1, /* Lithium polymere battery | */ + MAV_BATTERY_TYPE_LIFE=2, /* Lithium ferrite battery | */ + MAV_BATTERY_TYPE_LION=3, /* Lithium-ION battery | */ + MAV_BATTERY_TYPE_NIMH=4, /* Nickel metal hydride battery | */ + MAV_BATTERY_TYPE_ENUM_END=5, /* | */ +} MAV_BATTERY_TYPE; +#endif + +/** @brief Enumeration of battery functions */ +#ifndef HAVE_ENUM_MAV_BATTERY_FUNCTION +#define HAVE_ENUM_MAV_BATTERY_FUNCTION +typedef enum MAV_BATTERY_FUNCTION +{ + MAV_BATTERY_FUNCTION_UNKNOWN=0, /* Lithium polymere battery | */ + MAV_BATTERY_FUNCTION_ALL=1, /* Battery supports all flight systems | */ + MAV_BATTERY_FUNCTION_PROPULSION=2, /* Battery for the propulsion system | */ + MAV_BATTERY_FUNCTION_AVIONICS=3, /* Avionics battery | */ + MAV_BATTERY_TYPE_PAYLOAD=4, /* Payload battery | */ + MAV_BATTERY_FUNCTION_ENUM_END=5, /* | */ +} MAV_BATTERY_FUNCTION; #endif @@ -485,30 +677,29 @@ enum MAV_SEVERITY #include "./mavlink_msg_mission_ack.h" #include "./mavlink_msg_set_gps_global_origin.h" #include "./mavlink_msg_gps_global_origin.h" -#include "./mavlink_msg_set_local_position_setpoint.h" -#include "./mavlink_msg_local_position_setpoint.h" -#include "./mavlink_msg_global_position_setpoint_int.h" -#include "./mavlink_msg_set_global_position_setpoint_int.h" #include "./mavlink_msg_safety_set_allowed_area.h" #include "./mavlink_msg_safety_allowed_area.h" -#include "./mavlink_msg_set_roll_pitch_yaw_thrust.h" -#include "./mavlink_msg_set_roll_pitch_yaw_speed_thrust.h" -#include "./mavlink_msg_roll_pitch_yaw_thrust_setpoint.h" -#include "./mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h" -#include "./mavlink_msg_set_quad_motors_setpoint.h" -#include "./mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h" +#include "./mavlink_msg_attitude_quaternion_cov.h" #include "./mavlink_msg_nav_controller_output.h" -#include "./mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h" -#include "./mavlink_msg_state_correction.h" +#include "./mavlink_msg_global_position_int_cov.h" +#include "./mavlink_msg_local_position_ned_cov.h" +#include "./mavlink_msg_rc_channels.h" #include "./mavlink_msg_request_data_stream.h" #include "./mavlink_msg_data_stream.h" #include "./mavlink_msg_manual_control.h" #include "./mavlink_msg_rc_channels_override.h" +#include "./mavlink_msg_mission_item_int.h" #include "./mavlink_msg_vfr_hud.h" +#include "./mavlink_msg_command_int.h" #include "./mavlink_msg_command_long.h" #include "./mavlink_msg_command_ack.h" -#include "./mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h" #include "./mavlink_msg_manual_setpoint.h" +#include "./mavlink_msg_set_attitude_target.h" +#include "./mavlink_msg_attitude_target.h" +#include "./mavlink_msg_set_position_target_local_ned.h" +#include "./mavlink_msg_position_target_local_ned.h" +#include "./mavlink_msg_set_position_target_global_int.h" +#include "./mavlink_msg_position_target_global_int.h" #include "./mavlink_msg_local_position_ned_system_global_offset.h" #include "./mavlink_msg_hil_state.h" #include "./mavlink_msg_hil_controls.h" @@ -519,13 +710,11 @@ enum MAV_SEVERITY #include "./mavlink_msg_vision_speed_estimate.h" #include "./mavlink_msg_vicon_position_estimate.h" #include "./mavlink_msg_highres_imu.h" -#include "./mavlink_msg_omnidirectional_flow.h" +#include "./mavlink_msg_optical_flow_rad.h" #include "./mavlink_msg_hil_sensor.h" #include "./mavlink_msg_sim_state.h" #include "./mavlink_msg_radio_status.h" -#include "./mavlink_msg_file_transfer_start.h" -#include "./mavlink_msg_file_transfer_dir_list.h" -#include "./mavlink_msg_file_transfer_res.h" +#include "./mavlink_msg_file_transfer_protocol.h" #include "./mavlink_msg_hil_gps.h" #include "./mavlink_msg_hil_optical_flow.h" #include "./mavlink_msg_hil_state_quaternion.h" @@ -538,11 +727,20 @@ enum MAV_SEVERITY #include "./mavlink_msg_log_request_end.h" #include "./mavlink_msg_gps_inject_data.h" #include "./mavlink_msg_gps2_raw.h" +#include "./mavlink_msg_power_status.h" +#include "./mavlink_msg_serial_control.h" +#include "./mavlink_msg_gps_rtk.h" +#include "./mavlink_msg_gps2_rtk.h" #include "./mavlink_msg_data_transmission_handshake.h" #include "./mavlink_msg_encapsulated_data.h" +#include "./mavlink_msg_distance_sensor.h" +#include "./mavlink_msg_terrain_request.h" +#include "./mavlink_msg_terrain_data.h" +#include "./mavlink_msg_terrain_check.h" +#include "./mavlink_msg_terrain_report.h" #include "./mavlink_msg_battery_status.h" -#include "./mavlink_msg_setpoint_8dof.h" -#include "./mavlink_msg_setpoint_6dof.h" +#include "./mavlink_msg_autopilot_version.h" +#include "./mavlink_msg_v2_extension.h" #include "./mavlink_msg_memory_vect.h" #include "./mavlink_msg_debug_vect.h" #include "./mavlink_msg_named_value_float.h" @@ -553,4 +751,4 @@ enum MAV_SEVERITY #ifdef __cplusplus } #endif // __cplusplus -#endif // COMMON_H +#endif // MAVLINK_COMMON_H diff --git a/mavlink/include/mavlink/v1.0/common/mavlink.h b/mavlink/include/mavlink/v1.0/common/mavlink.h index 17b7329..f92b2b2 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink.h @@ -1,6 +1,6 @@ /** @file * @brief MAVLink comm protocol built from common.xml - * @see http://pixhawk.ethz.ch/software/mavlink + * @see http://mavlink.org */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h index 8ddf5bf..ff2f104 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint32_t ti #endif } +#if MAVLINK_MSG_ID_ATTITUDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_attitude_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, 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, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, rollspeed); + _mav_put_float(buf, 20, pitchspeed); + _mav_put_float(buf, 24, yawspeed); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, MAVLINK_MSG_ID_ATTITUDE_LEN); +#endif +#else + mavlink_attitude_t *packet = (mavlink_attitude_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->rollspeed = rollspeed; + packet->pitchspeed = pitchspeed; + packet->yawspeed = yawspeed; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE ATTITUDE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h index 9f8d587..35170a7 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h @@ -5,10 +5,10 @@ typedef struct __mavlink_attitude_quaternion_t { uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - float q1; ///< Quaternion component 1 - float q2; ///< Quaternion component 2 - float q3; ///< Quaternion component 3 - float q4; ///< Quaternion component 4 + 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) @@ -44,10 +44,10 @@ typedef struct __mavlink_attitude_quaternion_t * @param msg The MAVLink message to compress the data into * * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param q1 Quaternion component 1 - * @param q2 Quaternion component 2 - * @param q3 Quaternion component 3 - * @param q4 Quaternion component 4 + * @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) @@ -97,10 +97,10 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, u * @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 - * @param q2 Quaternion component 2 - * @param q3 Quaternion component 3 - * @param q4 Quaternion component 4 + * @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) @@ -176,10 +176,10 @@ static inline uint16_t mavlink_msg_attitude_quaternion_encode_chan(uint8_t syste * @param chan MAVLink channel to send the message * * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param q1 Quaternion component 1 - * @param q2 Quaternion component 2 - * @param q3 Quaternion component 3 - * @param q4 Quaternion component 4 + * @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) @@ -223,6 +223,52 @@ static inline void mavlink_msg_attitude_quaternion_send(mavlink_channel_t chan, #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 + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_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); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE ATTITUDE_QUATERNION UNPACKING @@ -241,7 +287,7 @@ static inline uint32_t mavlink_msg_attitude_quaternion_get_time_boot_ms(const ma /** * @brief Get field q1 from attitude_quaternion message * - * @return Quaternion component 1 + * @return Quaternion component 1, w (1 in null-rotation) */ static inline float mavlink_msg_attitude_quaternion_get_q1(const mavlink_message_t* msg) { @@ -251,7 +297,7 @@ static inline float mavlink_msg_attitude_quaternion_get_q1(const mavlink_message /** * @brief Get field q2 from attitude_quaternion message * - * @return Quaternion component 2 + * @return Quaternion component 2, x (0 in null-rotation) */ static inline float mavlink_msg_attitude_quaternion_get_q2(const mavlink_message_t* msg) { @@ -261,7 +307,7 @@ static inline float mavlink_msg_attitude_quaternion_get_q2(const mavlink_message /** * @brief Get field q3 from attitude_quaternion message * - * @return Quaternion component 3 + * @return Quaternion component 3, y (0 in null-rotation) */ static inline float mavlink_msg_attitude_quaternion_get_q3(const mavlink_message_t* msg) { @@ -271,7 +317,7 @@ static inline float mavlink_msg_attitude_quaternion_get_q3(const mavlink_message /** * @brief Get field q4 from attitude_quaternion message * - * @return Quaternion component 4 + * @return Quaternion component 4, z (0 in null-rotation) */ static inline float mavlink_msg_attitude_quaternion_get_q4(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion_cov.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion_cov.h new file mode 100644 index 0000000..38b8f24 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion_cov.h @@ -0,0 +1,322 @@ +// MESSAGE ATTITUDE_QUATERNION_COV PACKING + +#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV 61 + +typedef struct __mavlink_attitude_quaternion_cov_t +{ + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + float q[4]; ///< Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + float rollspeed; ///< Roll angular speed (rad/s) + float pitchspeed; ///< Pitch angular speed (rad/s) + float yawspeed; ///< Yaw angular speed (rad/s) + float covariance[9]; ///< Attitude covariance +} mavlink_attitude_quaternion_cov_t; + +#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN 68 +#define MAVLINK_MSG_ID_61_LEN 68 + +#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_CRC 153 +#define MAVLINK_MSG_ID_61_CRC 153 + +#define MAVLINK_MSG_ATTITUDE_QUATERNION_COV_FIELD_Q_LEN 4 +#define MAVLINK_MSG_ATTITUDE_QUATERNION_COV_FIELD_COVARIANCE_LEN 9 + +#define MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION_COV { \ + "ATTITUDE_QUATERNION_COV", \ + 6, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_attitude_quaternion_cov_t, time_boot_ms) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 4, offsetof(mavlink_attitude_quaternion_cov_t, q) }, \ + { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_attitude_quaternion_cov_t, rollspeed) }, \ + { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_attitude_quaternion_cov_t, pitchspeed) }, \ + { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_attitude_quaternion_cov_t, yawspeed) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 9, 32, offsetof(mavlink_attitude_quaternion_cov_t, covariance) }, \ + } \ +} + + +/** + * @brief Pack a attitude_quaternion_cov message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @param covariance Attitude covariance + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_quaternion_cov_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, const float *q, float rollspeed, float pitchspeed, float yawspeed, const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_float_array(buf, 4, q, 4); + _mav_put_float_array(buf, 32, covariance, 9); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#else + mavlink_attitude_quaternion_cov_t packet; + packet.time_boot_ms = time_boot_ms; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*9); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#endif +} + +/** + * @brief Pack a attitude_quaternion_cov message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @param covariance Attitude covariance + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_quaternion_cov_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,const float *q,float rollspeed,float pitchspeed,float yawspeed,const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_float_array(buf, 4, q, 4); + _mav_put_float_array(buf, 32, covariance, 9); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#else + mavlink_attitude_quaternion_cov_t packet; + packet.time_boot_ms = time_boot_ms; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*9); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#endif +} + +/** + * @brief Encode a attitude_quaternion_cov struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param attitude_quaternion_cov C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_quaternion_cov_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_quaternion_cov_t* attitude_quaternion_cov) +{ + return mavlink_msg_attitude_quaternion_cov_pack(system_id, component_id, msg, attitude_quaternion_cov->time_boot_ms, attitude_quaternion_cov->q, attitude_quaternion_cov->rollspeed, attitude_quaternion_cov->pitchspeed, attitude_quaternion_cov->yawspeed, attitude_quaternion_cov->covariance); +} + +/** + * @brief Encode a attitude_quaternion_cov struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param attitude_quaternion_cov C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_quaternion_cov_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_quaternion_cov_t* attitude_quaternion_cov) +{ + return mavlink_msg_attitude_quaternion_cov_pack_chan(system_id, component_id, chan, msg, attitude_quaternion_cov->time_boot_ms, attitude_quaternion_cov->q, attitude_quaternion_cov->rollspeed, attitude_quaternion_cov->pitchspeed, attitude_quaternion_cov->yawspeed, attitude_quaternion_cov->covariance); +} + +/** + * @brief Send a attitude_quaternion_cov message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @param covariance Attitude covariance + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_attitude_quaternion_cov_send(mavlink_channel_t chan, uint32_t time_boot_ms, const float *q, float rollspeed, float pitchspeed, float yawspeed, const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_float_array(buf, 4, q, 4); + _mav_put_float_array(buf, 32, covariance, 9); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#endif +#else + mavlink_attitude_quaternion_cov_t packet; + packet.time_boot_ms = time_boot_ms; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*9); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_attitude_quaternion_cov_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, const float *q, float rollspeed, float pitchspeed, float yawspeed, const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_float_array(buf, 4, q, 4); + _mav_put_float_array(buf, 32, covariance, 9); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#endif +#else + mavlink_attitude_quaternion_cov_t *packet = (mavlink_attitude_quaternion_cov_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->rollspeed = rollspeed; + packet->pitchspeed = pitchspeed; + packet->yawspeed = yawspeed; + mav_array_memcpy(packet->q, q, sizeof(float)*4); + mav_array_memcpy(packet->covariance, covariance, sizeof(float)*9); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE ATTITUDE_QUATERNION_COV UNPACKING + + +/** + * @brief Get field time_boot_ms from attitude_quaternion_cov message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_attitude_quaternion_cov_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field q from attitude_quaternion_cov message + * + * @return Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + */ +static inline uint16_t mavlink_msg_attitude_quaternion_cov_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 4); +} + +/** + * @brief Get field rollspeed from attitude_quaternion_cov message + * + * @return Roll angular speed (rad/s) + */ +static inline float mavlink_msg_attitude_quaternion_cov_get_rollspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field pitchspeed from attitude_quaternion_cov message + * + * @return Pitch angular speed (rad/s) + */ +static inline float mavlink_msg_attitude_quaternion_cov_get_pitchspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field yawspeed from attitude_quaternion_cov message + * + * @return Yaw angular speed (rad/s) + */ +static inline float mavlink_msg_attitude_quaternion_cov_get_yawspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field covariance from attitude_quaternion_cov message + * + * @return Attitude covariance + */ +static inline uint16_t mavlink_msg_attitude_quaternion_cov_get_covariance(const mavlink_message_t* msg, float *covariance) +{ + return _MAV_RETURN_float_array(msg, covariance, 9, 32); +} + +/** + * @brief Decode a attitude_quaternion_cov message into a struct + * + * @param msg The message to decode + * @param attitude_quaternion_cov C-struct to decode the message contents into + */ +static inline void mavlink_msg_attitude_quaternion_cov_decode(const mavlink_message_t* msg, mavlink_attitude_quaternion_cov_t* attitude_quaternion_cov) +{ +#if MAVLINK_NEED_BYTE_SWAP + attitude_quaternion_cov->time_boot_ms = mavlink_msg_attitude_quaternion_cov_get_time_boot_ms(msg); + mavlink_msg_attitude_quaternion_cov_get_q(msg, attitude_quaternion_cov->q); + attitude_quaternion_cov->rollspeed = mavlink_msg_attitude_quaternion_cov_get_rollspeed(msg); + attitude_quaternion_cov->pitchspeed = mavlink_msg_attitude_quaternion_cov_get_pitchspeed(msg); + attitude_quaternion_cov->yawspeed = mavlink_msg_attitude_quaternion_cov_get_yawspeed(msg); + mavlink_msg_attitude_quaternion_cov_get_covariance(msg, attitude_quaternion_cov->covariance); +#else + memcpy(attitude_quaternion_cov, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_target.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_target.h new file mode 100644 index 0000000..7c4b1c7 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_target.h @@ -0,0 +1,345 @@ +// MESSAGE ATTITUDE_TARGET PACKING + +#define MAVLINK_MSG_ID_ATTITUDE_TARGET 83 + +typedef struct __mavlink_attitude_target_t +{ + uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot + float q[4]; ///< Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + float body_roll_rate; ///< Body roll rate in radians per second + float body_pitch_rate; ///< Body roll rate in radians per second + float body_yaw_rate; ///< Body roll rate in radians per second + float thrust; ///< Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + uint8_t type_mask; ///< Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude +} mavlink_attitude_target_t; + +#define MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN 37 +#define MAVLINK_MSG_ID_83_LEN 37 + +#define MAVLINK_MSG_ID_ATTITUDE_TARGET_CRC 22 +#define MAVLINK_MSG_ID_83_CRC 22 + +#define MAVLINK_MSG_ATTITUDE_TARGET_FIELD_Q_LEN 4 + +#define MAVLINK_MESSAGE_INFO_ATTITUDE_TARGET { \ + "ATTITUDE_TARGET", \ + 7, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_attitude_target_t, time_boot_ms) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 4, offsetof(mavlink_attitude_target_t, q) }, \ + { "body_roll_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_attitude_target_t, body_roll_rate) }, \ + { "body_pitch_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_attitude_target_t, body_pitch_rate) }, \ + { "body_yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_attitude_target_t, body_yaw_rate) }, \ + { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_attitude_target_t, thrust) }, \ + { "type_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_attitude_target_t, type_mask) }, \ + } \ +} + + +/** + * @brief Pack a attitude_target message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude + * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param body_roll_rate Body roll rate in radians per second + * @param body_pitch_rate Body roll rate in radians per second + * @param body_yaw_rate Body roll rate in radians per second + * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_target_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, type_mask); + _mav_put_float_array(buf, 4, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#else + mavlink_attitude_target_t packet; + packet.time_boot_ms = time_boot_ms; + packet.body_roll_rate = body_roll_rate; + packet.body_pitch_rate = body_pitch_rate; + packet.body_yaw_rate = body_yaw_rate; + packet.thrust = thrust; + packet.type_mask = type_mask; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_TARGET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_ATTITUDE_TARGET_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#endif +} + +/** + * @brief Pack a attitude_target message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude + * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param body_roll_rate Body roll rate in radians per second + * @param body_pitch_rate Body roll rate in radians per second + * @param body_yaw_rate Body roll rate in radians per second + * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_target_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t type_mask,const float *q,float body_roll_rate,float body_pitch_rate,float body_yaw_rate,float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, type_mask); + _mav_put_float_array(buf, 4, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#else + mavlink_attitude_target_t packet; + packet.time_boot_ms = time_boot_ms; + packet.body_roll_rate = body_roll_rate; + packet.body_pitch_rate = body_pitch_rate; + packet.body_yaw_rate = body_yaw_rate; + packet.thrust = thrust; + packet.type_mask = type_mask; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_TARGET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_ATTITUDE_TARGET_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#endif +} + +/** + * @brief Encode a attitude_target struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param attitude_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_target_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_target_t* attitude_target) +{ + return mavlink_msg_attitude_target_pack(system_id, component_id, msg, attitude_target->time_boot_ms, attitude_target->type_mask, attitude_target->q, attitude_target->body_roll_rate, attitude_target->body_pitch_rate, attitude_target->body_yaw_rate, attitude_target->thrust); +} + +/** + * @brief Encode a attitude_target struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param attitude_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_target_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_target_t* attitude_target) +{ + return mavlink_msg_attitude_target_pack_chan(system_id, component_id, chan, msg, attitude_target->time_boot_ms, attitude_target->type_mask, attitude_target->q, attitude_target->body_roll_rate, attitude_target->body_pitch_rate, attitude_target->body_yaw_rate, attitude_target->thrust); +} + +/** + * @brief Send a attitude_target message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude + * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param body_roll_rate Body roll rate in radians per second + * @param body_pitch_rate Body roll rate in radians per second + * @param body_yaw_rate Body roll rate in radians per second + * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_attitude_target_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, type_mask); + _mav_put_float_array(buf, 4, q, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_TARGET, buf, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_ATTITUDE_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_TARGET, buf, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#endif +#else + mavlink_attitude_target_t packet; + packet.time_boot_ms = time_boot_ms; + packet.body_roll_rate = body_roll_rate; + packet.body_pitch_rate = body_pitch_rate; + packet.body_yaw_rate = body_yaw_rate; + packet.thrust = thrust; + packet.type_mask = type_mask; + mav_array_memcpy(packet.q, q, sizeof(float)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_TARGET, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_ATTITUDE_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_TARGET, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_attitude_target_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, type_mask); + _mav_put_float_array(buf, 4, q, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_TARGET, buf, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_ATTITUDE_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_TARGET, buf, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#endif +#else + mavlink_attitude_target_t *packet = (mavlink_attitude_target_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->body_roll_rate = body_roll_rate; + packet->body_pitch_rate = body_pitch_rate; + packet->body_yaw_rate = body_yaw_rate; + packet->thrust = thrust; + packet->type_mask = type_mask; + mav_array_memcpy(packet->q, q, sizeof(float)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_TARGET, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_ATTITUDE_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_TARGET, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE ATTITUDE_TARGET UNPACKING + + +/** + * @brief Get field time_boot_ms from attitude_target message + * + * @return Timestamp in milliseconds since system boot + */ +static inline uint32_t mavlink_msg_attitude_target_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field type_mask from attitude_target message + * + * @return Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude + */ +static inline uint8_t mavlink_msg_attitude_target_get_type_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 36); +} + +/** + * @brief Get field q from attitude_target message + * + * @return Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + */ +static inline uint16_t mavlink_msg_attitude_target_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 4); +} + +/** + * @brief Get field body_roll_rate from attitude_target message + * + * @return Body roll rate in radians per second + */ +static inline float mavlink_msg_attitude_target_get_body_roll_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field body_pitch_rate from attitude_target message + * + * @return Body roll rate in radians per second + */ +static inline float mavlink_msg_attitude_target_get_body_pitch_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field body_yaw_rate from attitude_target message + * + * @return Body roll rate in radians per second + */ +static inline float mavlink_msg_attitude_target_get_body_yaw_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field thrust from attitude_target message + * + * @return Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + */ +static inline float mavlink_msg_attitude_target_get_thrust(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Decode a attitude_target message into a struct + * + * @param msg The message to decode + * @param attitude_target C-struct to decode the message contents into + */ +static inline void mavlink_msg_attitude_target_decode(const mavlink_message_t* msg, mavlink_attitude_target_t* attitude_target) +{ +#if MAVLINK_NEED_BYTE_SWAP + attitude_target->time_boot_ms = mavlink_msg_attitude_target_get_time_boot_ms(msg); + mavlink_msg_attitude_target_get_q(msg, attitude_target->q); + attitude_target->body_roll_rate = mavlink_msg_attitude_target_get_body_roll_rate(msg); + attitude_target->body_pitch_rate = mavlink_msg_attitude_target_get_body_pitch_rate(msg); + attitude_target->body_yaw_rate = mavlink_msg_attitude_target_get_body_yaw_rate(msg); + attitude_target->thrust = mavlink_msg_attitude_target_get_thrust(msg); + attitude_target->type_mask = mavlink_msg_attitude_target_get_type_mask(msg); +#else + memcpy(attitude_target, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h index 5703a59..f31b6bb 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h @@ -146,6 +146,38 @@ static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char #endif } +#if MAVLINK_MSG_ID_AUTH_KEY_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_auth_key_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *key) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + + _mav_put_char_array(buf, 0, key, 32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, MAVLINK_MSG_ID_AUTH_KEY_LEN); +#endif +#else + mavlink_auth_key_t *packet = (mavlink_auth_key_t *)msgbuf; + + mav_array_memcpy(packet->key, key, sizeof(char)*32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)packet, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)packet, MAVLINK_MSG_ID_AUTH_KEY_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE AUTH_KEY UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_autopilot_version.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_autopilot_version.h new file mode 100644 index 0000000..4acb1d9 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_autopilot_version.h @@ -0,0 +1,249 @@ +// MESSAGE AUTOPILOT_VERSION PACKING + +#define MAVLINK_MSG_ID_AUTOPILOT_VERSION 148 + +typedef struct __mavlink_autopilot_version_t +{ + uint64_t capabilities; ///< bitmask of capabilities (see MAV_PROTOCOL_CAPABILITY enum) + uint32_t version; ///< Firmware version number + uint8_t custom_version[8]; ///< Custom version field, commonly the first 8 bytes (16 characters) of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. +} mavlink_autopilot_version_t; + +#define MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN 20 +#define MAVLINK_MSG_ID_148_LEN 20 + +#define MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC 49 +#define MAVLINK_MSG_ID_148_CRC 49 + +#define MAVLINK_MSG_AUTOPILOT_VERSION_FIELD_CUSTOM_VERSION_LEN 8 + +#define MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION { \ + "AUTOPILOT_VERSION", \ + 3, \ + { { "capabilities", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_autopilot_version_t, capabilities) }, \ + { "version", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_autopilot_version_t, version) }, \ + { "custom_version", NULL, MAVLINK_TYPE_UINT8_T, 8, 12, offsetof(mavlink_autopilot_version_t, custom_version) }, \ + } \ +} + + +/** + * @brief Pack a autopilot_version message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param capabilities bitmask of capabilities (see MAV_PROTOCOL_CAPABILITY enum) + * @param version Firmware version number + * @param custom_version Custom version field, commonly the first 8 bytes (16 characters) of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_autopilot_version_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t capabilities, uint32_t version, const uint8_t *custom_version) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN]; + _mav_put_uint64_t(buf, 0, capabilities); + _mav_put_uint32_t(buf, 8, version); + _mav_put_uint8_t_array(buf, 12, custom_version, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#else + mavlink_autopilot_version_t packet; + packet.capabilities = capabilities; + packet.version = version; + mav_array_memcpy(packet.custom_version, custom_version, sizeof(uint8_t)*8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AUTOPILOT_VERSION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#endif +} + +/** + * @brief Pack a autopilot_version message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param capabilities bitmask of capabilities (see MAV_PROTOCOL_CAPABILITY enum) + * @param version Firmware version number + * @param custom_version Custom version field, commonly the first 8 bytes (16 characters) of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_autopilot_version_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t capabilities,uint32_t version,const uint8_t *custom_version) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN]; + _mav_put_uint64_t(buf, 0, capabilities); + _mav_put_uint32_t(buf, 8, version); + _mav_put_uint8_t_array(buf, 12, custom_version, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#else + mavlink_autopilot_version_t packet; + packet.capabilities = capabilities; + packet.version = version; + mav_array_memcpy(packet.custom_version, custom_version, sizeof(uint8_t)*8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AUTOPILOT_VERSION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#endif +} + +/** + * @brief Encode a autopilot_version struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param autopilot_version C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_autopilot_version_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_autopilot_version_t* autopilot_version) +{ + return mavlink_msg_autopilot_version_pack(system_id, component_id, msg, autopilot_version->capabilities, autopilot_version->version, autopilot_version->custom_version); +} + +/** + * @brief Encode a autopilot_version struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param autopilot_version C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_autopilot_version_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_autopilot_version_t* autopilot_version) +{ + return mavlink_msg_autopilot_version_pack_chan(system_id, component_id, chan, msg, autopilot_version->capabilities, autopilot_version->version, autopilot_version->custom_version); +} + +/** + * @brief Send a autopilot_version message + * @param chan MAVLink channel to send the message + * + * @param capabilities bitmask of capabilities (see MAV_PROTOCOL_CAPABILITY enum) + * @param version Firmware version number + * @param custom_version Custom version field, commonly the first 8 bytes (16 characters) of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_autopilot_version_send(mavlink_channel_t chan, uint64_t capabilities, uint32_t version, const uint8_t *custom_version) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN]; + _mav_put_uint64_t(buf, 0, capabilities); + _mav_put_uint32_t(buf, 8, version); + _mav_put_uint8_t_array(buf, 12, custom_version, 8); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#endif +#else + mavlink_autopilot_version_t packet; + packet.capabilities = capabilities; + packet.version = version; + mav_array_memcpy(packet.custom_version, custom_version, sizeof(uint8_t)*8); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, (const char *)&packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, (const char *)&packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_autopilot_version_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t capabilities, uint32_t version, const uint8_t *custom_version) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, capabilities); + _mav_put_uint32_t(buf, 8, version); + _mav_put_uint8_t_array(buf, 12, custom_version, 8); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#endif +#else + mavlink_autopilot_version_t *packet = (mavlink_autopilot_version_t *)msgbuf; + packet->capabilities = capabilities; + packet->version = version; + mav_array_memcpy(packet->custom_version, custom_version, sizeof(uint8_t)*8); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, (const char *)packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, (const char *)packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE AUTOPILOT_VERSION UNPACKING + + +/** + * @brief Get field capabilities from autopilot_version message + * + * @return bitmask of capabilities (see MAV_PROTOCOL_CAPABILITY enum) + */ +static inline uint64_t mavlink_msg_autopilot_version_get_capabilities(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field version from autopilot_version message + * + * @return Firmware version number + */ +static inline uint32_t mavlink_msg_autopilot_version_get_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field custom_version from autopilot_version message + * + * @return Custom version field, commonly the first 8 bytes (16 characters) of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + */ +static inline uint16_t mavlink_msg_autopilot_version_get_custom_version(const mavlink_message_t* msg, uint8_t *custom_version) +{ + return _MAV_RETURN_uint8_t_array(msg, custom_version, 8, 12); +} + +/** + * @brief Decode a autopilot_version message into a struct + * + * @param msg The message to decode + * @param autopilot_version C-struct to decode the message contents into + */ +static inline void mavlink_msg_autopilot_version_decode(const mavlink_message_t* msg, mavlink_autopilot_version_t* autopilot_version) +{ +#if MAVLINK_NEED_BYTE_SWAP + autopilot_version->capabilities = mavlink_msg_autopilot_version_get_capabilities(msg); + autopilot_version->version = mavlink_msg_autopilot_version_get_version(msg); + mavlink_msg_autopilot_version_get_custom_version(msg, autopilot_version->custom_version); +#else + memcpy(autopilot_version, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h index 03e4d56..12371e6 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h @@ -6,39 +6,35 @@ typedef struct __mavlink_battery_status_t { int32_t current_consumed; ///< Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate int32_t energy_consumed; ///< Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate - uint16_t voltage_cell_1; ///< Battery voltage of cell 1, in millivolts (1 = 1 millivolt) - uint16_t voltage_cell_2; ///< Battery voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell - uint16_t voltage_cell_3; ///< Battery voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell - uint16_t voltage_cell_4; ///< Battery voltage of cell 4, in millivolts (1 = 1 millivolt), -1: no cell - uint16_t voltage_cell_5; ///< Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell - uint16_t voltage_cell_6; ///< Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell + int16_t temperature; ///< Temperature of the battery in centi-degrees celsius. INT16_MAX for unknown temperature. + uint16_t voltages[10]; ///< Battery voltage of cells, in millivolts (1 = 1 millivolt) int16_t current_battery; ///< Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - uint8_t accu_id; ///< Accupack ID + uint8_t id; ///< Battery ID + uint8_t battery_function; ///< Function of the battery + uint8_t type; ///< Type (chemistry) of the battery int8_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery } mavlink_battery_status_t; -#define MAVLINK_MSG_ID_BATTERY_STATUS_LEN 24 -#define MAVLINK_MSG_ID_147_LEN 24 - -#define MAVLINK_MSG_ID_BATTERY_STATUS_CRC 177 -#define MAVLINK_MSG_ID_147_CRC 177 +#define MAVLINK_MSG_ID_BATTERY_STATUS_LEN 36 +#define MAVLINK_MSG_ID_147_LEN 36 +#define MAVLINK_MSG_ID_BATTERY_STATUS_CRC 154 +#define MAVLINK_MSG_ID_147_CRC 154 +#define MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN 10 #define MAVLINK_MESSAGE_INFO_BATTERY_STATUS { \ "BATTERY_STATUS", \ - 11, \ + 9, \ { { "current_consumed", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_battery_status_t, current_consumed) }, \ { "energy_consumed", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_battery_status_t, energy_consumed) }, \ - { "voltage_cell_1", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_battery_status_t, voltage_cell_1) }, \ - { "voltage_cell_2", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_battery_status_t, voltage_cell_2) }, \ - { "voltage_cell_3", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_battery_status_t, voltage_cell_3) }, \ - { "voltage_cell_4", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_battery_status_t, voltage_cell_4) }, \ - { "voltage_cell_5", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_battery_status_t, voltage_cell_5) }, \ - { "voltage_cell_6", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_battery_status_t, voltage_cell_6) }, \ - { "current_battery", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_battery_status_t, current_battery) }, \ - { "accu_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_battery_status_t, accu_id) }, \ - { "battery_remaining", NULL, MAVLINK_TYPE_INT8_T, 0, 23, offsetof(mavlink_battery_status_t, battery_remaining) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_battery_status_t, temperature) }, \ + { "voltages", NULL, MAVLINK_TYPE_UINT16_T, 10, 10, offsetof(mavlink_battery_status_t, voltages) }, \ + { "current_battery", NULL, MAVLINK_TYPE_INT16_T, 0, 30, offsetof(mavlink_battery_status_t, current_battery) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_battery_status_t, id) }, \ + { "battery_function", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_battery_status_t, battery_function) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_battery_status_t, type) }, \ + { "battery_remaining", NULL, MAVLINK_TYPE_INT8_T, 0, 35, offsetof(mavlink_battery_status_t, battery_remaining) }, \ } \ } @@ -49,13 +45,11 @@ typedef struct __mavlink_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 accu_id Accupack ID - * @param voltage_cell_1 Battery voltage of cell 1, in millivolts (1 = 1 millivolt) - * @param voltage_cell_2 Battery voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_3 Battery voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_4 Battery voltage of cell 4, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_5 Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_6 Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell + * @param id Battery ID + * @param battery_function Function of the battery + * @param type Type (chemistry) of the battery + * @param temperature Temperature of the battery in centi-degrees celsius. INT16_MAX for unknown temperature. + * @param voltages Battery voltage of cells, in millivolts (1 = 1 millivolt) * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current * @param current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate * @param energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate @@ -63,37 +57,31 @@ typedef struct __mavlink_battery_status_t * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t accu_id, uint16_t voltage_cell_1, uint16_t voltage_cell_2, uint16_t voltage_cell_3, uint16_t voltage_cell_4, uint16_t voltage_cell_5, uint16_t voltage_cell_6, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining) + uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, const uint16_t *voltages, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_BATTERY_STATUS_LEN]; _mav_put_int32_t(buf, 0, current_consumed); _mav_put_int32_t(buf, 4, energy_consumed); - _mav_put_uint16_t(buf, 8, voltage_cell_1); - _mav_put_uint16_t(buf, 10, voltage_cell_2); - _mav_put_uint16_t(buf, 12, voltage_cell_3); - _mav_put_uint16_t(buf, 14, voltage_cell_4); - _mav_put_uint16_t(buf, 16, voltage_cell_5); - _mav_put_uint16_t(buf, 18, voltage_cell_6); - _mav_put_int16_t(buf, 20, current_battery); - _mav_put_uint8_t(buf, 22, accu_id); - _mav_put_int8_t(buf, 23, battery_remaining); - + _mav_put_int16_t(buf, 8, temperature); + _mav_put_int16_t(buf, 30, current_battery); + _mav_put_uint8_t(buf, 32, id); + _mav_put_uint8_t(buf, 33, battery_function); + _mav_put_uint8_t(buf, 34, type); + _mav_put_int8_t(buf, 35, battery_remaining); + _mav_put_uint16_t_array(buf, 10, voltages, 10); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); #else mavlink_battery_status_t packet; packet.current_consumed = current_consumed; packet.energy_consumed = energy_consumed; - packet.voltage_cell_1 = voltage_cell_1; - packet.voltage_cell_2 = voltage_cell_2; - packet.voltage_cell_3 = voltage_cell_3; - packet.voltage_cell_4 = voltage_cell_4; - packet.voltage_cell_5 = voltage_cell_5; - packet.voltage_cell_6 = voltage_cell_6; + packet.temperature = temperature; packet.current_battery = current_battery; - packet.accu_id = accu_id; + packet.id = id; + packet.battery_function = battery_function; + packet.type = type; packet.battery_remaining = battery_remaining; - + mav_array_memcpy(packet.voltages, voltages, sizeof(uint16_t)*10); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); #endif @@ -111,13 +99,11 @@ static inline uint16_t mavlink_msg_battery_status_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 accu_id Accupack ID - * @param voltage_cell_1 Battery voltage of cell 1, in millivolts (1 = 1 millivolt) - * @param voltage_cell_2 Battery voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_3 Battery voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_4 Battery voltage of cell 4, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_5 Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_6 Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell + * @param id Battery ID + * @param battery_function Function of the battery + * @param type Type (chemistry) of the battery + * @param temperature Temperature of the battery in centi-degrees celsius. INT16_MAX for unknown temperature. + * @param voltages Battery voltage of cells, in millivolts (1 = 1 millivolt) * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current * @param current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate * @param energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate @@ -126,37 +112,31 @@ static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t accu_id,uint16_t voltage_cell_1,uint16_t voltage_cell_2,uint16_t voltage_cell_3,uint16_t voltage_cell_4,uint16_t voltage_cell_5,uint16_t voltage_cell_6,int16_t current_battery,int32_t current_consumed,int32_t energy_consumed,int8_t battery_remaining) + uint8_t id,uint8_t battery_function,uint8_t type,int16_t temperature,const uint16_t *voltages,int16_t current_battery,int32_t current_consumed,int32_t energy_consumed,int8_t battery_remaining) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_BATTERY_STATUS_LEN]; _mav_put_int32_t(buf, 0, current_consumed); _mav_put_int32_t(buf, 4, energy_consumed); - _mav_put_uint16_t(buf, 8, voltage_cell_1); - _mav_put_uint16_t(buf, 10, voltage_cell_2); - _mav_put_uint16_t(buf, 12, voltage_cell_3); - _mav_put_uint16_t(buf, 14, voltage_cell_4); - _mav_put_uint16_t(buf, 16, voltage_cell_5); - _mav_put_uint16_t(buf, 18, voltage_cell_6); - _mav_put_int16_t(buf, 20, current_battery); - _mav_put_uint8_t(buf, 22, accu_id); - _mav_put_int8_t(buf, 23, battery_remaining); - + _mav_put_int16_t(buf, 8, temperature); + _mav_put_int16_t(buf, 30, current_battery); + _mav_put_uint8_t(buf, 32, id); + _mav_put_uint8_t(buf, 33, battery_function); + _mav_put_uint8_t(buf, 34, type); + _mav_put_int8_t(buf, 35, battery_remaining); + _mav_put_uint16_t_array(buf, 10, voltages, 10); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); #else mavlink_battery_status_t packet; packet.current_consumed = current_consumed; packet.energy_consumed = energy_consumed; - packet.voltage_cell_1 = voltage_cell_1; - packet.voltage_cell_2 = voltage_cell_2; - packet.voltage_cell_3 = voltage_cell_3; - packet.voltage_cell_4 = voltage_cell_4; - packet.voltage_cell_5 = voltage_cell_5; - packet.voltage_cell_6 = voltage_cell_6; + packet.temperature = temperature; packet.current_battery = current_battery; - packet.accu_id = accu_id; + packet.id = id; + packet.battery_function = battery_function; + packet.type = type; packet.battery_remaining = battery_remaining; - + mav_array_memcpy(packet.voltages, voltages, sizeof(uint16_t)*10); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); #endif @@ -178,7 +158,7 @@ static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, u */ static inline uint16_t mavlink_msg_battery_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_battery_status_t* battery_status) { - return mavlink_msg_battery_status_pack(system_id, component_id, msg, battery_status->accu_id, battery_status->voltage_cell_1, battery_status->voltage_cell_2, battery_status->voltage_cell_3, battery_status->voltage_cell_4, battery_status->voltage_cell_5, battery_status->voltage_cell_6, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining); + return mavlink_msg_battery_status_pack(system_id, component_id, msg, battery_status->id, battery_status->battery_function, battery_status->type, battery_status->temperature, battery_status->voltages, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining); } /** @@ -192,20 +172,18 @@ static inline uint16_t mavlink_msg_battery_status_encode(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_battery_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_battery_status_t* battery_status) { - return mavlink_msg_battery_status_pack_chan(system_id, component_id, chan, msg, battery_status->accu_id, battery_status->voltage_cell_1, battery_status->voltage_cell_2, battery_status->voltage_cell_3, battery_status->voltage_cell_4, battery_status->voltage_cell_5, battery_status->voltage_cell_6, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining); + return mavlink_msg_battery_status_pack_chan(system_id, component_id, chan, msg, battery_status->id, battery_status->battery_function, battery_status->type, battery_status->temperature, battery_status->voltages, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining); } /** * @brief Send a battery_status message * @param chan MAVLink channel to send the message * - * @param accu_id Accupack ID - * @param voltage_cell_1 Battery voltage of cell 1, in millivolts (1 = 1 millivolt) - * @param voltage_cell_2 Battery voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_3 Battery voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_4 Battery voltage of cell 4, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_5 Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_6 Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell + * @param id Battery ID + * @param battery_function Function of the battery + * @param type Type (chemistry) of the battery + * @param temperature Temperature of the battery in centi-degrees celsius. INT16_MAX for unknown temperature. + * @param voltages Battery voltage of cells, in millivolts (1 = 1 millivolt) * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current * @param current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate * @param energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate @@ -213,22 +191,19 @@ static inline uint16_t mavlink_msg_battery_status_encode_chan(uint8_t system_id, */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8_t accu_id, uint16_t voltage_cell_1, uint16_t voltage_cell_2, uint16_t voltage_cell_3, uint16_t voltage_cell_4, uint16_t voltage_cell_5, uint16_t voltage_cell_6, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining) +static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, const uint16_t *voltages, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_BATTERY_STATUS_LEN]; _mav_put_int32_t(buf, 0, current_consumed); _mav_put_int32_t(buf, 4, energy_consumed); - _mav_put_uint16_t(buf, 8, voltage_cell_1); - _mav_put_uint16_t(buf, 10, voltage_cell_2); - _mav_put_uint16_t(buf, 12, voltage_cell_3); - _mav_put_uint16_t(buf, 14, voltage_cell_4); - _mav_put_uint16_t(buf, 16, voltage_cell_5); - _mav_put_uint16_t(buf, 18, voltage_cell_6); - _mav_put_int16_t(buf, 20, current_battery); - _mav_put_uint8_t(buf, 22, accu_id); - _mav_put_int8_t(buf, 23, battery_remaining); - + _mav_put_int16_t(buf, 8, temperature); + _mav_put_int16_t(buf, 30, current_battery); + _mav_put_uint8_t(buf, 32, id); + _mav_put_uint8_t(buf, 33, battery_function); + _mav_put_uint8_t(buf, 34, type); + _mav_put_int8_t(buf, 35, battery_remaining); + _mav_put_uint16_t_array(buf, 10, voltages, 10); #if MAVLINK_CRC_EXTRA _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); #else @@ -238,16 +213,13 @@ static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8 mavlink_battery_status_t packet; packet.current_consumed = current_consumed; packet.energy_consumed = energy_consumed; - packet.voltage_cell_1 = voltage_cell_1; - packet.voltage_cell_2 = voltage_cell_2; - packet.voltage_cell_3 = voltage_cell_3; - packet.voltage_cell_4 = voltage_cell_4; - packet.voltage_cell_5 = voltage_cell_5; - packet.voltage_cell_6 = voltage_cell_6; + packet.temperature = temperature; packet.current_battery = current_battery; - packet.accu_id = accu_id; + packet.id = id; + packet.battery_function = battery_function; + packet.type = type; packet.battery_remaining = battery_remaining; - + mav_array_memcpy(packet.voltages, voltages, sizeof(uint16_t)*10); #if MAVLINK_CRC_EXTRA _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)&packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); #else @@ -256,79 +228,105 @@ static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8 #endif } -#endif - -// MESSAGE BATTERY_STATUS UNPACKING - - -/** - * @brief Get field accu_id from battery_status message - * - * @return Accupack ID +#if MAVLINK_MSG_ID_BATTERY_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. */ -static inline uint8_t mavlink_msg_battery_status_get_accu_id(const mavlink_message_t* msg) +static inline void mavlink_msg_battery_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, const uint16_t *voltages, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining) { - return _MAV_RETURN_uint8_t(msg, 22); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, current_consumed); + _mav_put_int32_t(buf, 4, energy_consumed); + _mav_put_int16_t(buf, 8, temperature); + _mav_put_int16_t(buf, 30, current_battery); + _mav_put_uint8_t(buf, 32, id); + _mav_put_uint8_t(buf, 33, battery_function); + _mav_put_uint8_t(buf, 34, type); + _mav_put_int8_t(buf, 35, battery_remaining); + _mav_put_uint16_t_array(buf, 10, voltages, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); +#endif +#else + mavlink_battery_status_t *packet = (mavlink_battery_status_t *)msgbuf; + packet->current_consumed = current_consumed; + packet->energy_consumed = energy_consumed; + packet->temperature = temperature; + packet->current_battery = current_battery; + packet->id = id; + packet->battery_function = battery_function; + packet->type = type; + packet->battery_remaining = battery_remaining; + mav_array_memcpy(packet->voltages, voltages, sizeof(uint16_t)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); +#endif +#endif } +#endif + +#endif + +// MESSAGE BATTERY_STATUS UNPACKING -/** - * @brief Get field voltage_cell_1 from battery_status message - * - * @return Battery voltage of cell 1, in millivolts (1 = 1 millivolt) - */ -static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} /** - * @brief Get field voltage_cell_2 from battery_status message + * @brief Get field id from battery_status message * - * @return Battery voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell + * @return Battery ID */ -static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_2(const mavlink_message_t* msg) +static inline uint8_t mavlink_msg_battery_status_get_id(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 10); + return _MAV_RETURN_uint8_t(msg, 32); } /** - * @brief Get field voltage_cell_3 from battery_status message + * @brief Get field battery_function from battery_status message * - * @return Battery voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell + * @return Function of the battery */ -static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_3(const mavlink_message_t* msg) +static inline uint8_t mavlink_msg_battery_status_get_battery_function(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 12); + return _MAV_RETURN_uint8_t(msg, 33); } /** - * @brief Get field voltage_cell_4 from battery_status message + * @brief Get field type from battery_status message * - * @return Battery voltage of cell 4, in millivolts (1 = 1 millivolt), -1: no cell + * @return Type (chemistry) of the battery */ -static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_4(const mavlink_message_t* msg) +static inline uint8_t mavlink_msg_battery_status_get_type(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 14); + return _MAV_RETURN_uint8_t(msg, 34); } /** - * @brief Get field voltage_cell_5 from battery_status message + * @brief Get field temperature from battery_status message * - * @return Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell + * @return Temperature of the battery in centi-degrees celsius. INT16_MAX for unknown temperature. */ -static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_5(const mavlink_message_t* msg) +static inline int16_t mavlink_msg_battery_status_get_temperature(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 16); + return _MAV_RETURN_int16_t(msg, 8); } /** - * @brief Get field voltage_cell_6 from battery_status message + * @brief Get field voltages from battery_status message * - * @return Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell + * @return Battery voltage of cells, in millivolts (1 = 1 millivolt) */ -static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_6(const mavlink_message_t* msg) +static inline uint16_t mavlink_msg_battery_status_get_voltages(const mavlink_message_t* msg, uint16_t *voltages) { - return _MAV_RETURN_uint16_t(msg, 18); + return _MAV_RETURN_uint16_t_array(msg, voltages, 10, 10); } /** @@ -338,7 +336,7 @@ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_6(const mavli */ static inline int16_t mavlink_msg_battery_status_get_current_battery(const mavlink_message_t* msg) { - return _MAV_RETURN_int16_t(msg, 20); + return _MAV_RETURN_int16_t(msg, 30); } /** @@ -368,7 +366,7 @@ static inline int32_t mavlink_msg_battery_status_get_energy_consumed(const mavli */ static inline int8_t mavlink_msg_battery_status_get_battery_remaining(const mavlink_message_t* msg) { - return _MAV_RETURN_int8_t(msg, 23); + return _MAV_RETURN_int8_t(msg, 35); } /** @@ -382,14 +380,12 @@ static inline void mavlink_msg_battery_status_decode(const mavlink_message_t* ms #if MAVLINK_NEED_BYTE_SWAP battery_status->current_consumed = mavlink_msg_battery_status_get_current_consumed(msg); battery_status->energy_consumed = mavlink_msg_battery_status_get_energy_consumed(msg); - battery_status->voltage_cell_1 = mavlink_msg_battery_status_get_voltage_cell_1(msg); - battery_status->voltage_cell_2 = mavlink_msg_battery_status_get_voltage_cell_2(msg); - battery_status->voltage_cell_3 = mavlink_msg_battery_status_get_voltage_cell_3(msg); - battery_status->voltage_cell_4 = mavlink_msg_battery_status_get_voltage_cell_4(msg); - battery_status->voltage_cell_5 = mavlink_msg_battery_status_get_voltage_cell_5(msg); - battery_status->voltage_cell_6 = mavlink_msg_battery_status_get_voltage_cell_6(msg); + battery_status->temperature = mavlink_msg_battery_status_get_temperature(msg); + mavlink_msg_battery_status_get_voltages(msg, battery_status->voltages); battery_status->current_battery = mavlink_msg_battery_status_get_current_battery(msg); - battery_status->accu_id = mavlink_msg_battery_status_get_accu_id(msg); + battery_status->id = mavlink_msg_battery_status_get_id(msg); + battery_status->battery_function = mavlink_msg_battery_status_get_battery_function(msg); + battery_status->type = mavlink_msg_battery_status_get_type(msg); battery_status->battery_remaining = mavlink_msg_battery_status_get_battery_remaining(msg); #else memcpy(battery_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_BATTERY_STATUS_LEN); diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h index 0b6de93..3f0987f 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h @@ -173,6 +173,42 @@ static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t ch #endif } +#if MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_change_operator_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t control_request, uint8_t version, const char *passkey) +{ +#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, control_request); + _mav_put_uint8_t(buf, 2, version); + _mav_put_char_array(buf, 3, passkey, 25); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); +#endif +#else + mavlink_change_operator_control_t *packet = (mavlink_change_operator_control_t *)msgbuf; + packet->target_system = target_system; + packet->control_request = control_request; + packet->version = version; + mav_array_memcpy(packet->passkey, passkey, sizeof(char)*25); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE CHANGE_OPERATOR_CONTROL UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h index c6f6a28..768e7ed 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_change_operator_control_ack_send(mavlink_channel_ #endif } +#if MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_change_operator_control_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, gcs_system_id); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, ack); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); +#endif +#else + mavlink_change_operator_control_ack_t *packet = (mavlink_change_operator_control_ack_t *)msgbuf; + packet->gcs_system_id = gcs_system_id; + packet->control_request = control_request; + packet->ack = ack; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE CHANGE_OPERATOR_CONTROL_ACK UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h index dca2fe6..d3d1630 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, uint16_t #endif } +#if MAVLINK_MSG_ID_COMMAND_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_command_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t command, uint8_t result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, command); + _mav_put_uint8_t(buf, 2, result); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN); +#endif +#else + mavlink_command_ack_t *packet = (mavlink_command_ack_t *)msgbuf; + packet->command = command; + packet->result = result; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE COMMAND_ACK UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_int.h new file mode 100644 index 0000000..4713d01 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_int.h @@ -0,0 +1,497 @@ +// MESSAGE COMMAND_INT PACKING + +#define MAVLINK_MSG_ID_COMMAND_INT 75 + +typedef struct __mavlink_command_int_t +{ + float param1; ///< PARAM1, see MAV_CMD enum + float param2; ///< PARAM2, see MAV_CMD enum + float param3; ///< PARAM3, see MAV_CMD enum + float param4; ///< PARAM4, see MAV_CMD enum + int32_t x; ///< PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + int32_t y; ///< PARAM6 / local: y position in meters * 1e4, global: longitude in degrees * 10^7 + float z; ///< PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + uint16_t command; ///< The scheduled action for the mission item. see MAV_CMD in common.xml MAVLink specs + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t frame; ///< The coordinate system of the COMMAND. see MAV_FRAME in mavlink_types.h + uint8_t current; ///< false:0, true:1 + uint8_t autocontinue; ///< autocontinue to next wp +} mavlink_command_int_t; + +#define MAVLINK_MSG_ID_COMMAND_INT_LEN 35 +#define MAVLINK_MSG_ID_75_LEN 35 + +#define MAVLINK_MSG_ID_COMMAND_INT_CRC 158 +#define MAVLINK_MSG_ID_75_CRC 158 + + + +#define MAVLINK_MESSAGE_INFO_COMMAND_INT { \ + "COMMAND_INT", \ + 13, \ + { { "param1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_command_int_t, param1) }, \ + { "param2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_command_int_t, param2) }, \ + { "param3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_command_int_t, param3) }, \ + { "param4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_command_int_t, param4) }, \ + { "x", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_command_int_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_command_int_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_command_int_t, z) }, \ + { "command", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_command_int_t, command) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_command_int_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_command_int_t, target_component) }, \ + { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_command_int_t, frame) }, \ + { "current", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_command_int_t, current) }, \ + { "autocontinue", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_command_int_t, autocontinue) }, \ + } \ +} + + +/** + * @brief Pack a command_int message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param frame The coordinate system of the COMMAND. see MAV_FRAME in mavlink_types.h + * @param command The scheduled action for the mission item. see MAV_CMD in common.xml MAVLink specs + * @param current false:0, true:1 + * @param autocontinue autocontinue to next wp + * @param param1 PARAM1, see MAV_CMD enum + * @param param2 PARAM2, see MAV_CMD enum + * @param param3 PARAM3, see MAV_CMD enum + * @param param4 PARAM4, see MAV_CMD enum + * @param x PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + * @param y PARAM6 / local: y position in meters * 1e4, global: longitude in degrees * 10^7 + * @param z PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_command_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMMAND_INT_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_int32_t(buf, 16, x); + _mav_put_int32_t(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, command); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 32, frame); + _mav_put_uint8_t(buf, 33, current); + _mav_put_uint8_t(buf, 34, autocontinue); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#else + mavlink_command_int_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.current = current; + packet.autocontinue = autocontinue; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMMAND_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_INT_LEN, MAVLINK_MSG_ID_COMMAND_INT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#endif +} + +/** + * @brief Pack a command_int message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param frame The coordinate system of the COMMAND. see MAV_FRAME in mavlink_types.h + * @param command The scheduled action for the mission item. see MAV_CMD in common.xml MAVLink specs + * @param current false:0, true:1 + * @param autocontinue autocontinue to next wp + * @param param1 PARAM1, see MAV_CMD enum + * @param param2 PARAM2, see MAV_CMD enum + * @param param3 PARAM3, see MAV_CMD enum + * @param param4 PARAM4, see MAV_CMD enum + * @param x PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + * @param y PARAM6 / local: y position in meters * 1e4, global: longitude in degrees * 10^7 + * @param z PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_command_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t frame,uint16_t command,uint8_t current,uint8_t autocontinue,float param1,float param2,float param3,float param4,int32_t x,int32_t y,float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMMAND_INT_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_int32_t(buf, 16, x); + _mav_put_int32_t(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, command); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 32, frame); + _mav_put_uint8_t(buf, 33, current); + _mav_put_uint8_t(buf, 34, autocontinue); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#else + mavlink_command_int_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.current = current; + packet.autocontinue = autocontinue; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMMAND_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_INT_LEN, MAVLINK_MSG_ID_COMMAND_INT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#endif +} + +/** + * @brief Encode a command_int struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param command_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_command_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_command_int_t* command_int) +{ + return mavlink_msg_command_int_pack(system_id, component_id, msg, command_int->target_system, command_int->target_component, command_int->frame, command_int->command, command_int->current, command_int->autocontinue, command_int->param1, command_int->param2, command_int->param3, command_int->param4, command_int->x, command_int->y, command_int->z); +} + +/** + * @brief Encode a command_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param command_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_command_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_command_int_t* command_int) +{ + return mavlink_msg_command_int_pack_chan(system_id, component_id, chan, msg, command_int->target_system, command_int->target_component, command_int->frame, command_int->command, command_int->current, command_int->autocontinue, command_int->param1, command_int->param2, command_int->param3, command_int->param4, command_int->x, command_int->y, command_int->z); +} + +/** + * @brief Send a command_int message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param frame The coordinate system of the COMMAND. see MAV_FRAME in mavlink_types.h + * @param command The scheduled action for the mission item. see MAV_CMD in common.xml MAVLink specs + * @param current false:0, true:1 + * @param autocontinue autocontinue to next wp + * @param param1 PARAM1, see MAV_CMD enum + * @param param2 PARAM2, see MAV_CMD enum + * @param param3 PARAM3, see MAV_CMD enum + * @param param4 PARAM4, see MAV_CMD enum + * @param x PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + * @param y PARAM6 / local: y position in meters * 1e4, global: longitude in degrees * 10^7 + * @param z PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_command_int_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMMAND_INT_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_int32_t(buf, 16, x); + _mav_put_int32_t(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, command); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 32, frame); + _mav_put_uint8_t(buf, 33, current); + _mav_put_uint8_t(buf, 34, autocontinue); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_INT, buf, MAVLINK_MSG_ID_COMMAND_INT_LEN, MAVLINK_MSG_ID_COMMAND_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_INT, buf, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#endif +#else + mavlink_command_int_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.current = current; + packet.autocontinue = autocontinue; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_INT, (const char *)&packet, MAVLINK_MSG_ID_COMMAND_INT_LEN, MAVLINK_MSG_ID_COMMAND_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_INT, (const char *)&packet, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_COMMAND_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_command_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_int32_t(buf, 16, x); + _mav_put_int32_t(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, command); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 32, frame); + _mav_put_uint8_t(buf, 33, current); + _mav_put_uint8_t(buf, 34, autocontinue); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_INT, buf, MAVLINK_MSG_ID_COMMAND_INT_LEN, MAVLINK_MSG_ID_COMMAND_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_INT, buf, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#endif +#else + mavlink_command_int_t *packet = (mavlink_command_int_t *)msgbuf; + packet->param1 = param1; + packet->param2 = param2; + packet->param3 = param3; + packet->param4 = param4; + packet->x = x; + packet->y = y; + packet->z = z; + packet->command = command; + packet->target_system = target_system; + packet->target_component = target_component; + packet->frame = frame; + packet->current = current; + packet->autocontinue = autocontinue; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_INT, (const char *)packet, MAVLINK_MSG_ID_COMMAND_INT_LEN, MAVLINK_MSG_ID_COMMAND_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_INT, (const char *)packet, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE COMMAND_INT UNPACKING + + +/** + * @brief Get field target_system from command_int message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_command_int_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 30); +} + +/** + * @brief Get field target_component from command_int message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_command_int_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 31); +} + +/** + * @brief Get field frame from command_int message + * + * @return The coordinate system of the COMMAND. see MAV_FRAME in mavlink_types.h + */ +static inline uint8_t mavlink_msg_command_int_get_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field command from command_int message + * + * @return The scheduled action for the mission item. see MAV_CMD in common.xml MAVLink specs + */ +static inline uint16_t mavlink_msg_command_int_get_command(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field current from command_int message + * + * @return false:0, true:1 + */ +static inline uint8_t mavlink_msg_command_int_get_current(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 33); +} + +/** + * @brief Get field autocontinue from command_int message + * + * @return autocontinue to next wp + */ +static inline uint8_t mavlink_msg_command_int_get_autocontinue(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field param1 from command_int message + * + * @return PARAM1, see MAV_CMD enum + */ +static inline float mavlink_msg_command_int_get_param1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field param2 from command_int message + * + * @return PARAM2, see MAV_CMD enum + */ +static inline float mavlink_msg_command_int_get_param2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field param3 from command_int message + * + * @return PARAM3, see MAV_CMD enum + */ +static inline float mavlink_msg_command_int_get_param3(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field param4 from command_int message + * + * @return PARAM4, see MAV_CMD enum + */ +static inline float mavlink_msg_command_int_get_param4(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field x from command_int message + * + * @return PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + */ +static inline int32_t mavlink_msg_command_int_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field y from command_int message + * + * @return PARAM6 / local: y position in meters * 1e4, global: longitude in degrees * 10^7 + */ +static inline int32_t mavlink_msg_command_int_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 20); +} + +/** + * @brief Get field z from command_int message + * + * @return PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + */ +static inline float mavlink_msg_command_int_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Decode a command_int message into a struct + * + * @param msg The message to decode + * @param command_int C-struct to decode the message contents into + */ +static inline void mavlink_msg_command_int_decode(const mavlink_message_t* msg, mavlink_command_int_t* command_int) +{ +#if MAVLINK_NEED_BYTE_SWAP + command_int->param1 = mavlink_msg_command_int_get_param1(msg); + command_int->param2 = mavlink_msg_command_int_get_param2(msg); + command_int->param3 = mavlink_msg_command_int_get_param3(msg); + command_int->param4 = mavlink_msg_command_int_get_param4(msg); + command_int->x = mavlink_msg_command_int_get_x(msg); + command_int->y = mavlink_msg_command_int_get_y(msg); + command_int->z = mavlink_msg_command_int_get_z(msg); + command_int->command = mavlink_msg_command_int_get_command(msg); + command_int->target_system = mavlink_msg_command_int_get_target_system(msg); + command_int->target_component = mavlink_msg_command_int_get_target_component(msg); + command_int->frame = mavlink_msg_command_int_get_frame(msg); + command_int->current = mavlink_msg_command_int_get_current(msg); + command_int->autocontinue = mavlink_msg_command_int_get_autocontinue(msg); +#else + memcpy(command_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_COMMAND_INT_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h index 8f705c0..161896b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h @@ -256,6 +256,58 @@ static inline void mavlink_msg_command_long_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_COMMAND_LONG_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_command_long_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, param5); + _mav_put_float(buf, 20, param6); + _mav_put_float(buf, 24, param7); + _mav_put_uint16_t(buf, 28, command); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 32, confirmation); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN); +#endif +#else + mavlink_command_long_t *packet = (mavlink_command_long_t *)msgbuf; + packet->param1 = param1; + packet->param2 = param2; + packet->param3 = param3; + packet->param4 = param4; + packet->param5 = param5; + packet->param6 = param6; + packet->param7 = param7; + packet->command = command; + packet->target_system = target_system; + packet->target_component = target_component; + packet->confirmation = confirmation; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, (const char *)packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, (const char *)packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE COMMAND_LONG UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h index dc0768e..640ffeb 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_data_stream_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_DATA_STREAM_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_data_stream_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t stream_id, uint16_t message_rate, uint8_t on_off) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, message_rate); + _mav_put_uint8_t(buf, 2, stream_id); + _mav_put_uint8_t(buf, 3, on_off); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, buf, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, buf, MAVLINK_MSG_ID_DATA_STREAM_LEN); +#endif +#else + mavlink_data_stream_t *packet = (mavlink_data_stream_t *)msgbuf; + packet->message_rate = message_rate; + packet->stream_id = stream_id; + packet->on_off = on_off; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_DATA_STREAM_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DATA_STREAM UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h index fdd8fdd..d84a737 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_ #endif } +#if MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_data_transmission_handshake_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint32_t size, uint16_t width, uint16_t height, uint16_t packets, uint8_t payload, uint8_t jpg_quality) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, size); + _mav_put_uint16_t(buf, 4, width); + _mav_put_uint16_t(buf, 6, height); + _mav_put_uint16_t(buf, 8, packets); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, payload); + _mav_put_uint8_t(buf, 12, jpg_quality); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); +#endif +#else + mavlink_data_transmission_handshake_t *packet = (mavlink_data_transmission_handshake_t *)msgbuf; + packet->size = size; + packet->width = width; + packet->height = height; + packet->packets = packets; + packet->type = type; + packet->payload = payload; + packet->jpg_quality = jpg_quality; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, (const char *)packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, (const char *)packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DATA_TRANSMISSION_HANDSHAKE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h index 9a6ed87..2102af8 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint32_t time_ #endif } +#if MAVLINK_MSG_ID_DEBUG_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_debug_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t ind, float value) +{ +#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, value); + _mav_put_uint8_t(buf, 8, ind); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, MAVLINK_MSG_ID_DEBUG_LEN); +#endif +#else + mavlink_debug_t *packet = (mavlink_debug_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->value = value; + packet->ind = ind; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)packet, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)packet, MAVLINK_MSG_ID_DEBUG_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DEBUG UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h index 6cfc752..67c339e 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h @@ -184,6 +184,44 @@ static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const cha #endif } +#if MAVLINK_MSG_ID_DEBUG_VECT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_debug_vect_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *name, uint64_t time_usec, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_char_array(buf, 20, name, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN); +#endif +#else + mavlink_debug_vect_t *packet = (mavlink_debug_vect_t *)msgbuf; + packet->time_usec = time_usec; + packet->x = x; + packet->y = y; + packet->z = z; + mav_array_memcpy(packet->name, name, sizeof(char)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, (const char *)packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, (const char *)packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DEBUG_VECT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h new file mode 100644 index 0000000..8743b64 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h @@ -0,0 +1,377 @@ +// MESSAGE DISTANCE_SENSOR PACKING + +#define MAVLINK_MSG_ID_DISTANCE_SENSOR 132 + +typedef struct __mavlink_distance_sensor_t +{ + uint32_t time_boot_ms; ///< Time since system boot + uint16_t min_distance; ///< Minimum distance the sensor can measure in centimeters + uint16_t max_distance; ///< Maximum distance the sensor can measure in centimeters + uint16_t current_distance; ///< Current distance reading + uint8_t type; ///< Type from MAV_DISTANCE_SENSOR enum. + uint8_t id; ///< Onboard ID of the sensor + uint8_t orientation; ///< Direction the sensor faces from FIXME enum. + uint8_t covariance; ///< Measurement covariance in centimeters, 0 for unknown / invalid readings +} mavlink_distance_sensor_t; + +#define MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN 14 +#define MAVLINK_MSG_ID_132_LEN 14 + +#define MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC 85 +#define MAVLINK_MSG_ID_132_CRC 85 + + + +#define MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR { \ + "DISTANCE_SENSOR", \ + 8, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_distance_sensor_t, time_boot_ms) }, \ + { "min_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_distance_sensor_t, min_distance) }, \ + { "max_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_distance_sensor_t, max_distance) }, \ + { "current_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_distance_sensor_t, current_distance) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_distance_sensor_t, type) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_distance_sensor_t, id) }, \ + { "orientation", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_distance_sensor_t, orientation) }, \ + { "covariance", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_distance_sensor_t, covariance) }, \ + } \ +} + + +/** + * @brief Pack a distance_sensor message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Time since system boot + * @param min_distance Minimum distance the sensor can measure in centimeters + * @param max_distance Maximum distance the sensor can measure in centimeters + * @param current_distance Current distance reading + * @param type Type from MAV_DISTANCE_SENSOR enum. + * @param id Onboard ID of the sensor + * @param orientation Direction the sensor faces from FIXME enum. + * @param covariance Measurement covariance in centimeters, 0 for unknown / invalid readings + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_distance_sensor_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, min_distance); + _mav_put_uint16_t(buf, 6, max_distance); + _mav_put_uint16_t(buf, 8, current_distance); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, id); + _mav_put_uint8_t(buf, 12, orientation); + _mav_put_uint8_t(buf, 13, covariance); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#else + mavlink_distance_sensor_t packet; + packet.time_boot_ms = time_boot_ms; + packet.min_distance = min_distance; + packet.max_distance = max_distance; + packet.current_distance = current_distance; + packet.type = type; + packet.id = id; + packet.orientation = orientation; + packet.covariance = covariance; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DISTANCE_SENSOR; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +} + +/** + * @brief Pack a distance_sensor message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Time since system boot + * @param min_distance Minimum distance the sensor can measure in centimeters + * @param max_distance Maximum distance the sensor can measure in centimeters + * @param current_distance Current distance reading + * @param type Type from MAV_DISTANCE_SENSOR enum. + * @param id Onboard ID of the sensor + * @param orientation Direction the sensor faces from FIXME enum. + * @param covariance Measurement covariance in centimeters, 0 for unknown / invalid readings + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_distance_sensor_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint16_t min_distance,uint16_t max_distance,uint16_t current_distance,uint8_t type,uint8_t id,uint8_t orientation,uint8_t covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, min_distance); + _mav_put_uint16_t(buf, 6, max_distance); + _mav_put_uint16_t(buf, 8, current_distance); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, id); + _mav_put_uint8_t(buf, 12, orientation); + _mav_put_uint8_t(buf, 13, covariance); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#else + mavlink_distance_sensor_t packet; + packet.time_boot_ms = time_boot_ms; + packet.min_distance = min_distance; + packet.max_distance = max_distance; + packet.current_distance = current_distance; + packet.type = type; + packet.id = id; + packet.orientation = orientation; + packet.covariance = covariance; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DISTANCE_SENSOR; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +} + +/** + * @brief Encode a distance_sensor struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param distance_sensor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_distance_sensor_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_distance_sensor_t* distance_sensor) +{ + return mavlink_msg_distance_sensor_pack(system_id, component_id, msg, distance_sensor->time_boot_ms, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->covariance); +} + +/** + * @brief Encode a distance_sensor struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param distance_sensor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_distance_sensor_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_distance_sensor_t* distance_sensor) +{ + return mavlink_msg_distance_sensor_pack_chan(system_id, component_id, chan, msg, distance_sensor->time_boot_ms, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->covariance); +} + +/** + * @brief Send a distance_sensor message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Time since system boot + * @param min_distance Minimum distance the sensor can measure in centimeters + * @param max_distance Maximum distance the sensor can measure in centimeters + * @param current_distance Current distance reading + * @param type Type from MAV_DISTANCE_SENSOR enum. + * @param id Onboard ID of the sensor + * @param orientation Direction the sensor faces from FIXME enum. + * @param covariance Measurement covariance in centimeters, 0 for unknown / invalid readings + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_distance_sensor_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, min_distance); + _mav_put_uint16_t(buf, 6, max_distance); + _mav_put_uint16_t(buf, 8, current_distance); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, id); + _mav_put_uint8_t(buf, 12, orientation); + _mav_put_uint8_t(buf, 13, covariance); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +#else + mavlink_distance_sensor_t packet; + packet.time_boot_ms = time_boot_ms; + packet.min_distance = min_distance; + packet.max_distance = max_distance; + packet.current_distance = current_distance; + packet.type = type; + packet.id = id; + packet.orientation = orientation; + packet.covariance = covariance; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_distance_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance) +{ +#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, min_distance); + _mav_put_uint16_t(buf, 6, max_distance); + _mav_put_uint16_t(buf, 8, current_distance); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, id); + _mav_put_uint8_t(buf, 12, orientation); + _mav_put_uint8_t(buf, 13, covariance); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +#else + mavlink_distance_sensor_t *packet = (mavlink_distance_sensor_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->min_distance = min_distance; + packet->max_distance = max_distance; + packet->current_distance = current_distance; + packet->type = type; + packet->id = id; + packet->orientation = orientation; + packet->covariance = covariance; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE DISTANCE_SENSOR UNPACKING + + +/** + * @brief Get field time_boot_ms from distance_sensor message + * + * @return Time since system boot + */ +static inline uint32_t mavlink_msg_distance_sensor_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field min_distance from distance_sensor message + * + * @return Minimum distance the sensor can measure in centimeters + */ +static inline uint16_t mavlink_msg_distance_sensor_get_min_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field max_distance from distance_sensor message + * + * @return Maximum distance the sensor can measure in centimeters + */ +static inline uint16_t mavlink_msg_distance_sensor_get_max_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Get field current_distance from distance_sensor message + * + * @return Current distance reading + */ +static inline uint16_t mavlink_msg_distance_sensor_get_current_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field type from distance_sensor message + * + * @return Type from MAV_DISTANCE_SENSOR enum. + */ +static inline uint8_t mavlink_msg_distance_sensor_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 10); +} + +/** + * @brief Get field id from distance_sensor message + * + * @return Onboard ID of the sensor + */ +static inline uint8_t mavlink_msg_distance_sensor_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 11); +} + +/** + * @brief Get field orientation from distance_sensor message + * + * @return Direction the sensor faces from FIXME enum. + */ +static inline uint8_t mavlink_msg_distance_sensor_get_orientation(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 12); +} + +/** + * @brief Get field covariance from distance_sensor message + * + * @return Measurement covariance in centimeters, 0 for unknown / invalid readings + */ +static inline uint8_t mavlink_msg_distance_sensor_get_covariance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 13); +} + +/** + * @brief Decode a distance_sensor message into a struct + * + * @param msg The message to decode + * @param distance_sensor C-struct to decode the message contents into + */ +static inline void mavlink_msg_distance_sensor_decode(const mavlink_message_t* msg, mavlink_distance_sensor_t* distance_sensor) +{ +#if MAVLINK_NEED_BYTE_SWAP + distance_sensor->time_boot_ms = mavlink_msg_distance_sensor_get_time_boot_ms(msg); + distance_sensor->min_distance = mavlink_msg_distance_sensor_get_min_distance(msg); + distance_sensor->max_distance = mavlink_msg_distance_sensor_get_max_distance(msg); + distance_sensor->current_distance = mavlink_msg_distance_sensor_get_current_distance(msg); + distance_sensor->type = mavlink_msg_distance_sensor_get_type(msg); + distance_sensor->id = mavlink_msg_distance_sensor_get_id(msg); + distance_sensor->orientation = mavlink_msg_distance_sensor_get_orientation(msg); + distance_sensor->covariance = mavlink_msg_distance_sensor_get_covariance(msg); +#else + memcpy(distance_sensor, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h index 5900ea8..dacd7c9 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h @@ -151,6 +151,38 @@ static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_encapsulated_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t seqnr, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, seqnr); + _mav_put_uint8_t_array(buf, 2, data, 253); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); +#endif +#else + mavlink_encapsulated_data_t *packet = (mavlink_encapsulated_data_t *)msgbuf; + packet->seqnr = seqnr; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*253); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE ENCAPSULATED_DATA UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h deleted file mode 100644 index 4f31698..0000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h +++ /dev/null @@ -1,215 +0,0 @@ -// MESSAGE FILE_TRANSFER_DIR_LIST PACKING - -#define MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST 111 - -typedef struct __mavlink_file_transfer_dir_list_t -{ - uint64_t transfer_uid; ///< Unique transfer ID - char dir_path[240]; ///< Directory path to list - uint8_t flags; ///< RESERVED -} mavlink_file_transfer_dir_list_t; - -#define MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN 249 -#define MAVLINK_MSG_ID_111_LEN 249 - -#define MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_CRC 93 -#define MAVLINK_MSG_ID_111_CRC 93 - -#define MAVLINK_MSG_FILE_TRANSFER_DIR_LIST_FIELD_DIR_PATH_LEN 240 - -#define MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST { \ - "FILE_TRANSFER_DIR_LIST", \ - 3, \ - { { "transfer_uid", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_file_transfer_dir_list_t, transfer_uid) }, \ - { "dir_path", NULL, MAVLINK_TYPE_CHAR, 240, 8, offsetof(mavlink_file_transfer_dir_list_t, dir_path) }, \ - { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 248, offsetof(mavlink_file_transfer_dir_list_t, flags) }, \ - } \ -} - - -/** - * @brief Pack a file_transfer_dir_list message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param transfer_uid Unique transfer ID - * @param dir_path Directory path to list - * @param flags RESERVED - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_file_transfer_dir_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t transfer_uid, const char *dir_path, uint8_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN]; - _mav_put_uint64_t(buf, 0, transfer_uid); - _mav_put_uint8_t(buf, 248, flags); - _mav_put_char_array(buf, 8, dir_path, 240); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); -#else - mavlink_file_transfer_dir_list_t packet; - packet.transfer_uid = transfer_uid; - packet.flags = flags; - mav_array_memcpy(packet.dir_path, dir_path, sizeof(char)*240); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); -#endif -} - -/** - * @brief Pack a file_transfer_dir_list message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param transfer_uid Unique transfer ID - * @param dir_path Directory path to list - * @param flags RESERVED - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_file_transfer_dir_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t transfer_uid,const char *dir_path,uint8_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN]; - _mav_put_uint64_t(buf, 0, transfer_uid); - _mav_put_uint8_t(buf, 248, flags); - _mav_put_char_array(buf, 8, dir_path, 240); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); -#else - mavlink_file_transfer_dir_list_t packet; - packet.transfer_uid = transfer_uid; - packet.flags = flags; - mav_array_memcpy(packet.dir_path, dir_path, sizeof(char)*240); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); -#endif -} - -/** - * @brief Encode a file_transfer_dir_list struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param file_transfer_dir_list C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_file_transfer_dir_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_file_transfer_dir_list_t* file_transfer_dir_list) -{ - return mavlink_msg_file_transfer_dir_list_pack(system_id, component_id, msg, file_transfer_dir_list->transfer_uid, file_transfer_dir_list->dir_path, file_transfer_dir_list->flags); -} - -/** - * @brief Encode a file_transfer_dir_list struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param file_transfer_dir_list C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_file_transfer_dir_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_file_transfer_dir_list_t* file_transfer_dir_list) -{ - return mavlink_msg_file_transfer_dir_list_pack_chan(system_id, component_id, chan, msg, file_transfer_dir_list->transfer_uid, file_transfer_dir_list->dir_path, file_transfer_dir_list->flags); -} - -/** - * @brief Send a file_transfer_dir_list message - * @param chan MAVLink channel to send the message - * - * @param transfer_uid Unique transfer ID - * @param dir_path Directory path to list - * @param flags RESERVED - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_file_transfer_dir_list_send(mavlink_channel_t chan, uint64_t transfer_uid, const char *dir_path, uint8_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN]; - _mav_put_uint64_t(buf, 0, transfer_uid); - _mav_put_uint8_t(buf, 248, flags); - _mav_put_char_array(buf, 8, dir_path, 240); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, buf, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, buf, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); -#endif -#else - mavlink_file_transfer_dir_list_t packet; - packet.transfer_uid = transfer_uid; - packet.flags = flags; - mav_array_memcpy(packet.dir_path, dir_path, sizeof(char)*240); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, (const char *)&packet, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, (const char *)&packet, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); -#endif -#endif -} - -#endif - -// MESSAGE FILE_TRANSFER_DIR_LIST UNPACKING - - -/** - * @brief Get field transfer_uid from file_transfer_dir_list message - * - * @return Unique transfer ID - */ -static inline uint64_t mavlink_msg_file_transfer_dir_list_get_transfer_uid(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field dir_path from file_transfer_dir_list message - * - * @return Directory path to list - */ -static inline uint16_t mavlink_msg_file_transfer_dir_list_get_dir_path(const mavlink_message_t* msg, char *dir_path) -{ - return _MAV_RETURN_char_array(msg, dir_path, 240, 8); -} - -/** - * @brief Get field flags from file_transfer_dir_list message - * - * @return RESERVED - */ -static inline uint8_t mavlink_msg_file_transfer_dir_list_get_flags(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 248); -} - -/** - * @brief Decode a file_transfer_dir_list message into a struct - * - * @param msg The message to decode - * @param file_transfer_dir_list C-struct to decode the message contents into - */ -static inline void mavlink_msg_file_transfer_dir_list_decode(const mavlink_message_t* msg, mavlink_file_transfer_dir_list_t* file_transfer_dir_list) -{ -#if MAVLINK_NEED_BYTE_SWAP - file_transfer_dir_list->transfer_uid = mavlink_msg_file_transfer_dir_list_get_transfer_uid(msg); - mavlink_msg_file_transfer_dir_list_get_dir_path(msg, file_transfer_dir_list->dir_path); - file_transfer_dir_list->flags = mavlink_msg_file_transfer_dir_list_get_flags(msg); -#else - memcpy(file_transfer_dir_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_protocol.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_protocol.h new file mode 100644 index 0000000..f77cd94 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_protocol.h @@ -0,0 +1,273 @@ +// MESSAGE FILE_TRANSFER_PROTOCOL PACKING + +#define MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL 110 + +typedef struct __mavlink_file_transfer_protocol_t +{ + uint8_t target_network; ///< Network ID (0 for broadcast) + uint8_t target_system; ///< System ID (0 for broadcast) + uint8_t target_component; ///< Component ID (0 for broadcast) + uint8_t payload[251]; ///< Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. +} mavlink_file_transfer_protocol_t; + +#define MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN 254 +#define MAVLINK_MSG_ID_110_LEN 254 + +#define MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_CRC 84 +#define MAVLINK_MSG_ID_110_CRC 84 + +#define MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN 251 + +#define MAVLINK_MESSAGE_INFO_FILE_TRANSFER_PROTOCOL { \ + "FILE_TRANSFER_PROTOCOL", \ + 4, \ + { { "target_network", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_file_transfer_protocol_t, target_network) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_file_transfer_protocol_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_file_transfer_protocol_t, target_component) }, \ + { "payload", NULL, MAVLINK_TYPE_UINT8_T, 251, 3, offsetof(mavlink_file_transfer_protocol_t, payload) }, \ + } \ +} + + +/** + * @brief Pack a file_transfer_protocol message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_network Network ID (0 for broadcast) + * @param target_system System ID (0 for broadcast) + * @param target_component Component ID (0 for broadcast) + * @param payload Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_file_transfer_protocol_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_network, uint8_t target_system, uint8_t target_component, const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN]; + _mav_put_uint8_t(buf, 0, target_network); + _mav_put_uint8_t(buf, 1, target_system); + _mav_put_uint8_t(buf, 2, target_component); + _mav_put_uint8_t_array(buf, 3, payload, 251); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#else + mavlink_file_transfer_protocol_t packet; + packet.target_network = target_network; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*251); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#endif +} + +/** + * @brief Pack a file_transfer_protocol message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_network Network ID (0 for broadcast) + * @param target_system System ID (0 for broadcast) + * @param target_component Component ID (0 for broadcast) + * @param payload Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_file_transfer_protocol_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_network,uint8_t target_system,uint8_t target_component,const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN]; + _mav_put_uint8_t(buf, 0, target_network); + _mav_put_uint8_t(buf, 1, target_system); + _mav_put_uint8_t(buf, 2, target_component); + _mav_put_uint8_t_array(buf, 3, payload, 251); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#else + mavlink_file_transfer_protocol_t packet; + packet.target_network = target_network; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*251); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#endif +} + +/** + * @brief Encode a file_transfer_protocol struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param file_transfer_protocol C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_file_transfer_protocol_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_file_transfer_protocol_t* file_transfer_protocol) +{ + return mavlink_msg_file_transfer_protocol_pack(system_id, component_id, msg, file_transfer_protocol->target_network, file_transfer_protocol->target_system, file_transfer_protocol->target_component, file_transfer_protocol->payload); +} + +/** + * @brief Encode a file_transfer_protocol struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param file_transfer_protocol C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_file_transfer_protocol_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_file_transfer_protocol_t* file_transfer_protocol) +{ + return mavlink_msg_file_transfer_protocol_pack_chan(system_id, component_id, chan, msg, file_transfer_protocol->target_network, file_transfer_protocol->target_system, file_transfer_protocol->target_component, file_transfer_protocol->payload); +} + +/** + * @brief Send a file_transfer_protocol message + * @param chan MAVLink channel to send the message + * + * @param target_network Network ID (0 for broadcast) + * @param target_system System ID (0 for broadcast) + * @param target_component Component ID (0 for broadcast) + * @param payload Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_file_transfer_protocol_send(mavlink_channel_t chan, uint8_t target_network, uint8_t target_system, uint8_t target_component, const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN]; + _mav_put_uint8_t(buf, 0, target_network); + _mav_put_uint8_t(buf, 1, target_system); + _mav_put_uint8_t(buf, 2, target_component); + _mav_put_uint8_t_array(buf, 3, payload, 251); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, buf, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, buf, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#endif +#else + mavlink_file_transfer_protocol_t packet; + packet.target_network = target_network; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*251); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, (const char *)&packet, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, (const char *)&packet, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_file_transfer_protocol_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_network, uint8_t target_system, uint8_t target_component, const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_network); + _mav_put_uint8_t(buf, 1, target_system); + _mav_put_uint8_t(buf, 2, target_component); + _mav_put_uint8_t_array(buf, 3, payload, 251); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, buf, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, buf, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#endif +#else + mavlink_file_transfer_protocol_t *packet = (mavlink_file_transfer_protocol_t *)msgbuf; + packet->target_network = target_network; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->payload, payload, sizeof(uint8_t)*251); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE FILE_TRANSFER_PROTOCOL UNPACKING + + +/** + * @brief Get field target_network from file_transfer_protocol message + * + * @return Network ID (0 for broadcast) + */ +static inline uint8_t mavlink_msg_file_transfer_protocol_get_target_network(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_system from file_transfer_protocol message + * + * @return System ID (0 for broadcast) + */ +static inline uint8_t mavlink_msg_file_transfer_protocol_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field target_component from file_transfer_protocol message + * + * @return Component ID (0 for broadcast) + */ +static inline uint8_t mavlink_msg_file_transfer_protocol_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field payload from file_transfer_protocol message + * + * @return Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + */ +static inline uint16_t mavlink_msg_file_transfer_protocol_get_payload(const mavlink_message_t* msg, uint8_t *payload) +{ + return _MAV_RETURN_uint8_t_array(msg, payload, 251, 3); +} + +/** + * @brief Decode a file_transfer_protocol message into a struct + * + * @param msg The message to decode + * @param file_transfer_protocol C-struct to decode the message contents into + */ +static inline void mavlink_msg_file_transfer_protocol_decode(const mavlink_message_t* msg, mavlink_file_transfer_protocol_t* file_transfer_protocol) +{ +#if MAVLINK_NEED_BYTE_SWAP + file_transfer_protocol->target_network = mavlink_msg_file_transfer_protocol_get_target_network(msg); + file_transfer_protocol->target_system = mavlink_msg_file_transfer_protocol_get_target_system(msg); + file_transfer_protocol->target_component = mavlink_msg_file_transfer_protocol_get_target_component(msg); + mavlink_msg_file_transfer_protocol_get_payload(msg, file_transfer_protocol->payload); +#else + memcpy(file_transfer_protocol, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h deleted file mode 100644 index fc6247f..0000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h +++ /dev/null @@ -1,199 +0,0 @@ -// MESSAGE FILE_TRANSFER_RES PACKING - -#define MAVLINK_MSG_ID_FILE_TRANSFER_RES 112 - -typedef struct __mavlink_file_transfer_res_t -{ - uint64_t transfer_uid; ///< Unique transfer ID - uint8_t result; ///< 0: OK, 1: not permitted, 2: bad path / file name, 3: no space left on device -} mavlink_file_transfer_res_t; - -#define MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN 9 -#define MAVLINK_MSG_ID_112_LEN 9 - -#define MAVLINK_MSG_ID_FILE_TRANSFER_RES_CRC 124 -#define MAVLINK_MSG_ID_112_CRC 124 - - - -#define MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES { \ - "FILE_TRANSFER_RES", \ - 2, \ - { { "transfer_uid", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_file_transfer_res_t, transfer_uid) }, \ - { "result", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_file_transfer_res_t, result) }, \ - } \ -} - - -/** - * @brief Pack a file_transfer_res message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param transfer_uid Unique transfer ID - * @param result 0: OK, 1: not permitted, 2: bad path / file name, 3: no space left on device - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_file_transfer_res_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t transfer_uid, uint8_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN]; - _mav_put_uint64_t(buf, 0, transfer_uid); - _mav_put_uint8_t(buf, 8, result); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); -#else - mavlink_file_transfer_res_t packet; - packet.transfer_uid = transfer_uid; - packet.result = result; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_RES; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_RES_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); -#endif -} - -/** - * @brief Pack a file_transfer_res message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param transfer_uid Unique transfer ID - * @param result 0: OK, 1: not permitted, 2: bad path / file name, 3: no space left on device - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_file_transfer_res_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t transfer_uid,uint8_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN]; - _mav_put_uint64_t(buf, 0, transfer_uid); - _mav_put_uint8_t(buf, 8, result); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); -#else - mavlink_file_transfer_res_t packet; - packet.transfer_uid = transfer_uid; - packet.result = result; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_RES; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_RES_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); -#endif -} - -/** - * @brief Encode a file_transfer_res struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param file_transfer_res C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_file_transfer_res_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_file_transfer_res_t* file_transfer_res) -{ - return mavlink_msg_file_transfer_res_pack(system_id, component_id, msg, file_transfer_res->transfer_uid, file_transfer_res->result); -} - -/** - * @brief Encode a file_transfer_res struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param file_transfer_res C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_file_transfer_res_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_file_transfer_res_t* file_transfer_res) -{ - return mavlink_msg_file_transfer_res_pack_chan(system_id, component_id, chan, msg, file_transfer_res->transfer_uid, file_transfer_res->result); -} - -/** - * @brief Send a file_transfer_res message - * @param chan MAVLink channel to send the message - * - * @param transfer_uid Unique transfer ID - * @param result 0: OK, 1: not permitted, 2: bad path / file name, 3: no space left on device - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_file_transfer_res_send(mavlink_channel_t chan, uint64_t transfer_uid, uint8_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN]; - _mav_put_uint64_t(buf, 0, transfer_uid); - _mav_put_uint8_t(buf, 8, result); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, buf, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_RES_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, buf, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); -#endif -#else - mavlink_file_transfer_res_t packet; - packet.transfer_uid = transfer_uid; - packet.result = result; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, (const char *)&packet, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_RES_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, (const char *)&packet, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); -#endif -#endif -} - -#endif - -// MESSAGE FILE_TRANSFER_RES UNPACKING - - -/** - * @brief Get field transfer_uid from file_transfer_res message - * - * @return Unique transfer ID - */ -static inline uint64_t mavlink_msg_file_transfer_res_get_transfer_uid(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field result from file_transfer_res message - * - * @return 0: OK, 1: not permitted, 2: bad path / file name, 3: no space left on device - */ -static inline uint8_t mavlink_msg_file_transfer_res_get_result(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 8); -} - -/** - * @brief Decode a file_transfer_res message into a struct - * - * @param msg The message to decode - * @param file_transfer_res C-struct to decode the message contents into - */ -static inline void mavlink_msg_file_transfer_res_decode(const mavlink_message_t* msg, mavlink_file_transfer_res_t* file_transfer_res) -{ -#if MAVLINK_NEED_BYTE_SWAP - file_transfer_res->transfer_uid = mavlink_msg_file_transfer_res_get_transfer_uid(msg); - file_transfer_res->result = mavlink_msg_file_transfer_res_get_result(msg); -#else - memcpy(file_transfer_res, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h deleted file mode 100644 index 05be773..0000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h +++ /dev/null @@ -1,259 +0,0 @@ -// MESSAGE FILE_TRANSFER_START PACKING - -#define MAVLINK_MSG_ID_FILE_TRANSFER_START 110 - -typedef struct __mavlink_file_transfer_start_t -{ - uint64_t transfer_uid; ///< Unique transfer ID - uint32_t file_size; ///< File size in bytes - char dest_path[240]; ///< Destination path - uint8_t direction; ///< Transfer direction: 0: from requester, 1: to requester - uint8_t flags; ///< RESERVED -} mavlink_file_transfer_start_t; - -#define MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN 254 -#define MAVLINK_MSG_ID_110_LEN 254 - -#define MAVLINK_MSG_ID_FILE_TRANSFER_START_CRC 235 -#define MAVLINK_MSG_ID_110_CRC 235 - -#define MAVLINK_MSG_FILE_TRANSFER_START_FIELD_DEST_PATH_LEN 240 - -#define MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START { \ - "FILE_TRANSFER_START", \ - 5, \ - { { "transfer_uid", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_file_transfer_start_t, transfer_uid) }, \ - { "file_size", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_file_transfer_start_t, file_size) }, \ - { "dest_path", NULL, MAVLINK_TYPE_CHAR, 240, 12, offsetof(mavlink_file_transfer_start_t, dest_path) }, \ - { "direction", NULL, MAVLINK_TYPE_UINT8_T, 0, 252, offsetof(mavlink_file_transfer_start_t, direction) }, \ - { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 253, offsetof(mavlink_file_transfer_start_t, flags) }, \ - } \ -} - - -/** - * @brief Pack a file_transfer_start message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param transfer_uid Unique transfer ID - * @param dest_path Destination path - * @param direction Transfer direction: 0: from requester, 1: to requester - * @param file_size File size in bytes - * @param flags RESERVED - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_file_transfer_start_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t transfer_uid, const char *dest_path, uint8_t direction, uint32_t file_size, uint8_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN]; - _mav_put_uint64_t(buf, 0, transfer_uid); - _mav_put_uint32_t(buf, 8, file_size); - _mav_put_uint8_t(buf, 252, direction); - _mav_put_uint8_t(buf, 253, flags); - _mav_put_char_array(buf, 12, dest_path, 240); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); -#else - mavlink_file_transfer_start_t packet; - packet.transfer_uid = transfer_uid; - packet.file_size = file_size; - packet.direction = direction; - packet.flags = flags; - mav_array_memcpy(packet.dest_path, dest_path, sizeof(char)*240); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_START; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_START_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); -#endif -} - -/** - * @brief Pack a file_transfer_start message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param transfer_uid Unique transfer ID - * @param dest_path Destination path - * @param direction Transfer direction: 0: from requester, 1: to requester - * @param file_size File size in bytes - * @param flags RESERVED - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_file_transfer_start_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t transfer_uid,const char *dest_path,uint8_t direction,uint32_t file_size,uint8_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN]; - _mav_put_uint64_t(buf, 0, transfer_uid); - _mav_put_uint32_t(buf, 8, file_size); - _mav_put_uint8_t(buf, 252, direction); - _mav_put_uint8_t(buf, 253, flags); - _mav_put_char_array(buf, 12, dest_path, 240); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); -#else - mavlink_file_transfer_start_t packet; - packet.transfer_uid = transfer_uid; - packet.file_size = file_size; - packet.direction = direction; - packet.flags = flags; - mav_array_memcpy(packet.dest_path, dest_path, sizeof(char)*240); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_START; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_START_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); -#endif -} - -/** - * @brief Encode a file_transfer_start struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param file_transfer_start C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_file_transfer_start_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_file_transfer_start_t* file_transfer_start) -{ - return mavlink_msg_file_transfer_start_pack(system_id, component_id, msg, file_transfer_start->transfer_uid, file_transfer_start->dest_path, file_transfer_start->direction, file_transfer_start->file_size, file_transfer_start->flags); -} - -/** - * @brief Encode a file_transfer_start struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param file_transfer_start C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_file_transfer_start_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_file_transfer_start_t* file_transfer_start) -{ - return mavlink_msg_file_transfer_start_pack_chan(system_id, component_id, chan, msg, file_transfer_start->transfer_uid, file_transfer_start->dest_path, file_transfer_start->direction, file_transfer_start->file_size, file_transfer_start->flags); -} - -/** - * @brief Send a file_transfer_start message - * @param chan MAVLink channel to send the message - * - * @param transfer_uid Unique transfer ID - * @param dest_path Destination path - * @param direction Transfer direction: 0: from requester, 1: to requester - * @param file_size File size in bytes - * @param flags RESERVED - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_file_transfer_start_send(mavlink_channel_t chan, uint64_t transfer_uid, const char *dest_path, uint8_t direction, uint32_t file_size, uint8_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN]; - _mav_put_uint64_t(buf, 0, transfer_uid); - _mav_put_uint32_t(buf, 8, file_size); - _mav_put_uint8_t(buf, 252, direction); - _mav_put_uint8_t(buf, 253, flags); - _mav_put_char_array(buf, 12, dest_path, 240); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, buf, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_START_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, buf, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); -#endif -#else - mavlink_file_transfer_start_t packet; - packet.transfer_uid = transfer_uid; - packet.file_size = file_size; - packet.direction = direction; - packet.flags = flags; - mav_array_memcpy(packet.dest_path, dest_path, sizeof(char)*240); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, (const char *)&packet, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_START_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, (const char *)&packet, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); -#endif -#endif -} - -#endif - -// MESSAGE FILE_TRANSFER_START UNPACKING - - -/** - * @brief Get field transfer_uid from file_transfer_start message - * - * @return Unique transfer ID - */ -static inline uint64_t mavlink_msg_file_transfer_start_get_transfer_uid(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field dest_path from file_transfer_start message - * - * @return Destination path - */ -static inline uint16_t mavlink_msg_file_transfer_start_get_dest_path(const mavlink_message_t* msg, char *dest_path) -{ - return _MAV_RETURN_char_array(msg, dest_path, 240, 12); -} - -/** - * @brief Get field direction from file_transfer_start message - * - * @return Transfer direction: 0: from requester, 1: to requester - */ -static inline uint8_t mavlink_msg_file_transfer_start_get_direction(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 252); -} - -/** - * @brief Get field file_size from file_transfer_start message - * - * @return File size in bytes - */ -static inline uint32_t mavlink_msg_file_transfer_start_get_file_size(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 8); -} - -/** - * @brief Get field flags from file_transfer_start message - * - * @return RESERVED - */ -static inline uint8_t mavlink_msg_file_transfer_start_get_flags(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 253); -} - -/** - * @brief Decode a file_transfer_start message into a struct - * - * @param msg The message to decode - * @param file_transfer_start C-struct to decode the message contents into - */ -static inline void mavlink_msg_file_transfer_start_decode(const mavlink_message_t* msg, mavlink_file_transfer_start_t* file_transfer_start) -{ -#if MAVLINK_NEED_BYTE_SWAP - file_transfer_start->transfer_uid = mavlink_msg_file_transfer_start_get_transfer_uid(msg); - file_transfer_start->file_size = mavlink_msg_file_transfer_start_get_file_size(msg); - mavlink_msg_file_transfer_start_get_dest_path(msg, file_transfer_start->dest_path); - file_transfer_start->direction = mavlink_msg_file_transfer_start_get_direction(msg); - file_transfer_start->flags = mavlink_msg_file_transfer_start_get_flags(msg); -#else - memcpy(file_transfer_start, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h index 7ed3d2a..e1ffffb 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h @@ -7,7 +7,7 @@ typedef struct __mavlink_global_position_int_t uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) int32_t lat; ///< Latitude, expressed as * 1E7 int32_t lon; ///< Longitude, expressed as * 1E7 - int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters), above MSL + int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters), WGS84 (not AMSL) int32_t relative_alt; ///< Altitude above ground in meters, expressed as * 1000 (millimeters) int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 @@ -48,7 +48,7 @@ typedef struct __mavlink_global_position_int_t * @param time_boot_ms Timestamp (milliseconds since system boot) * @param lat Latitude, expressed as * 1E7 * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL + * @param alt Altitude in meters, expressed as * 1000 (millimeters), WGS84 (not AMSL) * @param relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) * @param vx Ground X Speed (Latitude), expressed as m/s * 100 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 @@ -104,7 +104,7 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u * @param time_boot_ms Timestamp (milliseconds since system boot) * @param lat Latitude, expressed as * 1E7 * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL + * @param alt Altitude in meters, expressed as * 1000 (millimeters), WGS84 (not AMSL) * @param relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) * @param vx Ground X Speed (Latitude), expressed as m/s * 100 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 @@ -186,7 +186,7 @@ static inline uint16_t mavlink_msg_global_position_int_encode_chan(uint8_t syste * @param time_boot_ms Timestamp (milliseconds since system boot) * @param lat Latitude, expressed as * 1E7 * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL + * @param alt Altitude in meters, expressed as * 1000 (millimeters), WGS84 (not AMSL) * @param relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) * @param vx Ground X Speed (Latitude), expressed as m/s * 100 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 @@ -234,6 +234,54 @@ static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_global_position_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, lon); + _mav_put_int32_t(buf, 12, alt); + _mav_put_int32_t(buf, 16, relative_alt); + _mav_put_int16_t(buf, 20, vx); + _mav_put_int16_t(buf, 22, vy); + _mav_put_int16_t(buf, 24, vz); + _mav_put_uint16_t(buf, 26, hdg); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); +#endif +#else + mavlink_global_position_int_t *packet = (mavlink_global_position_int_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->relative_alt = relative_alt; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->hdg = hdg; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GLOBAL_POSITION_INT UNPACKING @@ -272,7 +320,7 @@ static inline int32_t mavlink_msg_global_position_int_get_lon(const mavlink_mess /** * @brief Get field alt from global_position_int message * - * @return Altitude in meters, expressed as * 1000 (millimeters), above MSL + * @return Altitude in meters, expressed as * 1000 (millimeters), WGS84 (not AMSL) */ static inline int32_t mavlink_msg_global_position_int_get_alt(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int_cov.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int_cov.h new file mode 100644 index 0000000..4d053ba --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int_cov.h @@ -0,0 +1,441 @@ +// MESSAGE GLOBAL_POSITION_INT_COV PACKING + +#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV 63 + +typedef struct __mavlink_global_position_int_cov_t +{ + uint64_t time_utc; ///< Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + int32_t lat; ///< Latitude, expressed as degrees * 1E7 + int32_t lon; ///< Longitude, expressed as degrees * 1E7 + int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters), above MSL + int32_t relative_alt; ///< Altitude above ground in meters, expressed as * 1000 (millimeters) + float vx; ///< Ground X Speed (Latitude), expressed as m/s + float vy; ///< Ground Y Speed (Longitude), expressed as m/s + float vz; ///< Ground Z Speed (Altitude), expressed as m/s + float covariance[36]; ///< Covariance matrix (first six entries are the first ROW, next six entries are the second row, etc.) + uint8_t estimator_type; ///< Class id of the estimator this estimate originated from. +} mavlink_global_position_int_cov_t; + +#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN 185 +#define MAVLINK_MSG_ID_63_LEN 185 + +#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_CRC 51 +#define MAVLINK_MSG_ID_63_CRC 51 + +#define MAVLINK_MSG_GLOBAL_POSITION_INT_COV_FIELD_COVARIANCE_LEN 36 + +#define MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT_COV { \ + "GLOBAL_POSITION_INT_COV", \ + 11, \ + { { "time_utc", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_global_position_int_cov_t, time_utc) }, \ + { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_global_position_int_cov_t, time_boot_ms) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_global_position_int_cov_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_global_position_int_cov_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_global_position_int_cov_t, alt) }, \ + { "relative_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_global_position_int_cov_t, relative_alt) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_global_position_int_cov_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_global_position_int_cov_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_global_position_int_cov_t, vz) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 36, 40, offsetof(mavlink_global_position_int_cov_t, covariance) }, \ + { "estimator_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 184, offsetof(mavlink_global_position_int_cov_t, estimator_type) }, \ + } \ +} + + +/** + * @brief Pack a global_position_int_cov message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param time_utc Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. + * @param estimator_type Class id of the estimator this estimate originated from. + * @param lat Latitude, expressed as degrees * 1E7 + * @param lon Longitude, expressed as degrees * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL + * @param relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s + * @param vy Ground Y Speed (Longitude), expressed as m/s + * @param vz Ground Z Speed (Altitude), expressed as m/s + * @param covariance Covariance matrix (first six entries are the first ROW, next six entries are the second row, etc.) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_global_position_int_cov_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint64_t time_utc, uint8_t estimator_type, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, float vx, float vy, float vz, const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN]; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_int32_t(buf, 12, lat); + _mav_put_int32_t(buf, 16, lon); + _mav_put_int32_t(buf, 20, alt); + _mav_put_int32_t(buf, 24, relative_alt); + _mav_put_float(buf, 28, vx); + _mav_put_float(buf, 32, vy); + _mav_put_float(buf, 36, vz); + _mav_put_uint8_t(buf, 184, estimator_type); + _mav_put_float_array(buf, 40, covariance, 36); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#else + mavlink_global_position_int_cov_t packet; + packet.time_utc = time_utc; + packet.time_boot_ms = time_boot_ms; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.estimator_type = estimator_type; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*36); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#endif +} + +/** + * @brief Pack a global_position_int_cov message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param time_utc Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. + * @param estimator_type Class id of the estimator this estimate originated from. + * @param lat Latitude, expressed as degrees * 1E7 + * @param lon Longitude, expressed as degrees * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL + * @param relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s + * @param vy Ground Y Speed (Longitude), expressed as m/s + * @param vz Ground Z Speed (Altitude), expressed as m/s + * @param covariance Covariance matrix (first six entries are the first ROW, next six entries are the second row, etc.) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_global_position_int_cov_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint64_t time_utc,uint8_t estimator_type,int32_t lat,int32_t lon,int32_t alt,int32_t relative_alt,float vx,float vy,float vz,const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN]; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_int32_t(buf, 12, lat); + _mav_put_int32_t(buf, 16, lon); + _mav_put_int32_t(buf, 20, alt); + _mav_put_int32_t(buf, 24, relative_alt); + _mav_put_float(buf, 28, vx); + _mav_put_float(buf, 32, vy); + _mav_put_float(buf, 36, vz); + _mav_put_uint8_t(buf, 184, estimator_type); + _mav_put_float_array(buf, 40, covariance, 36); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#else + mavlink_global_position_int_cov_t packet; + packet.time_utc = time_utc; + packet.time_boot_ms = time_boot_ms; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.estimator_type = estimator_type; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*36); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#endif +} + +/** + * @brief Encode a global_position_int_cov struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param global_position_int_cov C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_position_int_cov_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_position_int_cov_t* global_position_int_cov) +{ + return mavlink_msg_global_position_int_cov_pack(system_id, component_id, msg, global_position_int_cov->time_boot_ms, global_position_int_cov->time_utc, global_position_int_cov->estimator_type, global_position_int_cov->lat, global_position_int_cov->lon, global_position_int_cov->alt, global_position_int_cov->relative_alt, global_position_int_cov->vx, global_position_int_cov->vy, global_position_int_cov->vz, global_position_int_cov->covariance); +} + +/** + * @brief Encode a global_position_int_cov struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param global_position_int_cov C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_position_int_cov_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_position_int_cov_t* global_position_int_cov) +{ + return mavlink_msg_global_position_int_cov_pack_chan(system_id, component_id, chan, msg, global_position_int_cov->time_boot_ms, global_position_int_cov->time_utc, global_position_int_cov->estimator_type, global_position_int_cov->lat, global_position_int_cov->lon, global_position_int_cov->alt, global_position_int_cov->relative_alt, global_position_int_cov->vx, global_position_int_cov->vy, global_position_int_cov->vz, global_position_int_cov->covariance); +} + +/** + * @brief Send a global_position_int_cov message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param time_utc Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. + * @param estimator_type Class id of the estimator this estimate originated from. + * @param lat Latitude, expressed as degrees * 1E7 + * @param lon Longitude, expressed as degrees * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL + * @param relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s + * @param vy Ground Y Speed (Longitude), expressed as m/s + * @param vz Ground Z Speed (Altitude), expressed as m/s + * @param covariance Covariance matrix (first six entries are the first ROW, next six entries are the second row, etc.) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_global_position_int_cov_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t time_utc, uint8_t estimator_type, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, float vx, float vy, float vz, const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN]; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_int32_t(buf, 12, lat); + _mav_put_int32_t(buf, 16, lon); + _mav_put_int32_t(buf, 20, alt); + _mav_put_int32_t(buf, 24, relative_alt); + _mav_put_float(buf, 28, vx); + _mav_put_float(buf, 32, vy); + _mav_put_float(buf, 36, vz); + _mav_put_uint8_t(buf, 184, estimator_type); + _mav_put_float_array(buf, 40, covariance, 36); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#endif +#else + mavlink_global_position_int_cov_t packet; + packet.time_utc = time_utc; + packet.time_boot_ms = time_boot_ms; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.estimator_type = estimator_type; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*36); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_global_position_int_cov_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t time_utc, uint8_t estimator_type, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, float vx, float vy, float vz, const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_int32_t(buf, 12, lat); + _mav_put_int32_t(buf, 16, lon); + _mav_put_int32_t(buf, 20, alt); + _mav_put_int32_t(buf, 24, relative_alt); + _mav_put_float(buf, 28, vx); + _mav_put_float(buf, 32, vy); + _mav_put_float(buf, 36, vz); + _mav_put_uint8_t(buf, 184, estimator_type); + _mav_put_float_array(buf, 40, covariance, 36); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#endif +#else + mavlink_global_position_int_cov_t *packet = (mavlink_global_position_int_cov_t *)msgbuf; + packet->time_utc = time_utc; + packet->time_boot_ms = time_boot_ms; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->relative_alt = relative_alt; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->estimator_type = estimator_type; + mav_array_memcpy(packet->covariance, covariance, sizeof(float)*36); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GLOBAL_POSITION_INT_COV UNPACKING + + +/** + * @brief Get field time_boot_ms from global_position_int_cov message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_global_position_int_cov_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field time_utc from global_position_int_cov message + * + * @return Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. + */ +static inline uint64_t mavlink_msg_global_position_int_cov_get_time_utc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field estimator_type from global_position_int_cov message + * + * @return Class id of the estimator this estimate originated from. + */ +static inline uint8_t mavlink_msg_global_position_int_cov_get_estimator_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 184); +} + +/** + * @brief Get field lat from global_position_int_cov message + * + * @return Latitude, expressed as degrees * 1E7 + */ +static inline int32_t mavlink_msg_global_position_int_cov_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field lon from global_position_int_cov message + * + * @return Longitude, expressed as degrees * 1E7 + */ +static inline int32_t mavlink_msg_global_position_int_cov_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field alt from global_position_int_cov message + * + * @return Altitude in meters, expressed as * 1000 (millimeters), above MSL + */ +static inline int32_t mavlink_msg_global_position_int_cov_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 20); +} + +/** + * @brief Get field relative_alt from global_position_int_cov message + * + * @return Altitude above ground in meters, expressed as * 1000 (millimeters) + */ +static inline int32_t mavlink_msg_global_position_int_cov_get_relative_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 24); +} + +/** + * @brief Get field vx from global_position_int_cov message + * + * @return Ground X Speed (Latitude), expressed as m/s + */ +static inline float mavlink_msg_global_position_int_cov_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field vy from global_position_int_cov message + * + * @return Ground Y Speed (Longitude), expressed as m/s + */ +static inline float mavlink_msg_global_position_int_cov_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field vz from global_position_int_cov message + * + * @return Ground Z Speed (Altitude), expressed as m/s + */ +static inline float mavlink_msg_global_position_int_cov_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field covariance from global_position_int_cov message + * + * @return Covariance matrix (first six entries are the first ROW, next six entries are the second row, etc.) + */ +static inline uint16_t mavlink_msg_global_position_int_cov_get_covariance(const mavlink_message_t* msg, float *covariance) +{ + return _MAV_RETURN_float_array(msg, covariance, 36, 40); +} + +/** + * @brief Decode a global_position_int_cov message into a struct + * + * @param msg The message to decode + * @param global_position_int_cov C-struct to decode the message contents into + */ +static inline void mavlink_msg_global_position_int_cov_decode(const mavlink_message_t* msg, mavlink_global_position_int_cov_t* global_position_int_cov) +{ +#if MAVLINK_NEED_BYTE_SWAP + global_position_int_cov->time_utc = mavlink_msg_global_position_int_cov_get_time_utc(msg); + global_position_int_cov->time_boot_ms = mavlink_msg_global_position_int_cov_get_time_boot_ms(msg); + global_position_int_cov->lat = mavlink_msg_global_position_int_cov_get_lat(msg); + global_position_int_cov->lon = mavlink_msg_global_position_int_cov_get_lon(msg); + global_position_int_cov->alt = mavlink_msg_global_position_int_cov_get_alt(msg); + global_position_int_cov->relative_alt = mavlink_msg_global_position_int_cov_get_relative_alt(msg); + global_position_int_cov->vx = mavlink_msg_global_position_int_cov_get_vx(msg); + global_position_int_cov->vy = mavlink_msg_global_position_int_cov_get_vy(msg); + global_position_int_cov->vz = mavlink_msg_global_position_int_cov_get_vz(msg); + mavlink_msg_global_position_int_cov_get_covariance(msg, global_position_int_cov->covariance); + global_position_int_cov->estimator_type = mavlink_msg_global_position_int_cov_get_estimator_type(msg); +#else + memcpy(global_position_int_cov, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h deleted file mode 100644 index 1a1c971..0000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h +++ /dev/null @@ -1,265 +0,0 @@ -// MESSAGE GLOBAL_POSITION_SETPOINT_INT PACKING - -#define MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT 52 - -typedef struct __mavlink_global_position_setpoint_int_t -{ - int32_t latitude; ///< Latitude (WGS84), in degrees * 1E7 - int32_t longitude; ///< Longitude (WGS84), in degrees * 1E7 - int32_t altitude; ///< Altitude (WGS84), in meters * 1000 (positive for up) - int16_t yaw; ///< Desired yaw angle in degrees * 100 - uint8_t coordinate_frame; ///< Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT -} mavlink_global_position_setpoint_int_t; - -#define MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN 15 -#define MAVLINK_MSG_ID_52_LEN 15 - -#define MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_CRC 141 -#define MAVLINK_MSG_ID_52_CRC 141 - - - -#define MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT { \ - "GLOBAL_POSITION_SETPOINT_INT", \ - 5, \ - { { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_global_position_setpoint_int_t, latitude) }, \ - { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_global_position_setpoint_int_t, longitude) }, \ - { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_global_position_setpoint_int_t, altitude) }, \ - { "yaw", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_global_position_setpoint_int_t, yaw) }, \ - { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_global_position_setpoint_int_t, coordinate_frame) }, \ - } \ -} - - -/** - * @brief Pack a global_position_setpoint_int message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT - * @param latitude Latitude (WGS84), in degrees * 1E7 - * @param longitude Longitude (WGS84), in degrees * 1E7 - * @param altitude Altitude (WGS84), in meters * 1000 (positive for up) - * @param yaw Desired yaw angle in degrees * 100 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_global_position_setpoint_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t coordinate_frame, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - _mav_put_int16_t(buf, 12, yaw); - _mav_put_uint8_t(buf, 14, coordinate_frame); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); -#else - mavlink_global_position_setpoint_int_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - packet.yaw = yaw; - packet.coordinate_frame = coordinate_frame; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -} - -/** - * @brief Pack a global_position_setpoint_int message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT - * @param latitude Latitude (WGS84), in degrees * 1E7 - * @param longitude Longitude (WGS84), in degrees * 1E7 - * @param altitude Altitude (WGS84), in meters * 1000 (positive for up) - * @param yaw Desired yaw angle in degrees * 100 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_global_position_setpoint_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t coordinate_frame,int32_t latitude,int32_t longitude,int32_t altitude,int16_t yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - _mav_put_int16_t(buf, 12, yaw); - _mav_put_uint8_t(buf, 14, coordinate_frame); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); -#else - mavlink_global_position_setpoint_int_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - packet.yaw = yaw; - packet.coordinate_frame = coordinate_frame; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -} - -/** - * @brief Encode a global_position_setpoint_int struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param global_position_setpoint_int C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_global_position_setpoint_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_position_setpoint_int_t* global_position_setpoint_int) -{ - return mavlink_msg_global_position_setpoint_int_pack(system_id, component_id, msg, global_position_setpoint_int->coordinate_frame, global_position_setpoint_int->latitude, global_position_setpoint_int->longitude, global_position_setpoint_int->altitude, global_position_setpoint_int->yaw); -} - -/** - * @brief Encode a global_position_setpoint_int struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param global_position_setpoint_int C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_global_position_setpoint_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_position_setpoint_int_t* global_position_setpoint_int) -{ - return mavlink_msg_global_position_setpoint_int_pack_chan(system_id, component_id, chan, msg, global_position_setpoint_int->coordinate_frame, global_position_setpoint_int->latitude, global_position_setpoint_int->longitude, global_position_setpoint_int->altitude, global_position_setpoint_int->yaw); -} - -/** - * @brief Send a global_position_setpoint_int message - * @param chan MAVLink channel to send the message - * - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT - * @param latitude Latitude (WGS84), in degrees * 1E7 - * @param longitude Longitude (WGS84), in degrees * 1E7 - * @param altitude Altitude (WGS84), in meters * 1000 (positive for up) - * @param yaw Desired yaw angle in degrees * 100 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_global_position_setpoint_int_send(mavlink_channel_t chan, uint8_t coordinate_frame, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - _mav_put_int16_t(buf, 12, yaw); - _mav_put_uint8_t(buf, 14, coordinate_frame); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -#else - mavlink_global_position_setpoint_int_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - packet.yaw = yaw; - packet.coordinate_frame = coordinate_frame; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -#endif -} - -#endif - -// MESSAGE GLOBAL_POSITION_SETPOINT_INT UNPACKING - - -/** - * @brief Get field coordinate_frame from global_position_setpoint_int message - * - * @return Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT - */ -static inline uint8_t mavlink_msg_global_position_setpoint_int_get_coordinate_frame(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 14); -} - -/** - * @brief Get field latitude from global_position_setpoint_int message - * - * @return Latitude (WGS84), in degrees * 1E7 - */ -static inline int32_t mavlink_msg_global_position_setpoint_int_get_latitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field longitude from global_position_setpoint_int message - * - * @return Longitude (WGS84), in degrees * 1E7 - */ -static inline int32_t mavlink_msg_global_position_setpoint_int_get_longitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field altitude from global_position_setpoint_int message - * - * @return Altitude (WGS84), in meters * 1000 (positive for up) - */ -static inline int32_t mavlink_msg_global_position_setpoint_int_get_altitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Get field yaw from global_position_setpoint_int message - * - * @return Desired yaw angle in degrees * 100 - */ -static inline int16_t mavlink_msg_global_position_setpoint_int_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 12); -} - -/** - * @brief Decode a global_position_setpoint_int message into a struct - * - * @param msg The message to decode - * @param global_position_setpoint_int C-struct to decode the message contents into - */ -static inline void mavlink_msg_global_position_setpoint_int_decode(const mavlink_message_t* msg, mavlink_global_position_setpoint_int_t* global_position_setpoint_int) -{ -#if MAVLINK_NEED_BYTE_SWAP - global_position_setpoint_int->latitude = mavlink_msg_global_position_setpoint_int_get_latitude(msg); - global_position_setpoint_int->longitude = mavlink_msg_global_position_setpoint_int_get_longitude(msg); - global_position_setpoint_int->altitude = mavlink_msg_global_position_setpoint_int_get_altitude(msg); - global_position_setpoint_int->yaw = mavlink_msg_global_position_setpoint_int_get_yaw(msg); - global_position_setpoint_int->coordinate_frame = mavlink_msg_global_position_setpoint_int_get_coordinate_frame(msg); -#else - memcpy(global_position_setpoint_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h index f7be74c..b2b75d7 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_global_vision_position_estimate_send(mavlink_chan #endif } +#if MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_global_vision_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); +#endif +#else + mavlink_global_vision_position_estimate_t *packet = (mavlink_global_vision_position_estimate_t *)msgbuf; + packet->usec = usec; + packet->x = x; + packet->y = y; + packet->z = z; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GLOBAL_VISION_POSITION_ESTIMATE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h index 17e5bd0..84f15bf 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h @@ -13,7 +13,7 @@ typedef struct __mavlink_gps2_raw_t uint16_t epv; ///< GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS fix, 5: RTK Fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255 uint8_t dgps_numch; ///< Number of DGPS satellites } mavlink_gps2_raw_t; @@ -52,7 +52,7 @@ typedef struct __mavlink_gps2_raw_t * @param msg The MAVLink message to compress the data into * * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS fix, 5: RTK Fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. * @param lat Latitude (WGS84), in degrees * 1E7 * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) @@ -117,7 +117,7 @@ static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t comp * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS fix, 5: RTK Fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. * @param lat Latitude (WGS84), in degrees * 1E7 * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) @@ -208,7 +208,7 @@ static inline uint16_t mavlink_msg_gps2_raw_encode_chan(uint8_t system_id, uint8 * @param chan MAVLink channel to send the message * * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS fix, 5: RTK Fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. * @param lat Latitude (WGS84), in degrees * 1E7 * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) @@ -267,6 +267,60 @@ static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t ti #endif } +#if MAVLINK_MSG_ID_GPS2_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps2_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint32_t(buf, 20, dgps_age); + _mav_put_uint16_t(buf, 24, eph); + _mav_put_uint16_t(buf, 26, epv); + _mav_put_uint16_t(buf, 28, vel); + _mav_put_uint16_t(buf, 30, cog); + _mav_put_uint8_t(buf, 32, fix_type); + _mav_put_uint8_t(buf, 33, satellites_visible); + _mav_put_uint8_t(buf, 34, dgps_numch); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +#else + mavlink_gps2_raw_t *packet = (mavlink_gps2_raw_t *)msgbuf; + packet->time_usec = time_usec; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->dgps_age = dgps_age; + packet->eph = eph; + packet->epv = epv; + packet->vel = vel; + packet->cog = cog; + packet->fix_type = fix_type; + packet->satellites_visible = satellites_visible; + packet->dgps_numch = dgps_numch; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)packet, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GPS2_RAW UNPACKING @@ -285,7 +339,7 @@ static inline uint64_t mavlink_msg_gps2_raw_get_time_usec(const mavlink_message_ /** * @brief Get field fix_type from gps2_raw message * - * @return 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @return 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS fix, 5: RTK Fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ static inline uint8_t mavlink_msg_gps2_raw_get_fix_type(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_rtk.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_rtk.h new file mode 100644 index 0000000..4c2d93c --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_rtk.h @@ -0,0 +1,497 @@ +// MESSAGE GPS2_RTK PACKING + +#define MAVLINK_MSG_ID_GPS2_RTK 128 + +typedef struct __mavlink_gps2_rtk_t +{ + uint32_t time_last_baseline_ms; ///< Time since boot of last baseline message received in ms. + uint32_t tow; ///< GPS Time of Week of last baseline + int32_t baseline_a_mm; ///< Current baseline in ECEF x or NED north component in mm. + int32_t baseline_b_mm; ///< Current baseline in ECEF y or NED east component in mm. + int32_t baseline_c_mm; ///< Current baseline in ECEF z or NED down component in mm. + uint32_t accuracy; ///< Current estimate of baseline accuracy. + int32_t iar_num_hypotheses; ///< Current number of integer ambiguity hypotheses. + uint16_t wn; ///< GPS Week Number of last baseline + uint8_t rtk_receiver_id; ///< Identification of connected RTK receiver. + uint8_t rtk_health; ///< GPS-specific health report for RTK data. + uint8_t rtk_rate; ///< Rate of baseline messages being received by GPS, in HZ + uint8_t nsats; ///< Current number of sats used for RTK calculation. + uint8_t baseline_coords_type; ///< Coordinate system of baseline. 0 == ECEF, 1 == NED +} mavlink_gps2_rtk_t; + +#define MAVLINK_MSG_ID_GPS2_RTK_LEN 35 +#define MAVLINK_MSG_ID_128_LEN 35 + +#define MAVLINK_MSG_ID_GPS2_RTK_CRC 226 +#define MAVLINK_MSG_ID_128_CRC 226 + + + +#define MAVLINK_MESSAGE_INFO_GPS2_RTK { \ + "GPS2_RTK", \ + 13, \ + { { "time_last_baseline_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gps2_rtk_t, time_last_baseline_ms) }, \ + { "tow", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_gps2_rtk_t, tow) }, \ + { "baseline_a_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps2_rtk_t, baseline_a_mm) }, \ + { "baseline_b_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_gps2_rtk_t, baseline_b_mm) }, \ + { "baseline_c_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_gps2_rtk_t, baseline_c_mm) }, \ + { "accuracy", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps2_rtk_t, accuracy) }, \ + { "iar_num_hypotheses", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_gps2_rtk_t, iar_num_hypotheses) }, \ + { "wn", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_gps2_rtk_t, wn) }, \ + { "rtk_receiver_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_gps2_rtk_t, rtk_receiver_id) }, \ + { "rtk_health", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_gps2_rtk_t, rtk_health) }, \ + { "rtk_rate", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps2_rtk_t, rtk_rate) }, \ + { "nsats", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gps2_rtk_t, nsats) }, \ + { "baseline_coords_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps2_rtk_t, baseline_coords_type) }, \ + } \ +} + + +/** + * @brief Pack a gps2_rtk message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_last_baseline_ms Time since boot of last baseline message received in ms. + * @param rtk_receiver_id Identification of connected RTK receiver. + * @param wn GPS Week Number of last baseline + * @param tow GPS Time of Week of last baseline + * @param rtk_health GPS-specific health report for RTK data. + * @param rtk_rate Rate of baseline messages being received by GPS, in HZ + * @param nsats Current number of sats used for RTK calculation. + * @param baseline_coords_type Coordinate system of baseline. 0 == ECEF, 1 == NED + * @param baseline_a_mm Current baseline in ECEF x or NED north component in mm. + * @param baseline_b_mm Current baseline in ECEF y or NED east component in mm. + * @param baseline_c_mm Current baseline in ECEF z or NED down component in mm. + * @param accuracy Current estimate of baseline accuracy. + * @param iar_num_hypotheses Current number of integer ambiguity hypotheses. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps2_rtk_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS2_RTK_LEN]; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#else + mavlink_gps2_rtk_t packet; + packet.time_last_baseline_ms = time_last_baseline_ms; + packet.tow = tow; + packet.baseline_a_mm = baseline_a_mm; + packet.baseline_b_mm = baseline_b_mm; + packet.baseline_c_mm = baseline_c_mm; + packet.accuracy = accuracy; + packet.iar_num_hypotheses = iar_num_hypotheses; + packet.wn = wn; + packet.rtk_receiver_id = rtk_receiver_id; + packet.rtk_health = rtk_health; + packet.rtk_rate = rtk_rate; + packet.nsats = nsats; + packet.baseline_coords_type = baseline_coords_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS2_RTK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +} + +/** + * @brief Pack a gps2_rtk message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_last_baseline_ms Time since boot of last baseline message received in ms. + * @param rtk_receiver_id Identification of connected RTK receiver. + * @param wn GPS Week Number of last baseline + * @param tow GPS Time of Week of last baseline + * @param rtk_health GPS-specific health report for RTK data. + * @param rtk_rate Rate of baseline messages being received by GPS, in HZ + * @param nsats Current number of sats used for RTK calculation. + * @param baseline_coords_type Coordinate system of baseline. 0 == ECEF, 1 == NED + * @param baseline_a_mm Current baseline in ECEF x or NED north component in mm. + * @param baseline_b_mm Current baseline in ECEF y or NED east component in mm. + * @param baseline_c_mm Current baseline in ECEF z or NED down component in mm. + * @param accuracy Current estimate of baseline accuracy. + * @param iar_num_hypotheses Current number of integer ambiguity hypotheses. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps2_rtk_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_last_baseline_ms,uint8_t rtk_receiver_id,uint16_t wn,uint32_t tow,uint8_t rtk_health,uint8_t rtk_rate,uint8_t nsats,uint8_t baseline_coords_type,int32_t baseline_a_mm,int32_t baseline_b_mm,int32_t baseline_c_mm,uint32_t accuracy,int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS2_RTK_LEN]; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#else + mavlink_gps2_rtk_t packet; + packet.time_last_baseline_ms = time_last_baseline_ms; + packet.tow = tow; + packet.baseline_a_mm = baseline_a_mm; + packet.baseline_b_mm = baseline_b_mm; + packet.baseline_c_mm = baseline_c_mm; + packet.accuracy = accuracy; + packet.iar_num_hypotheses = iar_num_hypotheses; + packet.wn = wn; + packet.rtk_receiver_id = rtk_receiver_id; + packet.rtk_health = rtk_health; + packet.rtk_rate = rtk_rate; + packet.nsats = nsats; + packet.baseline_coords_type = baseline_coords_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS2_RTK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +} + +/** + * @brief Encode a gps2_rtk struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gps2_rtk C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps2_rtk_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps2_rtk_t* gps2_rtk) +{ + return mavlink_msg_gps2_rtk_pack(system_id, component_id, msg, gps2_rtk->time_last_baseline_ms, gps2_rtk->rtk_receiver_id, gps2_rtk->wn, gps2_rtk->tow, gps2_rtk->rtk_health, gps2_rtk->rtk_rate, gps2_rtk->nsats, gps2_rtk->baseline_coords_type, gps2_rtk->baseline_a_mm, gps2_rtk->baseline_b_mm, gps2_rtk->baseline_c_mm, gps2_rtk->accuracy, gps2_rtk->iar_num_hypotheses); +} + +/** + * @brief Encode a gps2_rtk struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps2_rtk C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps2_rtk_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps2_rtk_t* gps2_rtk) +{ + return mavlink_msg_gps2_rtk_pack_chan(system_id, component_id, chan, msg, gps2_rtk->time_last_baseline_ms, gps2_rtk->rtk_receiver_id, gps2_rtk->wn, gps2_rtk->tow, gps2_rtk->rtk_health, gps2_rtk->rtk_rate, gps2_rtk->nsats, gps2_rtk->baseline_coords_type, gps2_rtk->baseline_a_mm, gps2_rtk->baseline_b_mm, gps2_rtk->baseline_c_mm, gps2_rtk->accuracy, gps2_rtk->iar_num_hypotheses); +} + +/** + * @brief Send a gps2_rtk message + * @param chan MAVLink channel to send the message + * + * @param time_last_baseline_ms Time since boot of last baseline message received in ms. + * @param rtk_receiver_id Identification of connected RTK receiver. + * @param wn GPS Week Number of last baseline + * @param tow GPS Time of Week of last baseline + * @param rtk_health GPS-specific health report for RTK data. + * @param rtk_rate Rate of baseline messages being received by GPS, in HZ + * @param nsats Current number of sats used for RTK calculation. + * @param baseline_coords_type Coordinate system of baseline. 0 == ECEF, 1 == NED + * @param baseline_a_mm Current baseline in ECEF x or NED north component in mm. + * @param baseline_b_mm Current baseline in ECEF y or NED east component in mm. + * @param baseline_c_mm Current baseline in ECEF z or NED down component in mm. + * @param accuracy Current estimate of baseline accuracy. + * @param iar_num_hypotheses Current number of integer ambiguity hypotheses. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gps2_rtk_send(mavlink_channel_t chan, uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS2_RTK_LEN]; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, buf, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, buf, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +#else + mavlink_gps2_rtk_t packet; + packet.time_last_baseline_ms = time_last_baseline_ms; + packet.tow = tow; + packet.baseline_a_mm = baseline_a_mm; + packet.baseline_b_mm = baseline_b_mm; + packet.baseline_c_mm = baseline_c_mm; + packet.accuracy = accuracy; + packet.iar_num_hypotheses = iar_num_hypotheses; + packet.wn = wn; + packet.rtk_receiver_id = rtk_receiver_id; + packet.rtk_health = rtk_health; + packet.rtk_rate = rtk_rate; + packet.nsats = nsats; + packet.baseline_coords_type = baseline_coords_type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GPS2_RTK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps2_rtk_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, buf, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, buf, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +#else + mavlink_gps2_rtk_t *packet = (mavlink_gps2_rtk_t *)msgbuf; + packet->time_last_baseline_ms = time_last_baseline_ms; + packet->tow = tow; + packet->baseline_a_mm = baseline_a_mm; + packet->baseline_b_mm = baseline_b_mm; + packet->baseline_c_mm = baseline_c_mm; + packet->accuracy = accuracy; + packet->iar_num_hypotheses = iar_num_hypotheses; + packet->wn = wn; + packet->rtk_receiver_id = rtk_receiver_id; + packet->rtk_health = rtk_health; + packet->rtk_rate = rtk_rate; + packet->nsats = nsats; + packet->baseline_coords_type = baseline_coords_type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, (const char *)packet, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, (const char *)packet, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GPS2_RTK UNPACKING + + +/** + * @brief Get field time_last_baseline_ms from gps2_rtk message + * + * @return Time since boot of last baseline message received in ms. + */ +static inline uint32_t mavlink_msg_gps2_rtk_get_time_last_baseline_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field rtk_receiver_id from gps2_rtk message + * + * @return Identification of connected RTK receiver. + */ +static inline uint8_t mavlink_msg_gps2_rtk_get_rtk_receiver_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 30); +} + +/** + * @brief Get field wn from gps2_rtk message + * + * @return GPS Week Number of last baseline + */ +static inline uint16_t mavlink_msg_gps2_rtk_get_wn(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field tow from gps2_rtk message + * + * @return GPS Time of Week of last baseline + */ +static inline uint32_t mavlink_msg_gps2_rtk_get_tow(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field rtk_health from gps2_rtk message + * + * @return GPS-specific health report for RTK data. + */ +static inline uint8_t mavlink_msg_gps2_rtk_get_rtk_health(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 31); +} + +/** + * @brief Get field rtk_rate from gps2_rtk message + * + * @return Rate of baseline messages being received by GPS, in HZ + */ +static inline uint8_t mavlink_msg_gps2_rtk_get_rtk_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field nsats from gps2_rtk message + * + * @return Current number of sats used for RTK calculation. + */ +static inline uint8_t mavlink_msg_gps2_rtk_get_nsats(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 33); +} + +/** + * @brief Get field baseline_coords_type from gps2_rtk message + * + * @return Coordinate system of baseline. 0 == ECEF, 1 == NED + */ +static inline uint8_t mavlink_msg_gps2_rtk_get_baseline_coords_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field baseline_a_mm from gps2_rtk message + * + * @return Current baseline in ECEF x or NED north component in mm. + */ +static inline int32_t mavlink_msg_gps2_rtk_get_baseline_a_mm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field baseline_b_mm from gps2_rtk message + * + * @return Current baseline in ECEF y or NED east component in mm. + */ +static inline int32_t mavlink_msg_gps2_rtk_get_baseline_b_mm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field baseline_c_mm from gps2_rtk message + * + * @return Current baseline in ECEF z or NED down component in mm. + */ +static inline int32_t mavlink_msg_gps2_rtk_get_baseline_c_mm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field accuracy from gps2_rtk message + * + * @return Current estimate of baseline accuracy. + */ +static inline uint32_t mavlink_msg_gps2_rtk_get_accuracy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 20); +} + +/** + * @brief Get field iar_num_hypotheses from gps2_rtk message + * + * @return Current number of integer ambiguity hypotheses. + */ +static inline int32_t mavlink_msg_gps2_rtk_get_iar_num_hypotheses(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 24); +} + +/** + * @brief Decode a gps2_rtk message into a struct + * + * @param msg The message to decode + * @param gps2_rtk C-struct to decode the message contents into + */ +static inline void mavlink_msg_gps2_rtk_decode(const mavlink_message_t* msg, mavlink_gps2_rtk_t* gps2_rtk) +{ +#if MAVLINK_NEED_BYTE_SWAP + gps2_rtk->time_last_baseline_ms = mavlink_msg_gps2_rtk_get_time_last_baseline_ms(msg); + gps2_rtk->tow = mavlink_msg_gps2_rtk_get_tow(msg); + gps2_rtk->baseline_a_mm = mavlink_msg_gps2_rtk_get_baseline_a_mm(msg); + gps2_rtk->baseline_b_mm = mavlink_msg_gps2_rtk_get_baseline_b_mm(msg); + gps2_rtk->baseline_c_mm = mavlink_msg_gps2_rtk_get_baseline_c_mm(msg); + gps2_rtk->accuracy = mavlink_msg_gps2_rtk_get_accuracy(msg); + gps2_rtk->iar_num_hypotheses = mavlink_msg_gps2_rtk_get_iar_num_hypotheses(msg); + gps2_rtk->wn = mavlink_msg_gps2_rtk_get_wn(msg); + gps2_rtk->rtk_receiver_id = mavlink_msg_gps2_rtk_get_rtk_receiver_id(msg); + gps2_rtk->rtk_health = mavlink_msg_gps2_rtk_get_rtk_health(msg); + gps2_rtk->rtk_rate = mavlink_msg_gps2_rtk_get_rtk_rate(msg); + gps2_rtk->nsats = mavlink_msg_gps2_rtk_get_nsats(msg); + gps2_rtk->baseline_coords_type = mavlink_msg_gps2_rtk_get_baseline_coords_type(msg); +#else + memcpy(gps2_rtk, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h index 016e9cb..1589c8c 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_gps_global_origin_send(mavlink_channel_t chan, in #endif } +#if MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps_global_origin_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); +#endif +#else + mavlink_gps_global_origin_t *packet = (mavlink_gps_global_origin_t *)msgbuf; + packet->latitude = latitude; + packet->longitude = longitude; + packet->altitude = altitude; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GPS_GLOBAL_ORIGIN UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h index 485d8a4..362e2d7 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h @@ -173,6 +173,42 @@ static inline void mavlink_msg_gps_inject_data_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps_inject_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, len); + _mav_put_uint8_t_array(buf, 3, data, 110); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +#else + mavlink_gps_inject_data_t *packet = (mavlink_gps_inject_data_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->len = len; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*110); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GPS_INJECT_DATA UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h index a105f8c..9ab81fc 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h @@ -12,7 +12,7 @@ typedef struct __mavlink_gps_raw_int_t uint16_t epv; ///< GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS, 5: RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255 } mavlink_gps_raw_int_t; @@ -48,7 +48,7 @@ typedef struct __mavlink_gps_raw_int_t * @param msg The MAVLink message to compress the data into * * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS, 5: RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. * @param lat Latitude (WGS84), in degrees * 1E7 * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) @@ -107,7 +107,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS, 5: RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. * @param lat Latitude (WGS84), in degrees * 1E7 * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) @@ -192,7 +192,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode_chan(uint8_t system_id, ui * @param chan MAVLink channel to send the message * * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS, 5: RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. * @param lat Latitude (WGS84), in degrees * 1E7 * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) @@ -245,6 +245,56 @@ static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t #endif } +#if MAVLINK_MSG_ID_GPS_RAW_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps_raw_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_uint16_t(buf, 26, cog); + _mav_put_uint8_t(buf, 28, fix_type); + _mav_put_uint8_t(buf, 29, satellites_visible); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); +#endif +#else + mavlink_gps_raw_int_t *packet = (mavlink_gps_raw_int_t *)msgbuf; + packet->time_usec = time_usec; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->eph = eph; + packet->epv = epv; + packet->vel = vel; + packet->cog = cog; + packet->fix_type = fix_type; + packet->satellites_visible = satellites_visible; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GPS_RAW_INT UNPACKING @@ -263,7 +313,7 @@ static inline uint64_t mavlink_msg_gps_raw_int_get_time_usec(const mavlink_messa /** * @brief Get field fix_type from gps_raw_int message * - * @return 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @return 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS, 5: RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ static inline uint8_t mavlink_msg_gps_raw_int_get_fix_type(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_rtk.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_rtk.h new file mode 100644 index 0000000..31052ec --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_rtk.h @@ -0,0 +1,497 @@ +// MESSAGE GPS_RTK PACKING + +#define MAVLINK_MSG_ID_GPS_RTK 127 + +typedef struct __mavlink_gps_rtk_t +{ + uint32_t time_last_baseline_ms; ///< Time since boot of last baseline message received in ms. + uint32_t tow; ///< GPS Time of Week of last baseline + int32_t baseline_a_mm; ///< Current baseline in ECEF x or NED north component in mm. + int32_t baseline_b_mm; ///< Current baseline in ECEF y or NED east component in mm. + int32_t baseline_c_mm; ///< Current baseline in ECEF z or NED down component in mm. + uint32_t accuracy; ///< Current estimate of baseline accuracy. + int32_t iar_num_hypotheses; ///< Current number of integer ambiguity hypotheses. + uint16_t wn; ///< GPS Week Number of last baseline + uint8_t rtk_receiver_id; ///< Identification of connected RTK receiver. + uint8_t rtk_health; ///< GPS-specific health report for RTK data. + uint8_t rtk_rate; ///< Rate of baseline messages being received by GPS, in HZ + uint8_t nsats; ///< Current number of sats used for RTK calculation. + uint8_t baseline_coords_type; ///< Coordinate system of baseline. 0 == ECEF, 1 == NED +} mavlink_gps_rtk_t; + +#define MAVLINK_MSG_ID_GPS_RTK_LEN 35 +#define MAVLINK_MSG_ID_127_LEN 35 + +#define MAVLINK_MSG_ID_GPS_RTK_CRC 25 +#define MAVLINK_MSG_ID_127_CRC 25 + + + +#define MAVLINK_MESSAGE_INFO_GPS_RTK { \ + "GPS_RTK", \ + 13, \ + { { "time_last_baseline_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gps_rtk_t, time_last_baseline_ms) }, \ + { "tow", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_gps_rtk_t, tow) }, \ + { "baseline_a_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_rtk_t, baseline_a_mm) }, \ + { "baseline_b_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_gps_rtk_t, baseline_b_mm) }, \ + { "baseline_c_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_gps_rtk_t, baseline_c_mm) }, \ + { "accuracy", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps_rtk_t, accuracy) }, \ + { "iar_num_hypotheses", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_gps_rtk_t, iar_num_hypotheses) }, \ + { "wn", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_gps_rtk_t, wn) }, \ + { "rtk_receiver_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_gps_rtk_t, rtk_receiver_id) }, \ + { "rtk_health", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_gps_rtk_t, rtk_health) }, \ + { "rtk_rate", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps_rtk_t, rtk_rate) }, \ + { "nsats", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gps_rtk_t, nsats) }, \ + { "baseline_coords_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps_rtk_t, baseline_coords_type) }, \ + } \ +} + + +/** + * @brief Pack a gps_rtk message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_last_baseline_ms Time since boot of last baseline message received in ms. + * @param rtk_receiver_id Identification of connected RTK receiver. + * @param wn GPS Week Number of last baseline + * @param tow GPS Time of Week of last baseline + * @param rtk_health GPS-specific health report for RTK data. + * @param rtk_rate Rate of baseline messages being received by GPS, in HZ + * @param nsats Current number of sats used for RTK calculation. + * @param baseline_coords_type Coordinate system of baseline. 0 == ECEF, 1 == NED + * @param baseline_a_mm Current baseline in ECEF x or NED north component in mm. + * @param baseline_b_mm Current baseline in ECEF y or NED east component in mm. + * @param baseline_c_mm Current baseline in ECEF z or NED down component in mm. + * @param accuracy Current estimate of baseline accuracy. + * @param iar_num_hypotheses Current number of integer ambiguity hypotheses. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_rtk_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_RTK_LEN]; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_RTK_LEN); +#else + mavlink_gps_rtk_t packet; + packet.time_last_baseline_ms = time_last_baseline_ms; + packet.tow = tow; + packet.baseline_a_mm = baseline_a_mm; + packet.baseline_b_mm = baseline_b_mm; + packet.baseline_c_mm = baseline_c_mm; + packet.accuracy = accuracy; + packet.iar_num_hypotheses = iar_num_hypotheses; + packet.wn = wn; + packet.rtk_receiver_id = rtk_receiver_id; + packet.rtk_health = rtk_health; + packet.rtk_rate = rtk_rate; + packet.nsats = nsats; + packet.baseline_coords_type = baseline_coords_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_RTK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +} + +/** + * @brief Pack a gps_rtk message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_last_baseline_ms Time since boot of last baseline message received in ms. + * @param rtk_receiver_id Identification of connected RTK receiver. + * @param wn GPS Week Number of last baseline + * @param tow GPS Time of Week of last baseline + * @param rtk_health GPS-specific health report for RTK data. + * @param rtk_rate Rate of baseline messages being received by GPS, in HZ + * @param nsats Current number of sats used for RTK calculation. + * @param baseline_coords_type Coordinate system of baseline. 0 == ECEF, 1 == NED + * @param baseline_a_mm Current baseline in ECEF x or NED north component in mm. + * @param baseline_b_mm Current baseline in ECEF y or NED east component in mm. + * @param baseline_c_mm Current baseline in ECEF z or NED down component in mm. + * @param accuracy Current estimate of baseline accuracy. + * @param iar_num_hypotheses Current number of integer ambiguity hypotheses. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_rtk_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_last_baseline_ms,uint8_t rtk_receiver_id,uint16_t wn,uint32_t tow,uint8_t rtk_health,uint8_t rtk_rate,uint8_t nsats,uint8_t baseline_coords_type,int32_t baseline_a_mm,int32_t baseline_b_mm,int32_t baseline_c_mm,uint32_t accuracy,int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_RTK_LEN]; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_RTK_LEN); +#else + mavlink_gps_rtk_t packet; + packet.time_last_baseline_ms = time_last_baseline_ms; + packet.tow = tow; + packet.baseline_a_mm = baseline_a_mm; + packet.baseline_b_mm = baseline_b_mm; + packet.baseline_c_mm = baseline_c_mm; + packet.accuracy = accuracy; + packet.iar_num_hypotheses = iar_num_hypotheses; + packet.wn = wn; + packet.rtk_receiver_id = rtk_receiver_id; + packet.rtk_health = rtk_health; + packet.rtk_rate = rtk_rate; + packet.nsats = nsats; + packet.baseline_coords_type = baseline_coords_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_RTK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +} + +/** + * @brief Encode a gps_rtk struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gps_rtk C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_rtk_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_rtk_t* gps_rtk) +{ + return mavlink_msg_gps_rtk_pack(system_id, component_id, msg, gps_rtk->time_last_baseline_ms, gps_rtk->rtk_receiver_id, gps_rtk->wn, gps_rtk->tow, gps_rtk->rtk_health, gps_rtk->rtk_rate, gps_rtk->nsats, gps_rtk->baseline_coords_type, gps_rtk->baseline_a_mm, gps_rtk->baseline_b_mm, gps_rtk->baseline_c_mm, gps_rtk->accuracy, gps_rtk->iar_num_hypotheses); +} + +/** + * @brief Encode a gps_rtk struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps_rtk C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_rtk_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_rtk_t* gps_rtk) +{ + return mavlink_msg_gps_rtk_pack_chan(system_id, component_id, chan, msg, gps_rtk->time_last_baseline_ms, gps_rtk->rtk_receiver_id, gps_rtk->wn, gps_rtk->tow, gps_rtk->rtk_health, gps_rtk->rtk_rate, gps_rtk->nsats, gps_rtk->baseline_coords_type, gps_rtk->baseline_a_mm, gps_rtk->baseline_b_mm, gps_rtk->baseline_c_mm, gps_rtk->accuracy, gps_rtk->iar_num_hypotheses); +} + +/** + * @brief Send a gps_rtk message + * @param chan MAVLink channel to send the message + * + * @param time_last_baseline_ms Time since boot of last baseline message received in ms. + * @param rtk_receiver_id Identification of connected RTK receiver. + * @param wn GPS Week Number of last baseline + * @param tow GPS Time of Week of last baseline + * @param rtk_health GPS-specific health report for RTK data. + * @param rtk_rate Rate of baseline messages being received by GPS, in HZ + * @param nsats Current number of sats used for RTK calculation. + * @param baseline_coords_type Coordinate system of baseline. 0 == ECEF, 1 == NED + * @param baseline_a_mm Current baseline in ECEF x or NED north component in mm. + * @param baseline_b_mm Current baseline in ECEF y or NED east component in mm. + * @param baseline_c_mm Current baseline in ECEF z or NED down component in mm. + * @param accuracy Current estimate of baseline accuracy. + * @param iar_num_hypotheses Current number of integer ambiguity hypotheses. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gps_rtk_send(mavlink_channel_t chan, uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_RTK_LEN]; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, buf, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, buf, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +#else + mavlink_gps_rtk_t packet; + packet.time_last_baseline_ms = time_last_baseline_ms; + packet.tow = tow; + packet.baseline_a_mm = baseline_a_mm; + packet.baseline_b_mm = baseline_b_mm; + packet.baseline_c_mm = baseline_c_mm; + packet.accuracy = accuracy; + packet.iar_num_hypotheses = iar_num_hypotheses; + packet.wn = wn; + packet.rtk_receiver_id = rtk_receiver_id; + packet.rtk_health = rtk_health; + packet.rtk_rate = rtk_rate; + packet.nsats = nsats; + packet.baseline_coords_type = baseline_coords_type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, (const char *)&packet, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, (const char *)&packet, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GPS_RTK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps_rtk_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, buf, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, buf, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +#else + mavlink_gps_rtk_t *packet = (mavlink_gps_rtk_t *)msgbuf; + packet->time_last_baseline_ms = time_last_baseline_ms; + packet->tow = tow; + packet->baseline_a_mm = baseline_a_mm; + packet->baseline_b_mm = baseline_b_mm; + packet->baseline_c_mm = baseline_c_mm; + packet->accuracy = accuracy; + packet->iar_num_hypotheses = iar_num_hypotheses; + packet->wn = wn; + packet->rtk_receiver_id = rtk_receiver_id; + packet->rtk_health = rtk_health; + packet->rtk_rate = rtk_rate; + packet->nsats = nsats; + packet->baseline_coords_type = baseline_coords_type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, (const char *)packet, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, (const char *)packet, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GPS_RTK UNPACKING + + +/** + * @brief Get field time_last_baseline_ms from gps_rtk message + * + * @return Time since boot of last baseline message received in ms. + */ +static inline uint32_t mavlink_msg_gps_rtk_get_time_last_baseline_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field rtk_receiver_id from gps_rtk message + * + * @return Identification of connected RTK receiver. + */ +static inline uint8_t mavlink_msg_gps_rtk_get_rtk_receiver_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 30); +} + +/** + * @brief Get field wn from gps_rtk message + * + * @return GPS Week Number of last baseline + */ +static inline uint16_t mavlink_msg_gps_rtk_get_wn(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field tow from gps_rtk message + * + * @return GPS Time of Week of last baseline + */ +static inline uint32_t mavlink_msg_gps_rtk_get_tow(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field rtk_health from gps_rtk message + * + * @return GPS-specific health report for RTK data. + */ +static inline uint8_t mavlink_msg_gps_rtk_get_rtk_health(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 31); +} + +/** + * @brief Get field rtk_rate from gps_rtk message + * + * @return Rate of baseline messages being received by GPS, in HZ + */ +static inline uint8_t mavlink_msg_gps_rtk_get_rtk_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field nsats from gps_rtk message + * + * @return Current number of sats used for RTK calculation. + */ +static inline uint8_t mavlink_msg_gps_rtk_get_nsats(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 33); +} + +/** + * @brief Get field baseline_coords_type from gps_rtk message + * + * @return Coordinate system of baseline. 0 == ECEF, 1 == NED + */ +static inline uint8_t mavlink_msg_gps_rtk_get_baseline_coords_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field baseline_a_mm from gps_rtk message + * + * @return Current baseline in ECEF x or NED north component in mm. + */ +static inline int32_t mavlink_msg_gps_rtk_get_baseline_a_mm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field baseline_b_mm from gps_rtk message + * + * @return Current baseline in ECEF y or NED east component in mm. + */ +static inline int32_t mavlink_msg_gps_rtk_get_baseline_b_mm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field baseline_c_mm from gps_rtk message + * + * @return Current baseline in ECEF z or NED down component in mm. + */ +static inline int32_t mavlink_msg_gps_rtk_get_baseline_c_mm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field accuracy from gps_rtk message + * + * @return Current estimate of baseline accuracy. + */ +static inline uint32_t mavlink_msg_gps_rtk_get_accuracy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 20); +} + +/** + * @brief Get field iar_num_hypotheses from gps_rtk message + * + * @return Current number of integer ambiguity hypotheses. + */ +static inline int32_t mavlink_msg_gps_rtk_get_iar_num_hypotheses(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 24); +} + +/** + * @brief Decode a gps_rtk message into a struct + * + * @param msg The message to decode + * @param gps_rtk C-struct to decode the message contents into + */ +static inline void mavlink_msg_gps_rtk_decode(const mavlink_message_t* msg, mavlink_gps_rtk_t* gps_rtk) +{ +#if MAVLINK_NEED_BYTE_SWAP + gps_rtk->time_last_baseline_ms = mavlink_msg_gps_rtk_get_time_last_baseline_ms(msg); + gps_rtk->tow = mavlink_msg_gps_rtk_get_tow(msg); + gps_rtk->baseline_a_mm = mavlink_msg_gps_rtk_get_baseline_a_mm(msg); + gps_rtk->baseline_b_mm = mavlink_msg_gps_rtk_get_baseline_b_mm(msg); + gps_rtk->baseline_c_mm = mavlink_msg_gps_rtk_get_baseline_c_mm(msg); + gps_rtk->accuracy = mavlink_msg_gps_rtk_get_accuracy(msg); + gps_rtk->iar_num_hypotheses = mavlink_msg_gps_rtk_get_iar_num_hypotheses(msg); + gps_rtk->wn = mavlink_msg_gps_rtk_get_wn(msg); + gps_rtk->rtk_receiver_id = mavlink_msg_gps_rtk_get_rtk_receiver_id(msg); + gps_rtk->rtk_health = mavlink_msg_gps_rtk_get_rtk_health(msg); + gps_rtk->rtk_rate = mavlink_msg_gps_rtk_get_rtk_rate(msg); + gps_rtk->nsats = mavlink_msg_gps_rtk_get_nsats(msg); + gps_rtk->baseline_coords_type = mavlink_msg_gps_rtk_get_baseline_coords_type(msg); +#else + memcpy(gps_rtk, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h index 28d6b57..10659d0 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h @@ -199,6 +199,46 @@ static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t s #endif } +#if MAVLINK_MSG_ID_GPS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t satellites_visible, const uint8_t *satellite_prn, const uint8_t *satellite_used, const uint8_t *satellite_elevation, const uint8_t *satellite_azimuth, const uint8_t *satellite_snr) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, satellites_visible); + _mav_put_uint8_t_array(buf, 1, satellite_prn, 20); + _mav_put_uint8_t_array(buf, 21, satellite_used, 20); + _mav_put_uint8_t_array(buf, 41, satellite_elevation, 20); + _mav_put_uint8_t_array(buf, 61, satellite_azimuth, 20); + _mav_put_uint8_t_array(buf, 81, satellite_snr, 20); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, buf, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, buf, MAVLINK_MSG_ID_GPS_STATUS_LEN); +#endif +#else + mavlink_gps_status_t *packet = (mavlink_gps_status_t *)msgbuf; + packet->satellites_visible = satellites_visible; + mav_array_memcpy(packet->satellite_prn, satellite_prn, sizeof(uint8_t)*20); + mav_array_memcpy(packet->satellite_used, satellite_used, sizeof(uint8_t)*20); + mav_array_memcpy(packet->satellite_elevation, satellite_elevation, sizeof(uint8_t)*20); + mav_array_memcpy(packet->satellite_azimuth, satellite_azimuth, sizeof(uint8_t)*20); + mav_array_memcpy(packet->satellite_snr, satellite_snr, sizeof(uint8_t)*20); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, (const char *)packet, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, (const char *)packet, MAVLINK_MSG_ID_GPS_STATUS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GPS_STATUS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h index 826138f..733886b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h @@ -7,7 +7,7 @@ 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_FLAGS ENUM in mavlink/include/mavlink_types.h + 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; @@ -41,7 +41,7 @@ typedef struct __mavlink_heartbeat_t * * @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_FLAGS ENUM in mavlink/include/mavlink_types.h + * @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) @@ -87,7 +87,7 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com * @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_FLAGS ENUM in mavlink/include/mavlink_types.h + * @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) @@ -159,7 +159,7 @@ static inline uint16_t mavlink_msg_heartbeat_encode_chan(uint8_t system_id, uint * * @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_FLAGS ENUM in mavlink/include/mavlink_types.h + * @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 */ @@ -198,6 +198,48 @@ static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t ty #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 + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_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, 3); + +#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 = 3; + +#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); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HEARTBEAT UNPACKING @@ -226,7 +268,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_ /** * @brief Get field base_mode from heartbeat message * - * @return System mode bitfield, see MAV_MODE_FLAGS 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) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h index 0dcd95e..2749cb0 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h @@ -300,6 +300,66 @@ static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t #endif } +#if MAVLINK_MSG_ID_HIGHRES_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_highres_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, 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, xmag); + _mav_put_float(buf, 36, ymag); + _mav_put_float(buf, 40, zmag); + _mav_put_float(buf, 44, abs_pressure); + _mav_put_float(buf, 48, diff_pressure); + _mav_put_float(buf, 52, pressure_alt); + _mav_put_float(buf, 56, temperature); + _mav_put_uint16_t(buf, 60, fields_updated); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); +#endif +#else + mavlink_highres_imu_t *packet = (mavlink_highres_imu_t *)msgbuf; + packet->time_usec = time_usec; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; + packet->abs_pressure = abs_pressure; + packet->diff_pressure = diff_pressure; + packet->pressure_alt = pressure_alt; + packet->temperature = temperature; + packet->fields_updated = fields_updated; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIGHRES_IMU UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h index aed5108..f7507b1 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h @@ -256,6 +256,58 @@ static inline void mavlink_msg_hil_controls_send(mavlink_channel_t chan, uint64_ #endif } +#if MAVLINK_MSG_ID_HIL_CONTROLS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_controls_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, float aux1, float aux2, float aux3, float aux4, uint8_t mode, uint8_t nav_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll_ailerons); + _mav_put_float(buf, 12, pitch_elevator); + _mav_put_float(buf, 16, yaw_rudder); + _mav_put_float(buf, 20, throttle); + _mav_put_float(buf, 24, aux1); + _mav_put_float(buf, 28, aux2); + _mav_put_float(buf, 32, aux3); + _mav_put_float(buf, 36, aux4); + _mav_put_uint8_t(buf, 40, mode); + _mav_put_uint8_t(buf, 41, nav_mode); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); +#endif +#else + mavlink_hil_controls_t *packet = (mavlink_hil_controls_t *)msgbuf; + packet->time_usec = time_usec; + packet->roll_ailerons = roll_ailerons; + packet->pitch_elevator = pitch_elevator; + packet->yaw_rudder = yaw_rudder; + packet->throttle = throttle; + packet->aux1 = aux1; + packet->aux2 = aux2; + packet->aux3 = aux3; + packet->aux4 = aux4; + packet->mode = mode; + packet->nav_mode = nav_mode; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, (const char *)packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, (const char *)packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIL_CONTROLS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h index 91aec5b..a22c047 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h @@ -278,6 +278,62 @@ static inline void mavlink_msg_hil_gps_send(mavlink_channel_t chan, uint64_t tim #endif } +#if MAVLINK_MSG_ID_HIL_GPS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_gps_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_int16_t(buf, 26, vn); + _mav_put_int16_t(buf, 28, ve); + _mav_put_int16_t(buf, 30, vd); + _mav_put_uint16_t(buf, 32, cog); + _mav_put_uint8_t(buf, 34, fix_type); + _mav_put_uint8_t(buf, 35, satellites_visible); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif +#else + mavlink_hil_gps_t *packet = (mavlink_hil_gps_t *)msgbuf; + packet->time_usec = time_usec; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->eph = eph; + packet->epv = epv; + packet->vel = vel; + packet->vn = vn; + packet->ve = ve; + packet->vd = vd; + packet->cog = cog; + packet->fix_type = fix_type; + packet->satellites_visible = satellites_visible; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)packet, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)packet, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIL_GPS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h index acb1392..eaadf24 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h @@ -223,6 +223,52 @@ static inline void mavlink_msg_hil_optical_flow_send(mavlink_channel_t chan, uin #endif } +#if MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_optical_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, flow_comp_m_x); + _mav_put_float(buf, 12, flow_comp_m_y); + _mav_put_float(buf, 16, ground_distance); + _mav_put_int16_t(buf, 20, flow_x); + _mav_put_int16_t(buf, 22, flow_y); + _mav_put_uint8_t(buf, 24, sensor_id); + _mav_put_uint8_t(buf, 25, quality); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif +#else + mavlink_hil_optical_flow_t *packet = (mavlink_hil_optical_flow_t *)msgbuf; + packet->time_usec = time_usec; + packet->flow_comp_m_x = flow_comp_m_x; + packet->flow_comp_m_y = flow_comp_m_y; + packet->ground_distance = ground_distance; + packet->flow_x = flow_x; + packet->flow_y = flow_y; + packet->sensor_id = sensor_id; + packet->quality = quality; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIL_OPTICAL_FLOW UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h index a42bde5..227cd9d 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h @@ -289,6 +289,64 @@ static inline void mavlink_msg_hil_rc_inputs_raw_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_rc_inputs_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, 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, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 8, chan1_raw); + _mav_put_uint16_t(buf, 10, chan2_raw); + _mav_put_uint16_t(buf, 12, chan3_raw); + _mav_put_uint16_t(buf, 14, chan4_raw); + _mav_put_uint16_t(buf, 16, chan5_raw); + _mav_put_uint16_t(buf, 18, chan6_raw); + _mav_put_uint16_t(buf, 20, chan7_raw); + _mav_put_uint16_t(buf, 22, chan8_raw); + _mav_put_uint16_t(buf, 24, chan9_raw); + _mav_put_uint16_t(buf, 26, chan10_raw); + _mav_put_uint16_t(buf, 28, chan11_raw); + _mav_put_uint16_t(buf, 30, chan12_raw); + _mav_put_uint8_t(buf, 32, rssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); +#endif +#else + mavlink_hil_rc_inputs_raw_t *packet = (mavlink_hil_rc_inputs_raw_t *)msgbuf; + packet->time_usec = time_usec; + 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->rssi = rssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, (const char *)packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, (const char *)packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIL_RC_INPUTS_RAW UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h index 6c26674..8672f8b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h @@ -300,6 +300,66 @@ static inline void mavlink_msg_hil_sensor_send(mavlink_channel_t chan, uint64_t #endif } +#if MAVLINK_MSG_ID_HIL_SENSOR_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, 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, xmag); + _mav_put_float(buf, 36, ymag); + _mav_put_float(buf, 40, zmag); + _mav_put_float(buf, 44, abs_pressure); + _mav_put_float(buf, 48, diff_pressure); + _mav_put_float(buf, 52, pressure_alt); + _mav_put_float(buf, 56, temperature); + _mav_put_uint32_t(buf, 60, fields_updated); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif +#else + mavlink_hil_sensor_t *packet = (mavlink_hil_sensor_t *)msgbuf; + packet->time_usec = time_usec; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; + packet->abs_pressure = abs_pressure; + packet->diff_pressure = diff_pressure; + packet->pressure_alt = pressure_alt; + packet->temperature = temperature; + packet->fields_updated = fields_updated; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIL_SENSOR UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h index bcc8577..923ed60 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h @@ -311,6 +311,68 @@ static inline void mavlink_msg_hil_state_send(mavlink_channel_t chan, uint64_t t #endif } +#if MAVLINK_MSG_ID_HIL_STATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_state_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll); + _mav_put_float(buf, 12, pitch); + _mav_put_float(buf, 16, yaw); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_int32_t(buf, 32, lat); + _mav_put_int32_t(buf, 36, lon); + _mav_put_int32_t(buf, 40, alt); + _mav_put_int16_t(buf, 44, vx); + _mav_put_int16_t(buf, 46, vy); + _mav_put_int16_t(buf, 48, vz); + _mav_put_int16_t(buf, 50, xacc); + _mav_put_int16_t(buf, 52, yacc); + _mav_put_int16_t(buf, 54, zacc); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, buf, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, buf, MAVLINK_MSG_ID_HIL_STATE_LEN); +#endif +#else + mavlink_hil_state_t *packet = (mavlink_hil_state_t *)msgbuf; + packet->time_usec = time_usec; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->rollspeed = rollspeed; + packet->pitchspeed = pitchspeed; + packet->yawspeed = yawspeed; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIL_STATE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h index 7321761..344a19e 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h @@ -5,7 +5,7 @@ typedef struct __mavlink_hil_state_quaternion_t { uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - float attitude_quaternion[4]; ///< Vehicle attitude expressed as normalized quaternion + float attitude_quaternion[4]; ///< Vehicle attitude expressed as normalized quaternion in w, x, y, z order (with 1 0 0 0 being the null-rotation) float rollspeed; ///< Body frame roll / phi angular speed (rad/s) float pitchspeed; ///< Body frame pitch / theta angular speed (rad/s) float yawspeed; ///< Body frame yaw / psi angular speed (rad/s) @@ -60,7 +60,7 @@ typedef struct __mavlink_hil_state_quaternion_t * @param msg The MAVLink message to compress the data into * * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion + * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion in w, x, y, z order (with 1 0 0 0 being the null-rotation) * @param rollspeed Body frame roll / phi angular speed (rad/s) * @param pitchspeed Body frame pitch / theta angular speed (rad/s) * @param yawspeed Body frame yaw / psi angular speed (rad/s) @@ -135,7 +135,7 @@ static inline uint16_t mavlink_msg_hil_state_quaternion_pack(uint8_t system_id, * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion + * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion in w, x, y, z order (with 1 0 0 0 being the null-rotation) * @param rollspeed Body frame roll / phi angular speed (rad/s) * @param pitchspeed Body frame pitch / theta angular speed (rad/s) * @param yawspeed Body frame yaw / psi angular speed (rad/s) @@ -236,7 +236,7 @@ static inline uint16_t mavlink_msg_hil_state_quaternion_encode_chan(uint8_t syst * @param chan MAVLink channel to send the message * * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion + * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion in w, x, y, z order (with 1 0 0 0 being the null-rotation) * @param rollspeed Body frame roll / phi angular speed (rad/s) * @param pitchspeed Body frame pitch / theta angular speed (rad/s) * @param yawspeed Body frame yaw / psi angular speed (rad/s) @@ -305,6 +305,66 @@ static inline void mavlink_msg_hil_state_quaternion_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_state_quaternion_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, const float *attitude_quaternion, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t ind_airspeed, uint16_t true_airspeed, int16_t xacc, int16_t yacc, int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 24, rollspeed); + _mav_put_float(buf, 28, pitchspeed); + _mav_put_float(buf, 32, yawspeed); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lon); + _mav_put_int32_t(buf, 44, alt); + _mav_put_int16_t(buf, 48, vx); + _mav_put_int16_t(buf, 50, vy); + _mav_put_int16_t(buf, 52, vz); + _mav_put_uint16_t(buf, 54, ind_airspeed); + _mav_put_uint16_t(buf, 56, true_airspeed); + _mav_put_int16_t(buf, 58, xacc); + _mav_put_int16_t(buf, 60, yacc); + _mav_put_int16_t(buf, 62, zacc); + _mav_put_float_array(buf, 8, attitude_quaternion, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif +#else + mavlink_hil_state_quaternion_t *packet = (mavlink_hil_state_quaternion_t *)msgbuf; + packet->time_usec = time_usec; + packet->rollspeed = rollspeed; + packet->pitchspeed = pitchspeed; + packet->yawspeed = yawspeed; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->ind_airspeed = ind_airspeed; + packet->true_airspeed = true_airspeed; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + mav_array_memcpy(packet->attitude_quaternion, attitude_quaternion, sizeof(float)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIL_STATE_QUATERNION UNPACKING @@ -323,7 +383,7 @@ static inline uint64_t mavlink_msg_hil_state_quaternion_get_time_usec(const mavl /** * @brief Get field attitude_quaternion from hil_state_quaternion message * - * @return Vehicle attitude expressed as normalized quaternion + * @return Vehicle attitude expressed as normalized quaternion in w, x, y, z order (with 1 0 0 0 being the null-rotation) */ static inline uint16_t mavlink_msg_hil_state_quaternion_get_attitude_quaternion(const mavlink_message_t* msg, float *attitude_quaternion) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h index a0b72c0..e18a948 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_local_position_ned_send(mavlink_channel_t chan, u #endif } +#if MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_local_position_ned_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float x, float y, float z, float vx, float vy, float vz) +{ +#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, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); +#endif +#else + mavlink_local_position_ned_t *packet = (mavlink_local_position_ned_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->x = x; + packet->y = y; + packet->z = z; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOCAL_POSITION_NED UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_cov.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_cov.h new file mode 100644 index 0000000..0a94af3 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_cov.h @@ -0,0 +1,417 @@ +// MESSAGE LOCAL_POSITION_NED_COV PACKING + +#define MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV 64 + +typedef struct __mavlink_local_position_ned_cov_t +{ + uint64_t time_utc; ///< Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + float x; ///< X Position + float y; ///< Y Position + float z; ///< Z Position + float vx; ///< X Speed + float vy; ///< Y Speed + float vz; ///< Z Speed + float covariance[36]; ///< Covariance matrix (first six entries are the first ROW, next six entries are the second row, etc.) + uint8_t estimator_type; ///< Class id of the estimator this estimate originated from. +} mavlink_local_position_ned_cov_t; + +#define MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN 181 +#define MAVLINK_MSG_ID_64_LEN 181 + +#define MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC 82 +#define MAVLINK_MSG_ID_64_CRC 82 + +#define MAVLINK_MSG_LOCAL_POSITION_NED_COV_FIELD_COVARIANCE_LEN 36 + +#define MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_COV { \ + "LOCAL_POSITION_NED_COV", \ + 10, \ + { { "time_utc", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_local_position_ned_cov_t, time_utc) }, \ + { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_local_position_ned_cov_t, time_boot_ms) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_position_ned_cov_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_local_position_ned_cov_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_local_position_ned_cov_t, z) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_local_position_ned_cov_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_local_position_ned_cov_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_local_position_ned_cov_t, vz) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 36, 36, offsetof(mavlink_local_position_ned_cov_t, covariance) }, \ + { "estimator_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 180, offsetof(mavlink_local_position_ned_cov_t, estimator_type) }, \ + } \ +} + + +/** + * @brief Pack a local_position_ned_cov message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param time_utc Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. + * @param estimator_type Class id of the estimator this estimate originated from. + * @param x X Position + * @param y Y Position + * @param z Z Position + * @param vx X Speed + * @param vy Y Speed + * @param vz Z Speed + * @param covariance Covariance matrix (first six entries are the first ROW, next six entries are the second row, etc.) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_local_position_ned_cov_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint64_t time_utc, uint8_t estimator_type, float x, float y, float z, float vx, float vy, float vz, const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN]; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_float(buf, 12, x); + _mav_put_float(buf, 16, y); + _mav_put_float(buf, 20, z); + _mav_put_float(buf, 24, vx); + _mav_put_float(buf, 28, vy); + _mav_put_float(buf, 32, vz); + _mav_put_uint8_t(buf, 180, estimator_type); + _mav_put_float_array(buf, 36, covariance, 36); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#else + mavlink_local_position_ned_cov_t packet; + packet.time_utc = time_utc; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.estimator_type = estimator_type; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*36); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#endif +} + +/** + * @brief Pack a local_position_ned_cov message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param time_utc Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. + * @param estimator_type Class id of the estimator this estimate originated from. + * @param x X Position + * @param y Y Position + * @param z Z Position + * @param vx X Speed + * @param vy Y Speed + * @param vz Z Speed + * @param covariance Covariance matrix (first six entries are the first ROW, next six entries are the second row, etc.) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_local_position_ned_cov_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint64_t time_utc,uint8_t estimator_type,float x,float y,float z,float vx,float vy,float vz,const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN]; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_float(buf, 12, x); + _mav_put_float(buf, 16, y); + _mav_put_float(buf, 20, z); + _mav_put_float(buf, 24, vx); + _mav_put_float(buf, 28, vy); + _mav_put_float(buf, 32, vz); + _mav_put_uint8_t(buf, 180, estimator_type); + _mav_put_float_array(buf, 36, covariance, 36); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#else + mavlink_local_position_ned_cov_t packet; + packet.time_utc = time_utc; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.estimator_type = estimator_type; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*36); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#endif +} + +/** + * @brief Encode a local_position_ned_cov struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param local_position_ned_cov C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_position_ned_cov_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_local_position_ned_cov_t* local_position_ned_cov) +{ + return mavlink_msg_local_position_ned_cov_pack(system_id, component_id, msg, local_position_ned_cov->time_boot_ms, local_position_ned_cov->time_utc, local_position_ned_cov->estimator_type, local_position_ned_cov->x, local_position_ned_cov->y, local_position_ned_cov->z, local_position_ned_cov->vx, local_position_ned_cov->vy, local_position_ned_cov->vz, local_position_ned_cov->covariance); +} + +/** + * @brief Encode a local_position_ned_cov struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param local_position_ned_cov C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_position_ned_cov_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_position_ned_cov_t* local_position_ned_cov) +{ + return mavlink_msg_local_position_ned_cov_pack_chan(system_id, component_id, chan, msg, local_position_ned_cov->time_boot_ms, local_position_ned_cov->time_utc, local_position_ned_cov->estimator_type, local_position_ned_cov->x, local_position_ned_cov->y, local_position_ned_cov->z, local_position_ned_cov->vx, local_position_ned_cov->vy, local_position_ned_cov->vz, local_position_ned_cov->covariance); +} + +/** + * @brief Send a local_position_ned_cov message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param time_utc Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. + * @param estimator_type Class id of the estimator this estimate originated from. + * @param x X Position + * @param y Y Position + * @param z Z Position + * @param vx X Speed + * @param vy Y Speed + * @param vz Z Speed + * @param covariance Covariance matrix (first six entries are the first ROW, next six entries are the second row, etc.) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_local_position_ned_cov_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t time_utc, uint8_t estimator_type, float x, float y, float z, float vx, float vy, float vz, const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN]; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_float(buf, 12, x); + _mav_put_float(buf, 16, y); + _mav_put_float(buf, 20, z); + _mav_put_float(buf, 24, vx); + _mav_put_float(buf, 28, vy); + _mav_put_float(buf, 32, vz); + _mav_put_uint8_t(buf, 180, estimator_type); + _mav_put_float_array(buf, 36, covariance, 36); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#endif +#else + mavlink_local_position_ned_cov_t packet; + packet.time_utc = time_utc; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.estimator_type = estimator_type; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*36); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, (const char *)&packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, (const char *)&packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_local_position_ned_cov_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t time_utc, uint8_t estimator_type, float x, float y, float z, float vx, float vy, float vz, const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_float(buf, 12, x); + _mav_put_float(buf, 16, y); + _mav_put_float(buf, 20, z); + _mav_put_float(buf, 24, vx); + _mav_put_float(buf, 28, vy); + _mav_put_float(buf, 32, vz); + _mav_put_uint8_t(buf, 180, estimator_type); + _mav_put_float_array(buf, 36, covariance, 36); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#endif +#else + mavlink_local_position_ned_cov_t *packet = (mavlink_local_position_ned_cov_t *)msgbuf; + packet->time_utc = time_utc; + packet->time_boot_ms = time_boot_ms; + packet->x = x; + packet->y = y; + packet->z = z; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->estimator_type = estimator_type; + mav_array_memcpy(packet->covariance, covariance, sizeof(float)*36); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE LOCAL_POSITION_NED_COV UNPACKING + + +/** + * @brief Get field time_boot_ms from local_position_ned_cov message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_local_position_ned_cov_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field time_utc from local_position_ned_cov message + * + * @return Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. + */ +static inline uint64_t mavlink_msg_local_position_ned_cov_get_time_utc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field estimator_type from local_position_ned_cov message + * + * @return Class id of the estimator this estimate originated from. + */ +static inline uint8_t mavlink_msg_local_position_ned_cov_get_estimator_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 180); +} + +/** + * @brief Get field x from local_position_ned_cov message + * + * @return X Position + */ +static inline float mavlink_msg_local_position_ned_cov_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field y from local_position_ned_cov message + * + * @return Y Position + */ +static inline float mavlink_msg_local_position_ned_cov_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field z from local_position_ned_cov message + * + * @return Z Position + */ +static inline float mavlink_msg_local_position_ned_cov_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field vx from local_position_ned_cov message + * + * @return X Speed + */ +static inline float mavlink_msg_local_position_ned_cov_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field vy from local_position_ned_cov message + * + * @return Y Speed + */ +static inline float mavlink_msg_local_position_ned_cov_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field vz from local_position_ned_cov message + * + * @return Z Speed + */ +static inline float mavlink_msg_local_position_ned_cov_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field covariance from local_position_ned_cov message + * + * @return Covariance matrix (first six entries are the first ROW, next six entries are the second row, etc.) + */ +static inline uint16_t mavlink_msg_local_position_ned_cov_get_covariance(const mavlink_message_t* msg, float *covariance) +{ + return _MAV_RETURN_float_array(msg, covariance, 36, 36); +} + +/** + * @brief Decode a local_position_ned_cov message into a struct + * + * @param msg The message to decode + * @param local_position_ned_cov C-struct to decode the message contents into + */ +static inline void mavlink_msg_local_position_ned_cov_decode(const mavlink_message_t* msg, mavlink_local_position_ned_cov_t* local_position_ned_cov) +{ +#if MAVLINK_NEED_BYTE_SWAP + local_position_ned_cov->time_utc = mavlink_msg_local_position_ned_cov_get_time_utc(msg); + local_position_ned_cov->time_boot_ms = mavlink_msg_local_position_ned_cov_get_time_boot_ms(msg); + local_position_ned_cov->x = mavlink_msg_local_position_ned_cov_get_x(msg); + local_position_ned_cov->y = mavlink_msg_local_position_ned_cov_get_y(msg); + local_position_ned_cov->z = mavlink_msg_local_position_ned_cov_get_z(msg); + local_position_ned_cov->vx = mavlink_msg_local_position_ned_cov_get_vx(msg); + local_position_ned_cov->vy = mavlink_msg_local_position_ned_cov_get_vy(msg); + local_position_ned_cov->vz = mavlink_msg_local_position_ned_cov_get_vz(msg); + mavlink_msg_local_position_ned_cov_get_covariance(msg, local_position_ned_cov->covariance); + local_position_ned_cov->estimator_type = mavlink_msg_local_position_ned_cov_get_estimator_type(msg); +#else + memcpy(local_position_ned_cov, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h index 8c46862..af7d195 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_local_position_ned_system_global_offset_send(mavl #endif } +#if MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_local_position_ned_system_global_offset_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float x, float y, float z, float roll, float pitch, float yaw) +{ +#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, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, roll); + _mav_put_float(buf, 20, pitch); + _mav_put_float(buf, 24, yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); +#endif +#else + mavlink_local_position_ned_system_global_offset_t *packet = (mavlink_local_position_ned_system_global_offset_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->x = x; + packet->y = y; + packet->z = z; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h deleted file mode 100644 index 1794815..0000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h +++ /dev/null @@ -1,265 +0,0 @@ -// MESSAGE LOCAL_POSITION_SETPOINT PACKING - -#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT 51 - -typedef struct __mavlink_local_position_setpoint_t -{ - float x; ///< x position - float y; ///< y position - float z; ///< z position - float yaw; ///< Desired yaw angle - uint8_t coordinate_frame; ///< Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU -} mavlink_local_position_setpoint_t; - -#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN 17 -#define MAVLINK_MSG_ID_51_LEN 17 - -#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_CRC 223 -#define MAVLINK_MSG_ID_51_CRC 223 - - - -#define MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT { \ - "LOCAL_POSITION_SETPOINT", \ - 5, \ - { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_local_position_setpoint_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_local_position_setpoint_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_local_position_setpoint_t, z) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_position_setpoint_t, yaw) }, \ - { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_local_position_setpoint_t, coordinate_frame) }, \ - } \ -} - - -/** - * @brief Pack a local_position_setpoint message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU - * @param x x position - * @param y y position - * @param z z position - * @param yaw Desired yaw angle - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_local_position_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t coordinate_frame, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, coordinate_frame); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); -#else - mavlink_local_position_setpoint_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.coordinate_frame = coordinate_frame; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); -#endif -} - -/** - * @brief Pack a local_position_setpoint message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU - * @param x x position - * @param y y position - * @param z z position - * @param yaw Desired yaw angle - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_local_position_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t coordinate_frame,float x,float y,float z,float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, coordinate_frame); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); -#else - mavlink_local_position_setpoint_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.coordinate_frame = coordinate_frame; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); -#endif -} - -/** - * @brief Encode a local_position_setpoint struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param local_position_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_local_position_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_local_position_setpoint_t* local_position_setpoint) -{ - return mavlink_msg_local_position_setpoint_pack(system_id, component_id, msg, local_position_setpoint->coordinate_frame, local_position_setpoint->x, local_position_setpoint->y, local_position_setpoint->z, local_position_setpoint->yaw); -} - -/** - * @brief Encode a local_position_setpoint struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param local_position_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_local_position_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_position_setpoint_t* local_position_setpoint) -{ - return mavlink_msg_local_position_setpoint_pack_chan(system_id, component_id, chan, msg, local_position_setpoint->coordinate_frame, local_position_setpoint->x, local_position_setpoint->y, local_position_setpoint->z, local_position_setpoint->yaw); -} - -/** - * @brief Send a local_position_setpoint message - * @param chan MAVLink channel to send the message - * - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU - * @param x x position - * @param y y position - * @param z z position - * @param yaw Desired yaw angle - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t chan, uint8_t coordinate_frame, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, coordinate_frame); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); -#endif -#else - mavlink_local_position_setpoint_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.coordinate_frame = coordinate_frame; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); -#endif -#endif -} - -#endif - -// MESSAGE LOCAL_POSITION_SETPOINT UNPACKING - - -/** - * @brief Get field coordinate_frame from local_position_setpoint message - * - * @return Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU - */ -static inline uint8_t mavlink_msg_local_position_setpoint_get_coordinate_frame(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Get field x from local_position_setpoint message - * - * @return x position - */ -static inline float mavlink_msg_local_position_setpoint_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field y from local_position_setpoint message - * - * @return y position - */ -static inline float mavlink_msg_local_position_setpoint_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field z from local_position_setpoint message - * - * @return z position - */ -static inline float mavlink_msg_local_position_setpoint_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yaw from local_position_setpoint message - * - * @return Desired yaw angle - */ -static inline float mavlink_msg_local_position_setpoint_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a local_position_setpoint message into a struct - * - * @param msg The message to decode - * @param local_position_setpoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_local_position_setpoint_decode(const mavlink_message_t* msg, mavlink_local_position_setpoint_t* local_position_setpoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - local_position_setpoint->x = mavlink_msg_local_position_setpoint_get_x(msg); - local_position_setpoint->y = mavlink_msg_local_position_setpoint_get_y(msg); - local_position_setpoint->z = mavlink_msg_local_position_setpoint_get_z(msg); - local_position_setpoint->yaw = mavlink_msg_local_position_setpoint_get_yaw(msg); - local_position_setpoint->coordinate_frame = mavlink_msg_local_position_setpoint_get_coordinate_frame(msg); -#else - memcpy(local_position_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h index 1cf5d15..48641f3 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h @@ -173,6 +173,42 @@ static inline void mavlink_msg_log_data_send(mavlink_channel_t chan, uint16_t id #endif } +#if MAVLINK_MSG_ID_LOG_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t id, uint32_t ofs, uint8_t count, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint16_t(buf, 4, id); + _mav_put_uint8_t(buf, 6, count); + _mav_put_uint8_t_array(buf, 7, data, 90); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, buf, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, buf, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif +#else + mavlink_log_data_t *packet = (mavlink_log_data_t *)msgbuf; + packet->ofs = ofs; + packet->id = id; + packet->count = count; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*90); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOG_DATA UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h index 681d8f0..8ecaec3 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_log_entry_send(mavlink_channel_t chan, uint16_t i #endif } +#if MAVLINK_MSG_ID_LOG_ENTRY_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_entry_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t id, uint16_t num_logs, uint16_t last_log_num, uint32_t time_utc, uint32_t size) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 4, size); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint16_t(buf, 10, num_logs); + _mav_put_uint16_t(buf, 12, last_log_num); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif +#else + mavlink_log_entry_t *packet = (mavlink_log_entry_t *)msgbuf; + packet->time_utc = time_utc; + packet->size = size; + packet->id = id; + packet->num_logs = num_logs; + packet->last_log_num = last_log_num; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, (const char *)packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, (const char *)packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOG_ENTRY UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h index feafeaf..957c4d4 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_log_erase_send(mavlink_channel_t chan, uint8_t ta #endif } +#if MAVLINK_MSG_ID_LOG_ERASE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_erase_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); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif +#else + mavlink_log_erase_t *packet = (mavlink_log_erase_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_LOG_ERASE, (const char *)packet, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)packet, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOG_ERASE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h index 5be9eea..ef5cbb6 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_log_request_data_send(mavlink_channel_t chan, uin #endif } +#if MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_request_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t id, uint32_t ofs, uint32_t count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint32_t(buf, 4, count); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint8_t(buf, 10, target_system); + _mav_put_uint8_t(buf, 11, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif +#else + mavlink_log_request_data_t *packet = (mavlink_log_request_data_t *)msgbuf; + packet->ofs = ofs; + packet->count = count; + packet->id = id; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOG_REQUEST_DATA UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h index 48a5a03..23fcca2 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_log_request_end_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_LOG_REQUEST_END_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_request_end_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); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif +#else + mavlink_log_request_end_t *packet = (mavlink_log_request_end_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_LOG_REQUEST_END, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOG_REQUEST_END UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h index 7a5b25c..e511b53 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_log_request_list_send(mavlink_channel_t chan, uin #endif } +#if MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t start, uint16_t end) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, start); + _mav_put_uint16_t(buf, 2, end); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif +#else + mavlink_log_request_list_t *packet = (mavlink_log_request_list_t *)msgbuf; + packet->start = start; + packet->end = end; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOG_REQUEST_LIST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h index 6b6e9e1..e93b759 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8 #endif } +#if MAVLINK_MSG_ID_MANUAL_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_manual_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target, int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, x); + _mav_put_int16_t(buf, 2, y); + _mav_put_int16_t(buf, 4, z); + _mav_put_int16_t(buf, 6, r); + _mav_put_uint16_t(buf, 8, buttons); + _mav_put_uint8_t(buf, 10, target); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); +#endif +#else + mavlink_manual_control_t *packet = (mavlink_manual_control_t *)msgbuf; + packet->x = x; + packet->y = y; + packet->z = z; + packet->r = r; + packet->buttons = buttons; + packet->target = target; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MANUAL_CONTROL UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h index a694947..b276267 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_manual_setpoint_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_manual_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust, uint8_t mode_switch, uint8_t manual_override_switch) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, thrust); + _mav_put_uint8_t(buf, 20, mode_switch); + _mav_put_uint8_t(buf, 21, manual_override_switch); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); +#endif +#else + mavlink_manual_setpoint_t *packet = (mavlink_manual_setpoint_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->thrust = thrust; + packet->mode_switch = mode_switch; + packet->manual_override_switch = manual_override_switch; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MANUAL_SETPOINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h index 5f79329..2eb60cc 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h @@ -173,6 +173,42 @@ static inline void mavlink_msg_memory_vect_send(mavlink_channel_t chan, uint16_t #endif } +#if MAVLINK_MSG_ID_MEMORY_VECT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_memory_vect_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t address, uint8_t ver, uint8_t type, const int8_t *value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, address); + _mav_put_uint8_t(buf, 2, ver); + _mav_put_uint8_t(buf, 3, type); + _mav_put_int8_t_array(buf, 4, value, 32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +#endif +#else + mavlink_memory_vect_t *packet = (mavlink_memory_vect_t *)msgbuf; + packet->address = address; + packet->ver = ver; + packet->type = type; + mav_array_memcpy(packet->value, value, sizeof(int8_t)*32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, (const char *)packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, (const char *)packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MEMORY_VECT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h index 7421d83..43afd00 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_mission_ack_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_MISSION_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_LEN); +#endif +#else + mavlink_mission_ack_t *packet = (mavlink_mission_ack_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->type = type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)packet, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)packet, MAVLINK_MSG_ID_MISSION_ACK_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_ACK UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h index 8f441c8..1209868 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_mission_clear_all_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_clear_all_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); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); +#endif +#else + mavlink_mission_clear_all_t *packet = (mavlink_mission_clear_all_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_MISSION_CLEAR_ALL, (const char *)packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_CLEAR_ALL UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h index eac7793..7e4748e 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_mission_count_send(mavlink_channel_t chan, uint8_ #endif } +#if MAVLINK_MSG_ID_MISSION_COUNT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_count_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, count); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN); +#endif +#else + mavlink_mission_count_t *packet = (mavlink_mission_count_t *)msgbuf; + packet->count = count; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_COUNT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h index dbcdbd3..201b7a6 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h @@ -146,6 +146,38 @@ static inline void mavlink_msg_mission_current_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_MISSION_CURRENT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_current_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, seq); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); +#endif +#else + mavlink_mission_current_t *packet = (mavlink_mission_current_t *)msgbuf; + packet->seq = seq; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_CURRENT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h index a8d60ec..ef9394f 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h @@ -289,6 +289,64 @@ static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_MISSION_ITEM_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_item_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, x); + _mav_put_float(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, seq); + _mav_put_uint16_t(buf, 30, command); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, frame); + _mav_put_uint8_t(buf, 35, current); + _mav_put_uint8_t(buf, 36, autocontinue); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN); +#endif +#else + mavlink_mission_item_t *packet = (mavlink_mission_item_t *)msgbuf; + packet->param1 = param1; + packet->param2 = param2; + packet->param3 = param3; + packet->param4 = param4; + packet->x = x; + packet->y = y; + packet->z = z; + packet->seq = seq; + packet->command = command; + packet->target_system = target_system; + packet->target_component = target_component; + packet->frame = frame; + packet->current = current; + packet->autocontinue = autocontinue; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_ITEM UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_int.h new file mode 100644 index 0000000..58d102e --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_int.h @@ -0,0 +1,521 @@ +// MESSAGE MISSION_ITEM_INT PACKING + +#define MAVLINK_MSG_ID_MISSION_ITEM_INT 73 + +typedef struct __mavlink_mission_item_int_t +{ + float param1; ///< PARAM1, see MAV_CMD enum + float param2; ///< PARAM2, see MAV_CMD enum + float param3; ///< PARAM3, see MAV_CMD enum + float param4; ///< PARAM4, see MAV_CMD enum + int32_t x; ///< PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + int32_t y; ///< PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 + float z; ///< PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + uint16_t seq; ///< Waypoint ID (sequence number). Starts at zero. Increases monotonically for each waypoint, no gaps in the sequence (0,1,2,3,4). + uint16_t command; ///< The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t frame; ///< The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h + uint8_t current; ///< false:0, true:1 + uint8_t autocontinue; ///< autocontinue to next wp +} mavlink_mission_item_int_t; + +#define MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN 37 +#define MAVLINK_MSG_ID_73_LEN 37 + +#define MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC 38 +#define MAVLINK_MSG_ID_73_CRC 38 + + + +#define MAVLINK_MESSAGE_INFO_MISSION_ITEM_INT { \ + "MISSION_ITEM_INT", \ + 14, \ + { { "param1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_mission_item_int_t, param1) }, \ + { "param2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mission_item_int_t, param2) }, \ + { "param3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mission_item_int_t, param3) }, \ + { "param4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_mission_item_int_t, param4) }, \ + { "x", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_mission_item_int_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_mission_item_int_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mission_item_int_t, z) }, \ + { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_mission_item_int_t, seq) }, \ + { "command", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_mission_item_int_t, command) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_mission_item_int_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_mission_item_int_t, target_component) }, \ + { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_mission_item_int_t, frame) }, \ + { "current", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_mission_item_int_t, current) }, \ + { "autocontinue", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_mission_item_int_t, autocontinue) }, \ + } \ +} + + +/** + * @brief Pack a mission_item_int message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param seq Waypoint ID (sequence number). Starts at zero. Increases monotonically for each waypoint, no gaps in the sequence (0,1,2,3,4). + * @param frame The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h + * @param command The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs + * @param current false:0, true:1 + * @param autocontinue autocontinue to next wp + * @param param1 PARAM1, see MAV_CMD enum + * @param param2 PARAM2, see MAV_CMD enum + * @param param3 PARAM3, see MAV_CMD enum + * @param param4 PARAM4, see MAV_CMD enum + * @param x PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + * @param y PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 + * @param z PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_item_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_int32_t(buf, 16, x); + _mav_put_int32_t(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, seq); + _mav_put_uint16_t(buf, 30, command); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, frame); + _mav_put_uint8_t(buf, 35, current); + _mav_put_uint8_t(buf, 36, autocontinue); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#else + mavlink_mission_item_int_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + packet.seq = seq; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.current = current; + packet.autocontinue = autocontinue; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#endif +} + +/** + * @brief Pack a mission_item_int message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param seq Waypoint ID (sequence number). Starts at zero. Increases monotonically for each waypoint, no gaps in the sequence (0,1,2,3,4). + * @param frame The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h + * @param command The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs + * @param current false:0, true:1 + * @param autocontinue autocontinue to next wp + * @param param1 PARAM1, see MAV_CMD enum + * @param param2 PARAM2, see MAV_CMD enum + * @param param3 PARAM3, see MAV_CMD enum + * @param param4 PARAM4, see MAV_CMD enum + * @param x PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + * @param y PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 + * @param z PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_item_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t seq,uint8_t frame,uint16_t command,uint8_t current,uint8_t autocontinue,float param1,float param2,float param3,float param4,int32_t x,int32_t y,float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_int32_t(buf, 16, x); + _mav_put_int32_t(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, seq); + _mav_put_uint16_t(buf, 30, command); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, frame); + _mav_put_uint8_t(buf, 35, current); + _mav_put_uint8_t(buf, 36, autocontinue); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#else + mavlink_mission_item_int_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + packet.seq = seq; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.current = current; + packet.autocontinue = autocontinue; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#endif +} + +/** + * @brief Encode a mission_item_int struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mission_item_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_item_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_item_int_t* mission_item_int) +{ + return mavlink_msg_mission_item_int_pack(system_id, component_id, msg, mission_item_int->target_system, mission_item_int->target_component, mission_item_int->seq, mission_item_int->frame, mission_item_int->command, mission_item_int->current, mission_item_int->autocontinue, mission_item_int->param1, mission_item_int->param2, mission_item_int->param3, mission_item_int->param4, mission_item_int->x, mission_item_int->y, mission_item_int->z); +} + +/** + * @brief Encode a mission_item_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_item_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_item_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_item_int_t* mission_item_int) +{ + return mavlink_msg_mission_item_int_pack_chan(system_id, component_id, chan, msg, mission_item_int->target_system, mission_item_int->target_component, mission_item_int->seq, mission_item_int->frame, mission_item_int->command, mission_item_int->current, mission_item_int->autocontinue, mission_item_int->param1, mission_item_int->param2, mission_item_int->param3, mission_item_int->param4, mission_item_int->x, mission_item_int->y, mission_item_int->z); +} + +/** + * @brief Send a mission_item_int message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param seq Waypoint ID (sequence number). Starts at zero. Increases monotonically for each waypoint, no gaps in the sequence (0,1,2,3,4). + * @param frame The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h + * @param command The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs + * @param current false:0, true:1 + * @param autocontinue autocontinue to next wp + * @param param1 PARAM1, see MAV_CMD enum + * @param param2 PARAM2, see MAV_CMD enum + * @param param3 PARAM3, see MAV_CMD enum + * @param param4 PARAM4, see MAV_CMD enum + * @param x PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + * @param y PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 + * @param z PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mission_item_int_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_int32_t(buf, 16, x); + _mav_put_int32_t(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, seq); + _mav_put_uint16_t(buf, 30, command); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, frame); + _mav_put_uint8_t(buf, 35, current); + _mav_put_uint8_t(buf, 36, autocontinue); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, buf, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, buf, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#endif +#else + mavlink_mission_item_int_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + packet.seq = seq; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.current = current; + packet.autocontinue = autocontinue; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_item_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_int32_t(buf, 16, x); + _mav_put_int32_t(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, seq); + _mav_put_uint16_t(buf, 30, command); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, frame); + _mav_put_uint8_t(buf, 35, current); + _mav_put_uint8_t(buf, 36, autocontinue); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, buf, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, buf, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#endif +#else + mavlink_mission_item_int_t *packet = (mavlink_mission_item_int_t *)msgbuf; + packet->param1 = param1; + packet->param2 = param2; + packet->param3 = param3; + packet->param4 = param4; + packet->x = x; + packet->y = y; + packet->z = z; + packet->seq = seq; + packet->command = command; + packet->target_system = target_system; + packet->target_component = target_component; + packet->frame = frame; + packet->current = current; + packet->autocontinue = autocontinue; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MISSION_ITEM_INT UNPACKING + + +/** + * @brief Get field target_system from mission_item_int message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_mission_item_int_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field target_component from mission_item_int message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_mission_item_int_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 33); +} + +/** + * @brief Get field seq from mission_item_int message + * + * @return Waypoint ID (sequence number). Starts at zero. Increases monotonically for each waypoint, no gaps in the sequence (0,1,2,3,4). + */ +static inline uint16_t mavlink_msg_mission_item_int_get_seq(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field frame from mission_item_int message + * + * @return The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h + */ +static inline uint8_t mavlink_msg_mission_item_int_get_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field command from mission_item_int message + * + * @return The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs + */ +static inline uint16_t mavlink_msg_mission_item_int_get_command(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 30); +} + +/** + * @brief Get field current from mission_item_int message + * + * @return false:0, true:1 + */ +static inline uint8_t mavlink_msg_mission_item_int_get_current(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 35); +} + +/** + * @brief Get field autocontinue from mission_item_int message + * + * @return autocontinue to next wp + */ +static inline uint8_t mavlink_msg_mission_item_int_get_autocontinue(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 36); +} + +/** + * @brief Get field param1 from mission_item_int message + * + * @return PARAM1, see MAV_CMD enum + */ +static inline float mavlink_msg_mission_item_int_get_param1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field param2 from mission_item_int message + * + * @return PARAM2, see MAV_CMD enum + */ +static inline float mavlink_msg_mission_item_int_get_param2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field param3 from mission_item_int message + * + * @return PARAM3, see MAV_CMD enum + */ +static inline float mavlink_msg_mission_item_int_get_param3(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field param4 from mission_item_int message + * + * @return PARAM4, see MAV_CMD enum + */ +static inline float mavlink_msg_mission_item_int_get_param4(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field x from mission_item_int message + * + * @return PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + */ +static inline int32_t mavlink_msg_mission_item_int_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field y from mission_item_int message + * + * @return PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 + */ +static inline int32_t mavlink_msg_mission_item_int_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 20); +} + +/** + * @brief Get field z from mission_item_int message + * + * @return PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + */ +static inline float mavlink_msg_mission_item_int_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Decode a mission_item_int message into a struct + * + * @param msg The message to decode + * @param mission_item_int C-struct to decode the message contents into + */ +static inline void mavlink_msg_mission_item_int_decode(const mavlink_message_t* msg, mavlink_mission_item_int_t* mission_item_int) +{ +#if MAVLINK_NEED_BYTE_SWAP + mission_item_int->param1 = mavlink_msg_mission_item_int_get_param1(msg); + mission_item_int->param2 = mavlink_msg_mission_item_int_get_param2(msg); + mission_item_int->param3 = mavlink_msg_mission_item_int_get_param3(msg); + mission_item_int->param4 = mavlink_msg_mission_item_int_get_param4(msg); + mission_item_int->x = mavlink_msg_mission_item_int_get_x(msg); + mission_item_int->y = mavlink_msg_mission_item_int_get_y(msg); + mission_item_int->z = mavlink_msg_mission_item_int_get_z(msg); + mission_item_int->seq = mavlink_msg_mission_item_int_get_seq(msg); + mission_item_int->command = mavlink_msg_mission_item_int_get_command(msg); + mission_item_int->target_system = mavlink_msg_mission_item_int_get_target_system(msg); + mission_item_int->target_component = mavlink_msg_mission_item_int_get_target_component(msg); + mission_item_int->frame = mavlink_msg_mission_item_int_get_frame(msg); + mission_item_int->current = mavlink_msg_mission_item_int_get_current(msg); + mission_item_int->autocontinue = mavlink_msg_mission_item_int_get_autocontinue(msg); +#else + memcpy(mission_item_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h index f3744fd..9dfa280 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h @@ -146,6 +146,38 @@ static inline void mavlink_msg_mission_item_reached_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_item_reached_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, seq); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); +#endif +#else + mavlink_mission_item_reached_t *packet = (mavlink_mission_item_reached_t *)msgbuf; + packet->seq = seq; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_ITEM_REACHED UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h index ac84e35..29b0ef6 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_mission_request_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_MISSION_REQUEST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_request_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); +#endif +#else + mavlink_mission_request_t *packet = (mavlink_mission_request_t *)msgbuf; + packet->seq = seq; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_REQUEST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h index d999bab..a275348 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_mission_request_list_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_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); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); +#endif +#else + mavlink_mission_request_list_t *packet = (mavlink_mission_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_MISSION_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_REQUEST_LIST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h index 35c7e12..79a88dc 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_mission_request_partial_list_send(mavlink_channel #endif } +#if MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_request_partial_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, start_index); + _mav_put_int16_t(buf, 2, end_index); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); +#endif +#else + mavlink_mission_request_partial_list_t *packet = (mavlink_mission_request_partial_list_t *)msgbuf; + packet->start_index = start_index; + packet->end_index = end_index; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_REQUEST_PARTIAL_LIST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h index 63b37cf..0c2a3ad 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_mission_set_current_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_set_current_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); +#endif +#else + mavlink_mission_set_current_t *packet = (mavlink_mission_set_current_t *)msgbuf; + packet->seq = seq; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_SET_CURRENT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h index 7de38bd..17a5a6b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_mission_write_partial_list_send(mavlink_channel_t #endif } +#if MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_write_partial_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, start_index); + _mav_put_int16_t(buf, 2, end_index); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); +#endif +#else + mavlink_mission_write_partial_list_t *packet = (mavlink_mission_write_partial_list_t *)msgbuf; + packet->start_index = start_index; + packet->end_index = end_index; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_WRITE_PARTIAL_LIST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h index fbf4f75..df82704 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h @@ -162,6 +162,40 @@ static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_named_value_float_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, const char *name, float value) +{ +#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, value); + _mav_put_char_array(buf, 8, name, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); +#endif +#else + mavlink_named_value_float_t *packet = (mavlink_named_value_float_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->value = value; + mav_array_memcpy(packet->name, name, sizeof(char)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE NAMED_VALUE_FLOAT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h index 052f247..cfdd73d 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h @@ -162,6 +162,40 @@ static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_named_value_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, const char *name, int32_t value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, value); + _mav_put_char_array(buf, 8, name, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); +#endif +#else + mavlink_named_value_int_t *packet = (mavlink_named_value_int_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->value = value; + mav_array_memcpy(packet->name, name, sizeof(char)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE NAMED_VALUE_INT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h index e95c144..b9a748b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h @@ -223,6 +223,52 @@ static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan #endif } +#if MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_nav_controller_output_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, nav_roll); + _mav_put_float(buf, 4, nav_pitch); + _mav_put_float(buf, 8, alt_error); + _mav_put_float(buf, 12, aspd_error); + _mav_put_float(buf, 16, xtrack_error); + _mav_put_int16_t(buf, 20, nav_bearing); + _mav_put_int16_t(buf, 22, target_bearing); + _mav_put_uint16_t(buf, 24, wp_dist); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); +#endif +#else + mavlink_nav_controller_output_t *packet = (mavlink_nav_controller_output_t *)msgbuf; + packet->nav_roll = nav_roll; + packet->nav_pitch = nav_pitch; + packet->alt_error = alt_error; + packet->aspd_error = aspd_error; + packet->xtrack_error = xtrack_error; + packet->nav_bearing = nav_bearing; + packet->target_bearing = target_bearing; + packet->wp_dist = wp_dist; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, (const char *)packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, (const char *)packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE NAV_CONTROLLER_OUTPUT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h deleted file mode 100644 index 4debb6e..0000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h +++ /dev/null @@ -1,282 +0,0 @@ -// MESSAGE OMNIDIRECTIONAL_FLOW PACKING - -#define MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW 106 - -typedef struct __mavlink_omnidirectional_flow_t -{ - uint64_t time_usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot) - float front_distance_m; ///< Front distance in meters. Positive value (including zero): distance known. Negative value: Unknown distance - int16_t left[10]; ///< Flow in deci pixels (1 = 0.1 pixel) on left hemisphere - int16_t right[10]; ///< Flow in deci pixels (1 = 0.1 pixel) on right hemisphere - uint8_t sensor_id; ///< Sensor ID - uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality -} mavlink_omnidirectional_flow_t; - -#define MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN 54 -#define MAVLINK_MSG_ID_106_LEN 54 - -#define MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_CRC 211 -#define MAVLINK_MSG_ID_106_CRC 211 - -#define MAVLINK_MSG_OMNIDIRECTIONAL_FLOW_FIELD_LEFT_LEN 10 -#define MAVLINK_MSG_OMNIDIRECTIONAL_FLOW_FIELD_RIGHT_LEN 10 - -#define MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW { \ - "OMNIDIRECTIONAL_FLOW", \ - 6, \ - { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_omnidirectional_flow_t, time_usec) }, \ - { "front_distance_m", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_omnidirectional_flow_t, front_distance_m) }, \ - { "left", NULL, MAVLINK_TYPE_INT16_T, 10, 12, offsetof(mavlink_omnidirectional_flow_t, left) }, \ - { "right", NULL, MAVLINK_TYPE_INT16_T, 10, 32, offsetof(mavlink_omnidirectional_flow_t, right) }, \ - { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_omnidirectional_flow_t, sensor_id) }, \ - { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 53, offsetof(mavlink_omnidirectional_flow_t, quality) }, \ - } \ -} - - -/** - * @brief Pack a omnidirectional_flow message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param sensor_id Sensor ID - * @param left Flow in deci pixels (1 = 0.1 pixel) on left hemisphere - * @param right Flow in deci pixels (1 = 0.1 pixel) on right hemisphere - * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality - * @param front_distance_m Front distance in meters. Positive value (including zero): distance known. Negative value: Unknown distance - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_omnidirectional_flow_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t sensor_id, const int16_t *left, const int16_t *right, uint8_t quality, float front_distance_m) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, front_distance_m); - _mav_put_uint8_t(buf, 52, sensor_id); - _mav_put_uint8_t(buf, 53, quality); - _mav_put_int16_t_array(buf, 12, left, 10); - _mav_put_int16_t_array(buf, 32, right, 10); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); -#else - mavlink_omnidirectional_flow_t packet; - packet.time_usec = time_usec; - packet.front_distance_m = front_distance_m; - packet.sensor_id = sensor_id; - packet.quality = quality; - mav_array_memcpy(packet.left, left, sizeof(int16_t)*10); - mav_array_memcpy(packet.right, right, sizeof(int16_t)*10); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); -#endif -} - -/** - * @brief Pack a omnidirectional_flow message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param sensor_id Sensor ID - * @param left Flow in deci pixels (1 = 0.1 pixel) on left hemisphere - * @param right Flow in deci pixels (1 = 0.1 pixel) on right hemisphere - * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality - * @param front_distance_m Front distance in meters. Positive value (including zero): distance known. Negative value: Unknown distance - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_omnidirectional_flow_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t time_usec,uint8_t sensor_id,const int16_t *left,const int16_t *right,uint8_t quality,float front_distance_m) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, front_distance_m); - _mav_put_uint8_t(buf, 52, sensor_id); - _mav_put_uint8_t(buf, 53, quality); - _mav_put_int16_t_array(buf, 12, left, 10); - _mav_put_int16_t_array(buf, 32, right, 10); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); -#else - mavlink_omnidirectional_flow_t packet; - packet.time_usec = time_usec; - packet.front_distance_m = front_distance_m; - packet.sensor_id = sensor_id; - packet.quality = quality; - mav_array_memcpy(packet.left, left, sizeof(int16_t)*10); - mav_array_memcpy(packet.right, right, sizeof(int16_t)*10); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); -#endif -} - -/** - * @brief Encode a omnidirectional_flow struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param omnidirectional_flow C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_omnidirectional_flow_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_omnidirectional_flow_t* omnidirectional_flow) -{ - return mavlink_msg_omnidirectional_flow_pack(system_id, component_id, msg, omnidirectional_flow->time_usec, omnidirectional_flow->sensor_id, omnidirectional_flow->left, omnidirectional_flow->right, omnidirectional_flow->quality, omnidirectional_flow->front_distance_m); -} - -/** - * @brief Encode a omnidirectional_flow struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param omnidirectional_flow C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_omnidirectional_flow_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_omnidirectional_flow_t* omnidirectional_flow) -{ - return mavlink_msg_omnidirectional_flow_pack_chan(system_id, component_id, chan, msg, omnidirectional_flow->time_usec, omnidirectional_flow->sensor_id, omnidirectional_flow->left, omnidirectional_flow->right, omnidirectional_flow->quality, omnidirectional_flow->front_distance_m); -} - -/** - * @brief Send a omnidirectional_flow message - * @param chan MAVLink channel to send the message - * - * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param sensor_id Sensor ID - * @param left Flow in deci pixels (1 = 0.1 pixel) on left hemisphere - * @param right Flow in deci pixels (1 = 0.1 pixel) on right hemisphere - * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality - * @param front_distance_m Front distance in meters. Positive value (including zero): distance known. Negative value: Unknown distance - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_omnidirectional_flow_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, const int16_t *left, const int16_t *right, uint8_t quality, float front_distance_m) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, front_distance_m); - _mav_put_uint8_t(buf, 52, sensor_id); - _mav_put_uint8_t(buf, 53, quality); - _mav_put_int16_t_array(buf, 12, left, 10); - _mav_put_int16_t_array(buf, 32, right, 10); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, buf, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, buf, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); -#endif -#else - mavlink_omnidirectional_flow_t packet; - packet.time_usec = time_usec; - packet.front_distance_m = front_distance_m; - packet.sensor_id = sensor_id; - packet.quality = quality; - mav_array_memcpy(packet.left, left, sizeof(int16_t)*10); - mav_array_memcpy(packet.right, right, sizeof(int16_t)*10); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, (const char *)&packet, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, (const char *)&packet, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); -#endif -#endif -} - -#endif - -// MESSAGE OMNIDIRECTIONAL_FLOW UNPACKING - - -/** - * @brief Get field time_usec from omnidirectional_flow message - * - * @return Timestamp (microseconds, synced to UNIX time or since system boot) - */ -static inline uint64_t mavlink_msg_omnidirectional_flow_get_time_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field sensor_id from omnidirectional_flow message - * - * @return Sensor ID - */ -static inline uint8_t mavlink_msg_omnidirectional_flow_get_sensor_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 52); -} - -/** - * @brief Get field left from omnidirectional_flow message - * - * @return Flow in deci pixels (1 = 0.1 pixel) on left hemisphere - */ -static inline uint16_t mavlink_msg_omnidirectional_flow_get_left(const mavlink_message_t* msg, int16_t *left) -{ - return _MAV_RETURN_int16_t_array(msg, left, 10, 12); -} - -/** - * @brief Get field right from omnidirectional_flow message - * - * @return Flow in deci pixels (1 = 0.1 pixel) on right hemisphere - */ -static inline uint16_t mavlink_msg_omnidirectional_flow_get_right(const mavlink_message_t* msg, int16_t *right) -{ - return _MAV_RETURN_int16_t_array(msg, right, 10, 32); -} - -/** - * @brief Get field quality from omnidirectional_flow message - * - * @return Optical flow quality / confidence. 0: bad, 255: maximum quality - */ -static inline uint8_t mavlink_msg_omnidirectional_flow_get_quality(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 53); -} - -/** - * @brief Get field front_distance_m from omnidirectional_flow message - * - * @return Front distance in meters. Positive value (including zero): distance known. Negative value: Unknown distance - */ -static inline float mavlink_msg_omnidirectional_flow_get_front_distance_m(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Decode a omnidirectional_flow message into a struct - * - * @param msg The message to decode - * @param omnidirectional_flow C-struct to decode the message contents into - */ -static inline void mavlink_msg_omnidirectional_flow_decode(const mavlink_message_t* msg, mavlink_omnidirectional_flow_t* omnidirectional_flow) -{ -#if MAVLINK_NEED_BYTE_SWAP - omnidirectional_flow->time_usec = mavlink_msg_omnidirectional_flow_get_time_usec(msg); - omnidirectional_flow->front_distance_m = mavlink_msg_omnidirectional_flow_get_front_distance_m(msg); - mavlink_msg_omnidirectional_flow_get_left(msg, omnidirectional_flow->left); - mavlink_msg_omnidirectional_flow_get_right(msg, omnidirectional_flow->right); - omnidirectional_flow->sensor_id = mavlink_msg_omnidirectional_flow_get_sensor_id(msg); - omnidirectional_flow->quality = mavlink_msg_omnidirectional_flow_get_quality(msg); -#else - memcpy(omnidirectional_flow, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h index cf6db91..6a2efc6 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h @@ -223,6 +223,52 @@ static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_ #endif } +#if MAVLINK_MSG_ID_OPTICAL_FLOW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_optical_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, flow_comp_m_x); + _mav_put_float(buf, 12, flow_comp_m_y); + _mav_put_float(buf, 16, ground_distance); + _mav_put_int16_t(buf, 20, flow_x); + _mav_put_int16_t(buf, 22, flow_y); + _mav_put_uint8_t(buf, 24, sensor_id); + _mav_put_uint8_t(buf, 25, quality); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); +#endif +#else + mavlink_optical_flow_t *packet = (mavlink_optical_flow_t *)msgbuf; + packet->time_usec = time_usec; + packet->flow_comp_m_x = flow_comp_m_x; + packet->flow_comp_m_y = flow_comp_m_y; + packet->ground_distance = ground_distance; + packet->flow_x = flow_x; + packet->flow_y = flow_y; + packet->sensor_id = sensor_id; + packet->quality = quality; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE OPTICAL_FLOW UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow_rad.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow_rad.h new file mode 100644 index 0000000..316dd99 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow_rad.h @@ -0,0 +1,377 @@ +// MESSAGE OPTICAL_FLOW_RAD PACKING + +#define MAVLINK_MSG_ID_OPTICAL_FLOW_RAD 106 + +typedef struct __mavlink_optical_flow_rad_t +{ + uint64_t time_usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot) + uint32_t integration_time_us; ///< Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + float integrated_x; ///< Flow in radians around X axis (in Y direction, clockwise rotation inducing positive speed, motion ALONG the positive Y axis inducing positive flow AROUND the X axis) + float integrated_y; ///< Flow in radians around Y axis (in X direction, clockwise rotation inducing positive speed, motion ALONG the positive X axis inducing positive flow AROUND the Y axis) + uint32_t time_delta_distance_us; ///< Time in microseconds since the distance was sampled. + float distance; ///< Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. + uint8_t sensor_id; ///< Sensor ID + uint8_t quality; ///< Optical flow quality / confidence. 0: no valid flow, 255: maximum quality +} mavlink_optical_flow_rad_t; + +#define MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN 30 +#define MAVLINK_MSG_ID_106_LEN 30 + +#define MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_CRC 102 +#define MAVLINK_MSG_ID_106_CRC 102 + + + +#define MAVLINK_MESSAGE_INFO_OPTICAL_FLOW_RAD { \ + "OPTICAL_FLOW_RAD", \ + 8, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_optical_flow_rad_t, time_usec) }, \ + { "integration_time_us", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_optical_flow_rad_t, integration_time_us) }, \ + { "integrated_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_optical_flow_rad_t, integrated_x) }, \ + { "integrated_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_optical_flow_rad_t, integrated_y) }, \ + { "time_delta_distance_us", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_optical_flow_rad_t, time_delta_distance_us) }, \ + { "distance", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_optical_flow_rad_t, distance) }, \ + { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_optical_flow_rad_t, sensor_id) }, \ + { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_optical_flow_rad_t, quality) }, \ + } \ +} + + +/** + * @brief Pack a optical_flow_rad message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param sensor_id Sensor ID + * @param integration_time_us Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + * @param integrated_x Flow in radians around X axis (in Y direction, clockwise rotation inducing positive speed, motion ALONG the positive Y axis inducing positive flow AROUND the X axis) + * @param integrated_y Flow in radians around Y axis (in X direction, clockwise rotation inducing positive speed, motion ALONG the positive X axis inducing positive flow AROUND the Y axis) + * @param quality Optical flow quality / confidence. 0: no valid flow, 255: maximum quality + * @param time_delta_distance_us Time in microseconds since the distance was sampled. + * @param distance Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_optical_flow_rad_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t sensor_id, uint32_t integration_time_us, float integrated_x, float integrated_y, uint8_t quality, uint32_t time_delta_distance_us, float distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, integration_time_us); + _mav_put_float(buf, 12, integrated_x); + _mav_put_float(buf, 16, integrated_y); + _mav_put_uint32_t(buf, 20, time_delta_distance_us); + _mav_put_float(buf, 24, distance); + _mav_put_uint8_t(buf, 28, sensor_id); + _mav_put_uint8_t(buf, 29, quality); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#else + mavlink_optical_flow_rad_t packet; + packet.time_usec = time_usec; + packet.integration_time_us = integration_time_us; + packet.integrated_x = integrated_x; + packet.integrated_y = integrated_y; + packet.time_delta_distance_us = time_delta_distance_us; + packet.distance = distance; + packet.sensor_id = sensor_id; + packet.quality = quality; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW_RAD; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#endif +} + +/** + * @brief Pack a optical_flow_rad message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param sensor_id Sensor ID + * @param integration_time_us Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + * @param integrated_x Flow in radians around X axis (in Y direction, clockwise rotation inducing positive speed, motion ALONG the positive Y axis inducing positive flow AROUND the X axis) + * @param integrated_y Flow in radians around Y axis (in X direction, clockwise rotation inducing positive speed, motion ALONG the positive X axis inducing positive flow AROUND the Y axis) + * @param quality Optical flow quality / confidence. 0: no valid flow, 255: maximum quality + * @param time_delta_distance_us Time in microseconds since the distance was sampled. + * @param distance Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_optical_flow_rad_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t sensor_id,uint32_t integration_time_us,float integrated_x,float integrated_y,uint8_t quality,uint32_t time_delta_distance_us,float distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, integration_time_us); + _mav_put_float(buf, 12, integrated_x); + _mav_put_float(buf, 16, integrated_y); + _mav_put_uint32_t(buf, 20, time_delta_distance_us); + _mav_put_float(buf, 24, distance); + _mav_put_uint8_t(buf, 28, sensor_id); + _mav_put_uint8_t(buf, 29, quality); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#else + mavlink_optical_flow_rad_t packet; + packet.time_usec = time_usec; + packet.integration_time_us = integration_time_us; + packet.integrated_x = integrated_x; + packet.integrated_y = integrated_y; + packet.time_delta_distance_us = time_delta_distance_us; + packet.distance = distance; + packet.sensor_id = sensor_id; + packet.quality = quality; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW_RAD; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#endif +} + +/** + * @brief Encode a optical_flow_rad struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param optical_flow_rad C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_optical_flow_rad_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_optical_flow_rad_t* optical_flow_rad) +{ + return mavlink_msg_optical_flow_rad_pack(system_id, component_id, msg, optical_flow_rad->time_usec, optical_flow_rad->sensor_id, optical_flow_rad->integration_time_us, optical_flow_rad->integrated_x, optical_flow_rad->integrated_y, optical_flow_rad->quality, optical_flow_rad->time_delta_distance_us, optical_flow_rad->distance); +} + +/** + * @brief Encode a optical_flow_rad struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param optical_flow_rad C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_optical_flow_rad_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_optical_flow_rad_t* optical_flow_rad) +{ + return mavlink_msg_optical_flow_rad_pack_chan(system_id, component_id, chan, msg, optical_flow_rad->time_usec, optical_flow_rad->sensor_id, optical_flow_rad->integration_time_us, optical_flow_rad->integrated_x, optical_flow_rad->integrated_y, optical_flow_rad->quality, optical_flow_rad->time_delta_distance_us, optical_flow_rad->distance); +} + +/** + * @brief Send a optical_flow_rad message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param sensor_id Sensor ID + * @param integration_time_us Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + * @param integrated_x Flow in radians around X axis (in Y direction, clockwise rotation inducing positive speed, motion ALONG the positive Y axis inducing positive flow AROUND the X axis) + * @param integrated_y Flow in radians around Y axis (in X direction, clockwise rotation inducing positive speed, motion ALONG the positive X axis inducing positive flow AROUND the Y axis) + * @param quality Optical flow quality / confidence. 0: no valid flow, 255: maximum quality + * @param time_delta_distance_us Time in microseconds since the distance was sampled. + * @param distance Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_optical_flow_rad_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, uint32_t integration_time_us, float integrated_x, float integrated_y, uint8_t quality, uint32_t time_delta_distance_us, float distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, integration_time_us); + _mav_put_float(buf, 12, integrated_x); + _mav_put_float(buf, 16, integrated_y); + _mav_put_uint32_t(buf, 20, time_delta_distance_us); + _mav_put_float(buf, 24, distance); + _mav_put_uint8_t(buf, 28, sensor_id); + _mav_put_uint8_t(buf, 29, quality); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#endif +#else + mavlink_optical_flow_rad_t packet; + packet.time_usec = time_usec; + packet.integration_time_us = integration_time_us; + packet.integrated_x = integrated_x; + packet.integrated_y = integrated_y; + packet.time_delta_distance_us = time_delta_distance_us; + packet.distance = distance; + packet.sensor_id = sensor_id; + packet.quality = quality; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, (const char *)&packet, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, (const char *)&packet, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_optical_flow_rad_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, uint32_t integration_time_us, float integrated_x, float integrated_y, uint8_t quality, uint32_t time_delta_distance_us, float distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, integration_time_us); + _mav_put_float(buf, 12, integrated_x); + _mav_put_float(buf, 16, integrated_y); + _mav_put_uint32_t(buf, 20, time_delta_distance_us); + _mav_put_float(buf, 24, distance); + _mav_put_uint8_t(buf, 28, sensor_id); + _mav_put_uint8_t(buf, 29, quality); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#endif +#else + mavlink_optical_flow_rad_t *packet = (mavlink_optical_flow_rad_t *)msgbuf; + packet->time_usec = time_usec; + packet->integration_time_us = integration_time_us; + packet->integrated_x = integrated_x; + packet->integrated_y = integrated_y; + packet->time_delta_distance_us = time_delta_distance_us; + packet->distance = distance; + packet->sensor_id = sensor_id; + packet->quality = quality; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, (const char *)packet, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, (const char *)packet, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE OPTICAL_FLOW_RAD UNPACKING + + +/** + * @brief Get field time_usec from optical_flow_rad message + * + * @return Timestamp (microseconds, synced to UNIX time or since system boot) + */ +static inline uint64_t mavlink_msg_optical_flow_rad_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field sensor_id from optical_flow_rad message + * + * @return Sensor ID + */ +static inline uint8_t mavlink_msg_optical_flow_rad_get_sensor_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 28); +} + +/** + * @brief Get field integration_time_us from optical_flow_rad message + * + * @return Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + */ +static inline uint32_t mavlink_msg_optical_flow_rad_get_integration_time_us(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field integrated_x from optical_flow_rad message + * + * @return Flow in radians around X axis (in Y direction, clockwise rotation inducing positive speed, motion ALONG the positive Y axis inducing positive flow AROUND the X axis) + */ +static inline float mavlink_msg_optical_flow_rad_get_integrated_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field integrated_y from optical_flow_rad message + * + * @return Flow in radians around Y axis (in X direction, clockwise rotation inducing positive speed, motion ALONG the positive X axis inducing positive flow AROUND the Y axis) + */ +static inline float mavlink_msg_optical_flow_rad_get_integrated_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field quality from optical_flow_rad message + * + * @return Optical flow quality / confidence. 0: no valid flow, 255: maximum quality + */ +static inline uint8_t mavlink_msg_optical_flow_rad_get_quality(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 29); +} + +/** + * @brief Get field time_delta_distance_us from optical_flow_rad message + * + * @return Time in microseconds since the distance was sampled. + */ +static inline uint32_t mavlink_msg_optical_flow_rad_get_time_delta_distance_us(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 20); +} + +/** + * @brief Get field distance from optical_flow_rad message + * + * @return Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. + */ +static inline float mavlink_msg_optical_flow_rad_get_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Decode a optical_flow_rad message into a struct + * + * @param msg The message to decode + * @param optical_flow_rad C-struct to decode the message contents into + */ +static inline void mavlink_msg_optical_flow_rad_decode(const mavlink_message_t* msg, mavlink_optical_flow_rad_t* optical_flow_rad) +{ +#if MAVLINK_NEED_BYTE_SWAP + optical_flow_rad->time_usec = mavlink_msg_optical_flow_rad_get_time_usec(msg); + optical_flow_rad->integration_time_us = mavlink_msg_optical_flow_rad_get_integration_time_us(msg); + optical_flow_rad->integrated_x = mavlink_msg_optical_flow_rad_get_integrated_x(msg); + optical_flow_rad->integrated_y = mavlink_msg_optical_flow_rad_get_integrated_y(msg); + optical_flow_rad->time_delta_distance_us = mavlink_msg_optical_flow_rad_get_time_delta_distance_us(msg); + optical_flow_rad->distance = mavlink_msg_optical_flow_rad_get_distance(msg); + optical_flow_rad->sensor_id = mavlink_msg_optical_flow_rad_get_sensor_id(msg); + optical_flow_rad->quality = mavlink_msg_optical_flow_rad_get_quality(msg); +#else + memcpy(optical_flow_rad, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h index 39e0072..f9466b0 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, u #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 + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_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); + +#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 +#else + 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 +#endif +} +#endif + #endif // MESSAGE PARAM_REQUEST_LIST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h index 5d91131..730cff0 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h @@ -173,6 +173,42 @@ static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, u #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 + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_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); +#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 +#endif +} +#endif + #endif // MESSAGE PARAM_REQUEST_READ UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h index 1bd1f00..f669af1 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h @@ -184,6 +184,44 @@ static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t ta #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 + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_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); +#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 +#endif +} +#endif + #endif // MESSAGE PARAM_SET UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h index 17c7598..c279579 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h @@ -184,6 +184,44 @@ static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const ch #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 + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_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); +#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 +#endif +} +#endif + #endif // MESSAGE PARAM_VALUE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h index 0c68ca1..6564ed0 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h @@ -4,7 +4,7 @@ typedef struct __mavlink_ping_t { - uint64_t time_usec; ///< Unix timestamp in microseconds + uint64_t time_usec; ///< Unix timestamp in microseconds or since system boot if smaller than MAVLink epoch (1.1.2009) uint32_t seq; ///< PING sequence uint8_t target_system; ///< 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system uint8_t target_component; ///< 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system @@ -35,7 +35,7 @@ typedef struct __mavlink_ping_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_usec Unix timestamp in microseconds + * @param time_usec Unix timestamp in microseconds or since system boot if smaller than MAVLink epoch (1.1.2009) * @param seq PING sequence * @param target_system 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system * @param target_component 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system @@ -76,7 +76,7 @@ static inline uint16_t mavlink_msg_ping_pack(uint8_t system_id, uint8_t componen * @param component_id ID of this component (e.g. 200 for IMU) * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into - * @param time_usec Unix timestamp in microseconds + * @param time_usec Unix timestamp in microseconds or since system boot if smaller than MAVLink epoch (1.1.2009) * @param seq PING sequence * @param target_system 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system * @param target_component 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system @@ -143,7 +143,7 @@ static inline uint16_t mavlink_msg_ping_encode_chan(uint8_t system_id, uint8_t c * @brief Send a ping message * @param chan MAVLink channel to send the message * - * @param time_usec Unix timestamp in microseconds + * @param time_usec Unix timestamp in microseconds or since system boot if smaller than MAVLink epoch (1.1.2009) * @param seq PING sequence * @param target_system 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system * @param target_component 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system @@ -179,6 +179,44 @@ static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint64_t time_u #endif } +#if MAVLINK_MSG_ID_PING_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_ping_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint32_t seq, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, seq); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, buf, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, buf, MAVLINK_MSG_ID_PING_LEN); +#endif +#else + mavlink_ping_t *packet = (mavlink_ping_t *)msgbuf; + packet->time_usec = time_usec; + packet->seq = seq; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)packet, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)packet, MAVLINK_MSG_ID_PING_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE PING UNPACKING @@ -187,7 +225,7 @@ static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint64_t time_u /** * @brief Get field time_usec from ping message * - * @return Unix timestamp in microseconds + * @return Unix timestamp in microseconds or since system boot if smaller than MAVLink epoch (1.1.2009) */ static inline uint64_t mavlink_msg_ping_get_time_usec(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_position_target_global_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_position_target_global_int.h new file mode 100644 index 0000000..f893f7f --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_position_target_global_int.h @@ -0,0 +1,521 @@ +// MESSAGE POSITION_TARGET_GLOBAL_INT PACKING + +#define MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT 87 + +typedef struct __mavlink_position_target_global_int_t +{ + uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + int32_t lat_int; ///< X Position in WGS84 frame in 1e7 * meters + int32_t lon_int; ///< Y Position in WGS84 frame in 1e7 * meters + float alt; ///< Altitude in meters in WGS84 altitude, not AMSL if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + float vx; ///< X velocity in NED frame in meter / s + float vy; ///< Y velocity in NED frame in meter / s + float vz; ///< Z velocity in NED frame in meter / s + float afx; ///< X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float afy; ///< Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float afz; ///< Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float yaw; ///< yaw setpoint in rad + float yaw_rate; ///< yaw rate setpoint in rad/s + uint16_t type_mask; ///< Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + uint8_t coordinate_frame; ///< Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 +} mavlink_position_target_global_int_t; + +#define MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN 51 +#define MAVLINK_MSG_ID_87_LEN 51 + +#define MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_CRC 150 +#define MAVLINK_MSG_ID_87_CRC 150 + + + +#define MAVLINK_MESSAGE_INFO_POSITION_TARGET_GLOBAL_INT { \ + "POSITION_TARGET_GLOBAL_INT", \ + 14, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_position_target_global_int_t, time_boot_ms) }, \ + { "lat_int", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_position_target_global_int_t, lat_int) }, \ + { "lon_int", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_position_target_global_int_t, lon_int) }, \ + { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_position_target_global_int_t, alt) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_position_target_global_int_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_position_target_global_int_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_position_target_global_int_t, vz) }, \ + { "afx", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_position_target_global_int_t, afx) }, \ + { "afy", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_position_target_global_int_t, afy) }, \ + { "afz", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_position_target_global_int_t, afz) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_position_target_global_int_t, yaw) }, \ + { "yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_position_target_global_int_t, yaw_rate) }, \ + { "type_mask", NULL, MAVLINK_TYPE_UINT16_T, 0, 48, offsetof(mavlink_position_target_global_int_t, type_mask) }, \ + { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 50, offsetof(mavlink_position_target_global_int_t, coordinate_frame) }, \ + } \ +} + + +/** + * @brief Pack a position_target_global_int message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param lat_int X Position in WGS84 frame in 1e7 * meters + * @param lon_int Y Position in WGS84 frame in 1e7 * meters + * @param alt Altitude in meters in WGS84 altitude, not AMSL if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_position_target_global_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t coordinate_frame, uint16_t type_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#else + mavlink_position_target_global_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_int = lat_int; + packet.lon_int = lon_int; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +} + +/** + * @brief Pack a position_target_global_int message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param lat_int X Position in WGS84 frame in 1e7 * meters + * @param lon_int Y Position in WGS84 frame in 1e7 * meters + * @param alt Altitude in meters in WGS84 altitude, not AMSL if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_position_target_global_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t coordinate_frame,uint16_t type_mask,int32_t lat_int,int32_t lon_int,float alt,float vx,float vy,float vz,float afx,float afy,float afz,float yaw,float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#else + mavlink_position_target_global_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_int = lat_int; + packet.lon_int = lon_int; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +} + +/** + * @brief Encode a position_target_global_int struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param position_target_global_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_position_target_global_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_position_target_global_int_t* position_target_global_int) +{ + return mavlink_msg_position_target_global_int_pack(system_id, component_id, msg, position_target_global_int->time_boot_ms, position_target_global_int->coordinate_frame, position_target_global_int->type_mask, position_target_global_int->lat_int, position_target_global_int->lon_int, position_target_global_int->alt, position_target_global_int->vx, position_target_global_int->vy, position_target_global_int->vz, position_target_global_int->afx, position_target_global_int->afy, position_target_global_int->afz, position_target_global_int->yaw, position_target_global_int->yaw_rate); +} + +/** + * @brief Encode a position_target_global_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param position_target_global_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_position_target_global_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_position_target_global_int_t* position_target_global_int) +{ + return mavlink_msg_position_target_global_int_pack_chan(system_id, component_id, chan, msg, position_target_global_int->time_boot_ms, position_target_global_int->coordinate_frame, position_target_global_int->type_mask, position_target_global_int->lat_int, position_target_global_int->lon_int, position_target_global_int->alt, position_target_global_int->vx, position_target_global_int->vy, position_target_global_int->vz, position_target_global_int->afx, position_target_global_int->afy, position_target_global_int->afz, position_target_global_int->yaw, position_target_global_int->yaw_rate); +} + +/** + * @brief Send a position_target_global_int message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param lat_int X Position in WGS84 frame in 1e7 * meters + * @param lon_int Y Position in WGS84 frame in 1e7 * meters + * @param alt Altitude in meters in WGS84 altitude, not AMSL if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_position_target_global_int_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t coordinate_frame, uint16_t type_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, buf, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, buf, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +#else + mavlink_position_target_global_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_int = lat_int; + packet.lon_int = lon_int; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, (const char *)&packet, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, (const char *)&packet, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_position_target_global_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t coordinate_frame, uint16_t type_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, buf, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, buf, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +#else + mavlink_position_target_global_int_t *packet = (mavlink_position_target_global_int_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->lat_int = lat_int; + packet->lon_int = lon_int; + packet->alt = alt; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->afx = afx; + packet->afy = afy; + packet->afz = afz; + packet->yaw = yaw; + packet->yaw_rate = yaw_rate; + packet->type_mask = type_mask; + packet->coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, (const char *)packet, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, (const char *)packet, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE POSITION_TARGET_GLOBAL_INT UNPACKING + + +/** + * @brief Get field time_boot_ms from position_target_global_int message + * + * @return Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + */ +static inline uint32_t mavlink_msg_position_target_global_int_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field coordinate_frame from position_target_global_int message + * + * @return Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + */ +static inline uint8_t mavlink_msg_position_target_global_int_get_coordinate_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 50); +} + +/** + * @brief Get field type_mask from position_target_global_int message + * + * @return Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + */ +static inline uint16_t mavlink_msg_position_target_global_int_get_type_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 48); +} + +/** + * @brief Get field lat_int from position_target_global_int message + * + * @return X Position in WGS84 frame in 1e7 * meters + */ +static inline int32_t mavlink_msg_position_target_global_int_get_lat_int(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field lon_int from position_target_global_int message + * + * @return Y Position in WGS84 frame in 1e7 * meters + */ +static inline int32_t mavlink_msg_position_target_global_int_get_lon_int(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field alt from position_target_global_int message + * + * @return Altitude in meters in WGS84 altitude, not AMSL if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + */ +static inline float mavlink_msg_position_target_global_int_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field vx from position_target_global_int message + * + * @return X velocity in NED frame in meter / s + */ +static inline float mavlink_msg_position_target_global_int_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field vy from position_target_global_int message + * + * @return Y velocity in NED frame in meter / s + */ +static inline float mavlink_msg_position_target_global_int_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field vz from position_target_global_int message + * + * @return Z velocity in NED frame in meter / s + */ +static inline float mavlink_msg_position_target_global_int_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field afx from position_target_global_int message + * + * @return X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_position_target_global_int_get_afx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field afy from position_target_global_int message + * + * @return Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_position_target_global_int_get_afy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field afz from position_target_global_int message + * + * @return Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_position_target_global_int_get_afz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field yaw from position_target_global_int message + * + * @return yaw setpoint in rad + */ +static inline float mavlink_msg_position_target_global_int_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field yaw_rate from position_target_global_int message + * + * @return yaw rate setpoint in rad/s + */ +static inline float mavlink_msg_position_target_global_int_get_yaw_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Decode a position_target_global_int message into a struct + * + * @param msg The message to decode + * @param position_target_global_int C-struct to decode the message contents into + */ +static inline void mavlink_msg_position_target_global_int_decode(const mavlink_message_t* msg, mavlink_position_target_global_int_t* position_target_global_int) +{ +#if MAVLINK_NEED_BYTE_SWAP + position_target_global_int->time_boot_ms = mavlink_msg_position_target_global_int_get_time_boot_ms(msg); + position_target_global_int->lat_int = mavlink_msg_position_target_global_int_get_lat_int(msg); + position_target_global_int->lon_int = mavlink_msg_position_target_global_int_get_lon_int(msg); + position_target_global_int->alt = mavlink_msg_position_target_global_int_get_alt(msg); + position_target_global_int->vx = mavlink_msg_position_target_global_int_get_vx(msg); + position_target_global_int->vy = mavlink_msg_position_target_global_int_get_vy(msg); + position_target_global_int->vz = mavlink_msg_position_target_global_int_get_vz(msg); + position_target_global_int->afx = mavlink_msg_position_target_global_int_get_afx(msg); + position_target_global_int->afy = mavlink_msg_position_target_global_int_get_afy(msg); + position_target_global_int->afz = mavlink_msg_position_target_global_int_get_afz(msg); + position_target_global_int->yaw = mavlink_msg_position_target_global_int_get_yaw(msg); + position_target_global_int->yaw_rate = mavlink_msg_position_target_global_int_get_yaw_rate(msg); + position_target_global_int->type_mask = mavlink_msg_position_target_global_int_get_type_mask(msg); + position_target_global_int->coordinate_frame = mavlink_msg_position_target_global_int_get_coordinate_frame(msg); +#else + memcpy(position_target_global_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_position_target_local_ned.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_position_target_local_ned.h new file mode 100644 index 0000000..d23d445 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_position_target_local_ned.h @@ -0,0 +1,521 @@ +// MESSAGE POSITION_TARGET_LOCAL_NED PACKING + +#define MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED 85 + +typedef struct __mavlink_position_target_local_ned_t +{ + uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot + float x; ///< X Position in NED frame in meters + float y; ///< Y Position in NED frame in meters + float z; ///< Z Position in NED frame in meters (note, altitude is negative in NED) + float vx; ///< X velocity in NED frame in meter / s + float vy; ///< Y velocity in NED frame in meter / s + float vz; ///< Z velocity in NED frame in meter / s + float afx; ///< X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float afy; ///< Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float afz; ///< Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float yaw; ///< yaw setpoint in rad + float yaw_rate; ///< yaw rate setpoint in rad/s + uint16_t type_mask; ///< Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + uint8_t coordinate_frame; ///< Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 +} mavlink_position_target_local_ned_t; + +#define MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN 51 +#define MAVLINK_MSG_ID_85_LEN 51 + +#define MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_CRC 140 +#define MAVLINK_MSG_ID_85_CRC 140 + + + +#define MAVLINK_MESSAGE_INFO_POSITION_TARGET_LOCAL_NED { \ + "POSITION_TARGET_LOCAL_NED", \ + 14, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_position_target_local_ned_t, time_boot_ms) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_position_target_local_ned_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_position_target_local_ned_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_position_target_local_ned_t, z) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_position_target_local_ned_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_position_target_local_ned_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_position_target_local_ned_t, vz) }, \ + { "afx", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_position_target_local_ned_t, afx) }, \ + { "afy", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_position_target_local_ned_t, afy) }, \ + { "afz", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_position_target_local_ned_t, afz) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_position_target_local_ned_t, yaw) }, \ + { "yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_position_target_local_ned_t, yaw_rate) }, \ + { "type_mask", NULL, MAVLINK_TYPE_UINT16_T, 0, 48, offsetof(mavlink_position_target_local_ned_t, type_mask) }, \ + { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 50, offsetof(mavlink_position_target_local_ned_t, coordinate_frame) }, \ + } \ +} + + +/** + * @brief Pack a position_target_local_ned message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param coordinate_frame Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param x X Position in NED frame in meters + * @param y Y Position in NED frame in meters + * @param z Z Position in NED frame in meters (note, altitude is negative in NED) + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_position_target_local_ned_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t coordinate_frame, uint16_t type_mask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#else + mavlink_position_target_local_ned_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#endif +} + +/** + * @brief Pack a position_target_local_ned message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param coordinate_frame Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param x X Position in NED frame in meters + * @param y Y Position in NED frame in meters + * @param z Z Position in NED frame in meters (note, altitude is negative in NED) + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_position_target_local_ned_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t coordinate_frame,uint16_t type_mask,float x,float y,float z,float vx,float vy,float vz,float afx,float afy,float afz,float yaw,float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#else + mavlink_position_target_local_ned_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#endif +} + +/** + * @brief Encode a position_target_local_ned struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param position_target_local_ned C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_position_target_local_ned_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_position_target_local_ned_t* position_target_local_ned) +{ + return mavlink_msg_position_target_local_ned_pack(system_id, component_id, msg, position_target_local_ned->time_boot_ms, position_target_local_ned->coordinate_frame, position_target_local_ned->type_mask, position_target_local_ned->x, position_target_local_ned->y, position_target_local_ned->z, position_target_local_ned->vx, position_target_local_ned->vy, position_target_local_ned->vz, position_target_local_ned->afx, position_target_local_ned->afy, position_target_local_ned->afz, position_target_local_ned->yaw, position_target_local_ned->yaw_rate); +} + +/** + * @brief Encode a position_target_local_ned struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param position_target_local_ned C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_position_target_local_ned_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_position_target_local_ned_t* position_target_local_ned) +{ + return mavlink_msg_position_target_local_ned_pack_chan(system_id, component_id, chan, msg, position_target_local_ned->time_boot_ms, position_target_local_ned->coordinate_frame, position_target_local_ned->type_mask, position_target_local_ned->x, position_target_local_ned->y, position_target_local_ned->z, position_target_local_ned->vx, position_target_local_ned->vy, position_target_local_ned->vz, position_target_local_ned->afx, position_target_local_ned->afy, position_target_local_ned->afz, position_target_local_ned->yaw, position_target_local_ned->yaw_rate); +} + +/** + * @brief Send a position_target_local_ned message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param coordinate_frame Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param x X Position in NED frame in meters + * @param y Y Position in NED frame in meters + * @param z Z Position in NED frame in meters (note, altitude is negative in NED) + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_position_target_local_ned_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t coordinate_frame, uint16_t type_mask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, buf, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, buf, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#endif +#else + mavlink_position_target_local_ned_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, (const char *)&packet, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, (const char *)&packet, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_position_target_local_ned_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t coordinate_frame, uint16_t type_mask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#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, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, buf, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, buf, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#endif +#else + mavlink_position_target_local_ned_t *packet = (mavlink_position_target_local_ned_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->x = x; + packet->y = y; + packet->z = z; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->afx = afx; + packet->afy = afy; + packet->afz = afz; + packet->yaw = yaw; + packet->yaw_rate = yaw_rate; + packet->type_mask = type_mask; + packet->coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, (const char *)packet, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, (const char *)packet, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE POSITION_TARGET_LOCAL_NED UNPACKING + + +/** + * @brief Get field time_boot_ms from position_target_local_ned message + * + * @return Timestamp in milliseconds since system boot + */ +static inline uint32_t mavlink_msg_position_target_local_ned_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field coordinate_frame from position_target_local_ned message + * + * @return Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + */ +static inline uint8_t mavlink_msg_position_target_local_ned_get_coordinate_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 50); +} + +/** + * @brief Get field type_mask from position_target_local_ned message + * + * @return Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + */ +static inline uint16_t mavlink_msg_position_target_local_ned_get_type_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 48); +} + +/** + * @brief Get field x from position_target_local_ned message + * + * @return X Position in NED frame in meters + */ +static inline float mavlink_msg_position_target_local_ned_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field y from position_target_local_ned message + * + * @return Y Position in NED frame in meters + */ +static inline float mavlink_msg_position_target_local_ned_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field z from position_target_local_ned message + * + * @return Z Position in NED frame in meters (note, altitude is negative in NED) + */ +static inline float mavlink_msg_position_target_local_ned_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field vx from position_target_local_ned message + * + * @return X velocity in NED frame in meter / s + */ +static inline float mavlink_msg_position_target_local_ned_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field vy from position_target_local_ned message + * + * @return Y velocity in NED frame in meter / s + */ +static inline float mavlink_msg_position_target_local_ned_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field vz from position_target_local_ned message + * + * @return Z velocity in NED frame in meter / s + */ +static inline float mavlink_msg_position_target_local_ned_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field afx from position_target_local_ned message + * + * @return X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_position_target_local_ned_get_afx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field afy from position_target_local_ned message + * + * @return Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_position_target_local_ned_get_afy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field afz from position_target_local_ned message + * + * @return Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_position_target_local_ned_get_afz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field yaw from position_target_local_ned message + * + * @return yaw setpoint in rad + */ +static inline float mavlink_msg_position_target_local_ned_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field yaw_rate from position_target_local_ned message + * + * @return yaw rate setpoint in rad/s + */ +static inline float mavlink_msg_position_target_local_ned_get_yaw_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Decode a position_target_local_ned message into a struct + * + * @param msg The message to decode + * @param position_target_local_ned C-struct to decode the message contents into + */ +static inline void mavlink_msg_position_target_local_ned_decode(const mavlink_message_t* msg, mavlink_position_target_local_ned_t* position_target_local_ned) +{ +#if MAVLINK_NEED_BYTE_SWAP + position_target_local_ned->time_boot_ms = mavlink_msg_position_target_local_ned_get_time_boot_ms(msg); + position_target_local_ned->x = mavlink_msg_position_target_local_ned_get_x(msg); + position_target_local_ned->y = mavlink_msg_position_target_local_ned_get_y(msg); + position_target_local_ned->z = mavlink_msg_position_target_local_ned_get_z(msg); + position_target_local_ned->vx = mavlink_msg_position_target_local_ned_get_vx(msg); + position_target_local_ned->vy = mavlink_msg_position_target_local_ned_get_vy(msg); + position_target_local_ned->vz = mavlink_msg_position_target_local_ned_get_vz(msg); + position_target_local_ned->afx = mavlink_msg_position_target_local_ned_get_afx(msg); + position_target_local_ned->afy = mavlink_msg_position_target_local_ned_get_afy(msg); + position_target_local_ned->afz = mavlink_msg_position_target_local_ned_get_afz(msg); + position_target_local_ned->yaw = mavlink_msg_position_target_local_ned_get_yaw(msg); + position_target_local_ned->yaw_rate = mavlink_msg_position_target_local_ned_get_yaw_rate(msg); + position_target_local_ned->type_mask = mavlink_msg_position_target_local_ned_get_type_mask(msg); + position_target_local_ned->coordinate_frame = mavlink_msg_position_target_local_ned_get_coordinate_frame(msg); +#else + memcpy(position_target_local_ned, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_power_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_power_status.h new file mode 100644 index 0000000..1a6259a --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_power_status.h @@ -0,0 +1,257 @@ +// MESSAGE POWER_STATUS PACKING + +#define MAVLINK_MSG_ID_POWER_STATUS 125 + +typedef struct __mavlink_power_status_t +{ + uint16_t Vcc; ///< 5V rail voltage in millivolts + uint16_t Vservo; ///< servo rail voltage in millivolts + uint16_t flags; ///< power supply status flags (see MAV_POWER_STATUS enum) +} mavlink_power_status_t; + +#define MAVLINK_MSG_ID_POWER_STATUS_LEN 6 +#define MAVLINK_MSG_ID_125_LEN 6 + +#define MAVLINK_MSG_ID_POWER_STATUS_CRC 203 +#define MAVLINK_MSG_ID_125_CRC 203 + + + +#define MAVLINK_MESSAGE_INFO_POWER_STATUS { \ + "POWER_STATUS", \ + 3, \ + { { "Vcc", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_power_status_t, Vcc) }, \ + { "Vservo", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_power_status_t, Vservo) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_power_status_t, flags) }, \ + } \ +} + + +/** + * @brief Pack a power_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param Vcc 5V rail voltage in millivolts + * @param Vservo servo rail voltage in millivolts + * @param flags power supply status flags (see MAV_POWER_STATUS enum) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_power_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t Vcc, uint16_t Vservo, uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POWER_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint16_t(buf, 2, Vservo); + _mav_put_uint16_t(buf, 4, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#else + mavlink_power_status_t packet; + packet.Vcc = Vcc; + packet.Vservo = Vservo; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_POWER_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +} + +/** + * @brief Pack a power_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param Vcc 5V rail voltage in millivolts + * @param Vservo servo rail voltage in millivolts + * @param flags power supply status flags (see MAV_POWER_STATUS enum) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_power_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t Vcc,uint16_t Vservo,uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POWER_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint16_t(buf, 2, Vservo); + _mav_put_uint16_t(buf, 4, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#else + mavlink_power_status_t packet; + packet.Vcc = Vcc; + packet.Vservo = Vservo; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_POWER_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +} + +/** + * @brief Encode a power_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param power_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_power_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_power_status_t* power_status) +{ + return mavlink_msg_power_status_pack(system_id, component_id, msg, power_status->Vcc, power_status->Vservo, power_status->flags); +} + +/** + * @brief Encode a power_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param power_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_power_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_power_status_t* power_status) +{ + return mavlink_msg_power_status_pack_chan(system_id, component_id, chan, msg, power_status->Vcc, power_status->Vservo, power_status->flags); +} + +/** + * @brief Send a power_status message + * @param chan MAVLink channel to send the message + * + * @param Vcc 5V rail voltage in millivolts + * @param Vservo servo rail voltage in millivolts + * @param flags power supply status flags (see MAV_POWER_STATUS enum) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_power_status_send(mavlink_channel_t chan, uint16_t Vcc, uint16_t Vservo, uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POWER_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint16_t(buf, 2, Vservo); + _mav_put_uint16_t(buf, 4, flags); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +#else + mavlink_power_status_t packet; + packet.Vcc = Vcc; + packet.Vservo = Vservo; + packet.flags = flags; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)&packet, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)&packet, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_POWER_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_power_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t Vcc, uint16_t Vservo, uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint16_t(buf, 2, Vservo); + _mav_put_uint16_t(buf, 4, flags); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +#else + mavlink_power_status_t *packet = (mavlink_power_status_t *)msgbuf; + packet->Vcc = Vcc; + packet->Vservo = Vservo; + packet->flags = flags; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)packet, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)packet, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE POWER_STATUS UNPACKING + + +/** + * @brief Get field Vcc from power_status message + * + * @return 5V rail voltage in millivolts + */ +static inline uint16_t mavlink_msg_power_status_get_Vcc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field Vservo from power_status message + * + * @return servo rail voltage in millivolts + */ +static inline uint16_t mavlink_msg_power_status_get_Vservo(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field flags from power_status message + * + * @return power supply status flags (see MAV_POWER_STATUS enum) + */ +static inline uint16_t mavlink_msg_power_status_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Decode a power_status message into a struct + * + * @param msg The message to decode + * @param power_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_power_status_decode(const mavlink_message_t* msg, mavlink_power_status_t* power_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + power_status->Vcc = mavlink_msg_power_status_get_Vcc(msg); + power_status->Vservo = mavlink_msg_power_status_get_Vservo(msg); + power_status->flags = mavlink_msg_power_status_get_flags(msg); +#else + memcpy(power_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h index fceb7d1..5763008 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_radio_status_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_RADIO_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_radio_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, rxerrors); + _mav_put_uint16_t(buf, 2, fixed); + _mav_put_uint8_t(buf, 4, rssi); + _mav_put_uint8_t(buf, 5, remrssi); + _mav_put_uint8_t(buf, 6, txbuf); + _mav_put_uint8_t(buf, 7, noise); + _mav_put_uint8_t(buf, 8, remnoise); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif +#else + mavlink_radio_status_t *packet = (mavlink_radio_status_t *)msgbuf; + packet->rxerrors = rxerrors; + packet->fixed = fixed; + packet->rssi = rssi; + packet->remrssi = remrssi; + packet->txbuf = txbuf; + packet->noise = noise; + packet->remnoise = remnoise; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, (const char *)packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, (const char *)packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RADIO_STATUS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h index 62a9b6e..a98e8ce 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t tim #endif } +#if MAVLINK_MSG_ID_RAW_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_raw_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, xacc); + _mav_put_int16_t(buf, 10, yacc); + _mav_put_int16_t(buf, 12, zacc); + _mav_put_int16_t(buf, 14, xgyro); + _mav_put_int16_t(buf, 16, ygyro); + _mav_put_int16_t(buf, 18, zgyro); + _mav_put_int16_t(buf, 20, xmag); + _mav_put_int16_t(buf, 22, ymag); + _mav_put_int16_t(buf, 24, zmag); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, MAVLINK_MSG_ID_RAW_IMU_LEN); +#endif +#else + mavlink_raw_imu_t *packet = (mavlink_raw_imu_t *)msgbuf; + packet->time_usec = time_usec; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)packet, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)packet, MAVLINK_MSG_ID_RAW_IMU_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RAW_IMU UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h index 82c5fad..6bd37fc 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_ #endif } +#if MAVLINK_MSG_ID_RAW_PRESSURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_raw_pressure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, press_abs); + _mav_put_int16_t(buf, 10, press_diff1); + _mav_put_int16_t(buf, 12, press_diff2); + _mav_put_int16_t(buf, 14, temperature); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); +#endif +#else + mavlink_raw_pressure_t *packet = (mavlink_raw_pressure_t *)msgbuf; + packet->time_usec = time_usec; + packet->press_abs = press_abs; + packet->press_diff1 = press_diff1; + packet->press_diff2 = press_diff2; + packet->temperature = temperature; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RAW_PRESSURE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels.h new file mode 100644 index 0000000..479af12 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels.h @@ -0,0 +1,689 @@ +// 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. +} mavlink_rc_channels_t; + +#define MAVLINK_MSG_ID_RC_CHANNELS_LEN 42 +#define MAVLINK_MSG_ID_65_LEN 42 + +#define MAVLINK_MSG_ID_RC_CHANNELS_CRC 118 +#define MAVLINK_MSG_ID_65_CRC 118 + + + +#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) }, \ + { "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) }, \ + { "chancount", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_rc_channels_t, chancount) }, \ + { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_rc_channels_t, rssi) }, \ + } \ +} + + +/** + * @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 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(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) +{ +#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); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +} + +/** + * @brief Pack a rc_channels message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms 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) +{ +#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_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 +} + +/** + * @brief Encode a rc_channels struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param rc_channels C-struct to read the message contents from + */ +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); +} + +/** + * @brief Encode a rc_channels struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rc_channels C-struct to read the message contents from + */ +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); +} + +/** + * @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. + */ +#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); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#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); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +#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 + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_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 +#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 +#endif +} +#endif + +#endif + +// MESSAGE RC_CHANNELS UNPACKING + + +/** + * @brief Get field time_boot_ms from rc_channels message + * + * @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); +} + +/** + * @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. + */ +static inline uint8_t mavlink_msg_rc_channels_get_chancount(const mavlink_message_t* msg) +{ + 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. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan1_raw(const mavlink_message_t* msg) +{ + 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. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan2_raw(const mavlink_message_t* msg) +{ + 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. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan3_raw(const mavlink_message_t* msg) +{ + 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. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan4_raw(const mavlink_message_t* msg) +{ + 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. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan5_raw(const mavlink_message_t* msg) +{ + 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. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan6_raw(const mavlink_message_t* msg) +{ + 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. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan7_raw(const mavlink_message_t* msg) +{ + 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. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan8_raw(const mavlink_message_t* msg) +{ + 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. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan9_raw(const mavlink_message_t* msg) +{ + 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. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan10_raw(const mavlink_message_t* msg) +{ + 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. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan11_raw(const mavlink_message_t* msg) +{ + 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. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan12_raw(const mavlink_message_t* msg) +{ + 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. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan13_raw(const mavlink_message_t* msg) +{ + 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. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan14_raw(const mavlink_message_t* msg) +{ + 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. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan15_raw(const mavlink_message_t* msg) +{ + 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. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan16_raw(const mavlink_message_t* msg) +{ + 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. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan17_raw(const mavlink_message_t* msg) +{ + 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. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan18_raw(const mavlink_message_t* msg) +{ + 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. + */ +static inline uint8_t mavlink_msg_rc_channels_get_rssi(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 41); +} + +/** + * @brief Decode a rc_channels message into a struct + * + * @param msg The message to decode + * @param rc_channels C-struct to decode the message contents into + */ +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); +#else + memcpy(rc_channels, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h index 0d8a151..15d4c6f 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rc_channels_override_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, chan1_raw); + _mav_put_uint16_t(buf, 2, chan2_raw); + _mav_put_uint16_t(buf, 4, chan3_raw); + _mav_put_uint16_t(buf, 6, chan4_raw); + _mav_put_uint16_t(buf, 8, chan5_raw); + _mav_put_uint16_t(buf, 10, chan6_raw); + _mav_put_uint16_t(buf, 12, chan7_raw); + _mav_put_uint16_t(buf, 14, chan8_raw); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +#endif +#else + mavlink_rc_channels_override_t *packet = (mavlink_rc_channels_override_t *)msgbuf; + 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->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RC_CHANNELS_OVERRIDE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h index 3c0aed0..d75a593 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h @@ -256,6 +256,58 @@ static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rc_channels_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t port, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t 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_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); +#endif +#else + mavlink_rc_channels_raw_t *packet = (mavlink_rc_channels_raw_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->port = port; + packet->rssi = rssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RC_CHANNELS_RAW UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h index 2153ab3..f400f98 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h @@ -256,6 +256,58 @@ static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, u #endif } +#if MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rc_channels_scaled_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t port, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, 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_int16_t(buf, 4, chan1_scaled); + _mav_put_int16_t(buf, 6, chan2_scaled); + _mav_put_int16_t(buf, 8, chan3_scaled); + _mav_put_int16_t(buf, 10, chan4_scaled); + _mav_put_int16_t(buf, 12, chan5_scaled); + _mav_put_int16_t(buf, 14, chan6_scaled); + _mav_put_int16_t(buf, 16, chan7_scaled); + _mav_put_int16_t(buf, 18, chan8_scaled); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); +#endif +#else + mavlink_rc_channels_scaled_t *packet = (mavlink_rc_channels_scaled_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->chan1_scaled = chan1_scaled; + packet->chan2_scaled = chan2_scaled; + packet->chan3_scaled = chan3_scaled; + packet->chan4_scaled = chan4_scaled; + packet->chan5_scaled = chan5_scaled; + packet->chan6_scaled = chan6_scaled; + packet->chan7_scaled = chan7_scaled; + packet->chan8_scaled = chan8_scaled; + packet->port = port; + packet->rssi = rssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RC_CHANNELS_SCALED UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h index c754ad8..2ebcc54 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_request_data_stream_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, req_message_rate); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, req_stream_id); + _mav_put_uint8_t(buf, 5, start_stop); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); +#endif +#else + mavlink_request_data_stream_t *packet = (mavlink_request_data_stream_t *)msgbuf; + packet->req_message_rate = req_message_rate; + packet->target_system = target_system; + packet->target_component = target_component; + packet->req_stream_id = req_stream_id; + packet->start_stop = start_stop; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE REQUEST_DATA_STREAM UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h deleted file mode 100644 index ac3ef4f..0000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h +++ /dev/null @@ -1,265 +0,0 @@ -// MESSAGE ROLL_PITCH_YAW_RATES_THRUST_SETPOINT PACKING - -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT 80 - -typedef struct __mavlink_roll_pitch_yaw_rates_thrust_setpoint_t -{ - uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot - float roll_rate; ///< Desired roll rate in radians per second - float pitch_rate; ///< Desired pitch rate in radians per second - float yaw_rate; ///< Desired yaw rate in radians per second - float thrust; ///< Collective thrust, normalized to 0 .. 1 -} mavlink_roll_pitch_yaw_rates_thrust_setpoint_t; - -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN 20 -#define MAVLINK_MSG_ID_80_LEN 20 - -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_CRC 127 -#define MAVLINK_MSG_ID_80_CRC 127 - - - -#define MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT { \ - "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT", \ - 5, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_roll_pitch_yaw_rates_thrust_setpoint_t, time_boot_ms) }, \ - { "roll_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_roll_pitch_yaw_rates_thrust_setpoint_t, roll_rate) }, \ - { "pitch_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_roll_pitch_yaw_rates_thrust_setpoint_t, pitch_rate) }, \ - { "yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_roll_pitch_yaw_rates_thrust_setpoint_t, yaw_rate) }, \ - { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_roll_pitch_yaw_rates_thrust_setpoint_t, thrust) }, \ - } \ -} - - -/** - * @brief Pack a roll_pitch_yaw_rates_thrust_setpoint message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp in milliseconds since system boot - * @param roll_rate Desired roll rate in radians per second - * @param pitch_rate Desired pitch rate in radians per second - * @param yaw_rate Desired yaw rate in radians per second - * @param thrust Collective thrust, normalized to 0 .. 1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, float roll_rate, float pitch_rate, float yaw_rate, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll_rate); - _mav_put_float(buf, 8, pitch_rate); - _mav_put_float(buf, 12, yaw_rate); - _mav_put_float(buf, 16, thrust); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); -#else - mavlink_roll_pitch_yaw_rates_thrust_setpoint_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll_rate = roll_rate; - packet.pitch_rate = pitch_rate; - packet.yaw_rate = yaw_rate; - packet.thrust = thrust; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); -#endif -} - -/** - * @brief Pack a roll_pitch_yaw_rates_thrust_setpoint message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp in milliseconds since system boot - * @param roll_rate Desired roll rate in radians per second - * @param pitch_rate Desired pitch rate in radians per second - * @param yaw_rate Desired yaw rate in radians per second - * @param thrust Collective thrust, normalized to 0 .. 1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,float roll_rate,float pitch_rate,float yaw_rate,float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll_rate); - _mav_put_float(buf, 8, pitch_rate); - _mav_put_float(buf, 12, yaw_rate); - _mav_put_float(buf, 16, thrust); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); -#else - mavlink_roll_pitch_yaw_rates_thrust_setpoint_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll_rate = roll_rate; - packet.pitch_rate = pitch_rate; - packet.yaw_rate = yaw_rate; - packet.thrust = thrust; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); -#endif -} - -/** - * @brief Encode a roll_pitch_yaw_rates_thrust_setpoint struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param roll_pitch_yaw_rates_thrust_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_rates_thrust_setpoint_t* roll_pitch_yaw_rates_thrust_setpoint) -{ - return mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_rates_thrust_setpoint->time_boot_ms, roll_pitch_yaw_rates_thrust_setpoint->roll_rate, roll_pitch_yaw_rates_thrust_setpoint->pitch_rate, roll_pitch_yaw_rates_thrust_setpoint->yaw_rate, roll_pitch_yaw_rates_thrust_setpoint->thrust); -} - -/** - * @brief Encode a roll_pitch_yaw_rates_thrust_setpoint struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param roll_pitch_yaw_rates_thrust_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_rates_thrust_setpoint_t* roll_pitch_yaw_rates_thrust_setpoint) -{ - return mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack_chan(system_id, component_id, chan, msg, roll_pitch_yaw_rates_thrust_setpoint->time_boot_ms, roll_pitch_yaw_rates_thrust_setpoint->roll_rate, roll_pitch_yaw_rates_thrust_setpoint->pitch_rate, roll_pitch_yaw_rates_thrust_setpoint->yaw_rate, roll_pitch_yaw_rates_thrust_setpoint->thrust); -} - -/** - * @brief Send a roll_pitch_yaw_rates_thrust_setpoint message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp in milliseconds since system boot - * @param roll_rate Desired roll rate in radians per second - * @param pitch_rate Desired pitch rate in radians per second - * @param yaw_rate Desired yaw rate in radians per second - * @param thrust Collective thrust, normalized to 0 .. 1 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(mavlink_channel_t chan, uint32_t time_boot_ms, float roll_rate, float pitch_rate, float yaw_rate, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll_rate); - _mav_put_float(buf, 8, pitch_rate); - _mav_put_float(buf, 12, yaw_rate); - _mav_put_float(buf, 16, thrust); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); -#endif -#else - mavlink_roll_pitch_yaw_rates_thrust_setpoint_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll_rate = roll_rate; - packet.pitch_rate = pitch_rate; - packet.yaw_rate = yaw_rate; - packet.thrust = thrust; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); -#endif -#endif -} - -#endif - -// MESSAGE ROLL_PITCH_YAW_RATES_THRUST_SETPOINT UNPACKING - - -/** - * @brief Get field time_boot_ms from roll_pitch_yaw_rates_thrust_setpoint message - * - * @return Timestamp in milliseconds since system boot - */ -static inline uint32_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field roll_rate from roll_pitch_yaw_rates_thrust_setpoint message - * - * @return Desired roll rate in radians per second - */ -static inline float mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_roll_rate(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field pitch_rate from roll_pitch_yaw_rates_thrust_setpoint message - * - * @return Desired pitch rate in radians per second - */ -static inline float mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_pitch_rate(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yaw_rate from roll_pitch_yaw_rates_thrust_setpoint message - * - * @return Desired yaw rate in radians per second - */ -static inline float mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_yaw_rate(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field thrust from roll_pitch_yaw_rates_thrust_setpoint message - * - * @return Collective thrust, normalized to 0 .. 1 - */ -static inline float mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_thrust(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Decode a roll_pitch_yaw_rates_thrust_setpoint message into a struct - * - * @param msg The message to decode - * @param roll_pitch_yaw_rates_thrust_setpoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_rates_thrust_setpoint_t* roll_pitch_yaw_rates_thrust_setpoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - roll_pitch_yaw_rates_thrust_setpoint->time_boot_ms = mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_time_boot_ms(msg); - roll_pitch_yaw_rates_thrust_setpoint->roll_rate = mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_roll_rate(msg); - roll_pitch_yaw_rates_thrust_setpoint->pitch_rate = mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_pitch_rate(msg); - roll_pitch_yaw_rates_thrust_setpoint->yaw_rate = mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_yaw_rate(msg); - roll_pitch_yaw_rates_thrust_setpoint->thrust = mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_thrust(msg); -#else - memcpy(roll_pitch_yaw_rates_thrust_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h deleted file mode 100644 index 6264773..0000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h +++ /dev/null @@ -1,265 +0,0 @@ -// MESSAGE ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT PACKING - -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT 59 - -typedef struct __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t -{ - uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot - float roll_speed; ///< Desired roll angular speed in rad/s - float pitch_speed; ///< Desired pitch angular speed in rad/s - float yaw_speed; ///< Desired yaw angular speed in rad/s - float thrust; ///< Collective thrust, normalized to 0 .. 1 -} mavlink_roll_pitch_yaw_speed_thrust_setpoint_t; - -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN 20 -#define MAVLINK_MSG_ID_59_LEN 20 - -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_CRC 238 -#define MAVLINK_MSG_ID_59_CRC 238 - - - -#define MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT { \ - "ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT", \ - 5, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, time_boot_ms) }, \ - { "roll_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, roll_speed) }, \ - { "pitch_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, pitch_speed) }, \ - { "yaw_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, yaw_speed) }, \ - { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, thrust) }, \ - } \ -} - - -/** - * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp in milliseconds since system boot - * @param roll_speed Desired roll angular speed in rad/s - * @param pitch_speed Desired pitch angular speed in rad/s - * @param yaw_speed Desired yaw angular speed in rad/s - * @param thrust Collective thrust, normalized to 0 .. 1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, float roll_speed, float pitch_speed, float yaw_speed, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll_speed); - _mav_put_float(buf, 8, pitch_speed); - _mav_put_float(buf, 12, yaw_speed); - _mav_put_float(buf, 16, thrust); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); -#else - mavlink_roll_pitch_yaw_speed_thrust_setpoint_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll_speed = roll_speed; - packet.pitch_speed = pitch_speed; - packet.yaw_speed = yaw_speed; - packet.thrust = thrust; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); -#endif -} - -/** - * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp in milliseconds since system boot - * @param roll_speed Desired roll angular speed in rad/s - * @param pitch_speed Desired pitch angular speed in rad/s - * @param yaw_speed Desired yaw angular speed in rad/s - * @param thrust Collective thrust, normalized to 0 .. 1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,float roll_speed,float pitch_speed,float yaw_speed,float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll_speed); - _mav_put_float(buf, 8, pitch_speed); - _mav_put_float(buf, 12, yaw_speed); - _mav_put_float(buf, 16, thrust); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); -#else - mavlink_roll_pitch_yaw_speed_thrust_setpoint_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll_speed = roll_speed; - packet.pitch_speed = pitch_speed; - packet.yaw_speed = yaw_speed; - packet.thrust = thrust; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); -#endif -} - -/** - * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param roll_pitch_yaw_speed_thrust_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint) -{ - return mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_speed_thrust_setpoint->time_boot_ms, roll_pitch_yaw_speed_thrust_setpoint->roll_speed, roll_pitch_yaw_speed_thrust_setpoint->pitch_speed, roll_pitch_yaw_speed_thrust_setpoint->yaw_speed, roll_pitch_yaw_speed_thrust_setpoint->thrust); -} - -/** - * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param roll_pitch_yaw_speed_thrust_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint) -{ - return mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan(system_id, component_id, chan, msg, roll_pitch_yaw_speed_thrust_setpoint->time_boot_ms, roll_pitch_yaw_speed_thrust_setpoint->roll_speed, roll_pitch_yaw_speed_thrust_setpoint->pitch_speed, roll_pitch_yaw_speed_thrust_setpoint->yaw_speed, roll_pitch_yaw_speed_thrust_setpoint->thrust); -} - -/** - * @brief Send a roll_pitch_yaw_speed_thrust_setpoint message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp in milliseconds since system boot - * @param roll_speed Desired roll angular speed in rad/s - * @param pitch_speed Desired pitch angular speed in rad/s - * @param yaw_speed Desired yaw angular speed in rad/s - * @param thrust Collective thrust, normalized to 0 .. 1 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send(mavlink_channel_t chan, uint32_t time_boot_ms, float roll_speed, float pitch_speed, float yaw_speed, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll_speed); - _mav_put_float(buf, 8, pitch_speed); - _mav_put_float(buf, 12, yaw_speed); - _mav_put_float(buf, 16, thrust); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); -#endif -#else - mavlink_roll_pitch_yaw_speed_thrust_setpoint_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll_speed = roll_speed; - packet.pitch_speed = pitch_speed; - packet.yaw_speed = yaw_speed; - packet.thrust = thrust; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); -#endif -#endif -} - -#endif - -// MESSAGE ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT UNPACKING - - -/** - * @brief Get field time_boot_ms from roll_pitch_yaw_speed_thrust_setpoint message - * - * @return Timestamp in milliseconds since system boot - */ -static inline uint32_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field roll_speed from roll_pitch_yaw_speed_thrust_setpoint message - * - * @return Desired roll angular speed in rad/s - */ -static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_speed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field pitch_speed from roll_pitch_yaw_speed_thrust_setpoint message - * - * @return Desired pitch angular speed in rad/s - */ -static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_speed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yaw_speed from roll_pitch_yaw_speed_thrust_setpoint message - * - * @return Desired yaw angular speed in rad/s - */ -static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_speed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field thrust from roll_pitch_yaw_speed_thrust_setpoint message - * - * @return Collective thrust, normalized to 0 .. 1 - */ -static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Decode a roll_pitch_yaw_speed_thrust_setpoint message into a struct - * - * @param msg The message to decode - * @param roll_pitch_yaw_speed_thrust_setpoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - roll_pitch_yaw_speed_thrust_setpoint->time_boot_ms = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time_boot_ms(msg); - roll_pitch_yaw_speed_thrust_setpoint->roll_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_speed(msg); - roll_pitch_yaw_speed_thrust_setpoint->pitch_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_speed(msg); - roll_pitch_yaw_speed_thrust_setpoint->yaw_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_speed(msg); - roll_pitch_yaw_speed_thrust_setpoint->thrust = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust(msg); -#else - memcpy(roll_pitch_yaw_speed_thrust_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h deleted file mode 100644 index ffcdc54..0000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h +++ /dev/null @@ -1,265 +0,0 @@ -// MESSAGE ROLL_PITCH_YAW_THRUST_SETPOINT PACKING - -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT 58 - -typedef struct __mavlink_roll_pitch_yaw_thrust_setpoint_t -{ - uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot - float roll; ///< Desired roll angle in radians - float pitch; ///< Desired pitch angle in radians - float yaw; ///< Desired yaw angle in radians - float thrust; ///< Collective thrust, normalized to 0 .. 1 -} mavlink_roll_pitch_yaw_thrust_setpoint_t; - -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN 20 -#define MAVLINK_MSG_ID_58_LEN 20 - -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_CRC 239 -#define MAVLINK_MSG_ID_58_CRC 239 - - - -#define MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT { \ - "ROLL_PITCH_YAW_THRUST_SETPOINT", \ - 5, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, time_boot_ms) }, \ - { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, yaw) }, \ - { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, thrust) }, \ - } \ -} - - -/** - * @brief Pack a roll_pitch_yaw_thrust_setpoint message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp in milliseconds since system boot - * @param roll Desired roll angle in radians - * @param pitch Desired pitch angle in radians - * @param yaw Desired yaw angle in radians - * @param thrust Collective thrust, normalized to 0 .. 1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll); - _mav_put_float(buf, 8, pitch); - _mav_put_float(buf, 12, yaw); - _mav_put_float(buf, 16, thrust); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); -#else - mavlink_roll_pitch_yaw_thrust_setpoint_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.thrust = thrust; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); -#endif -} - -/** - * @brief Pack a roll_pitch_yaw_thrust_setpoint message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp in milliseconds since system boot - * @param roll Desired roll angle in radians - * @param pitch Desired pitch angle in radians - * @param yaw Desired yaw angle in radians - * @param thrust Collective thrust, normalized to 0 .. 1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,float roll,float pitch,float yaw,float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll); - _mav_put_float(buf, 8, pitch); - _mav_put_float(buf, 12, yaw); - _mav_put_float(buf, 16, thrust); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); -#else - mavlink_roll_pitch_yaw_thrust_setpoint_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.thrust = thrust; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); -#endif -} - -/** - * @brief Encode a roll_pitch_yaw_thrust_setpoint struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param roll_pitch_yaw_thrust_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint) -{ - return mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_thrust_setpoint->time_boot_ms, roll_pitch_yaw_thrust_setpoint->roll, roll_pitch_yaw_thrust_setpoint->pitch, roll_pitch_yaw_thrust_setpoint->yaw, roll_pitch_yaw_thrust_setpoint->thrust); -} - -/** - * @brief Encode a roll_pitch_yaw_thrust_setpoint struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param roll_pitch_yaw_thrust_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint) -{ - return mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(system_id, component_id, chan, msg, roll_pitch_yaw_thrust_setpoint->time_boot_ms, roll_pitch_yaw_thrust_setpoint->roll, roll_pitch_yaw_thrust_setpoint->pitch, roll_pitch_yaw_thrust_setpoint->yaw, roll_pitch_yaw_thrust_setpoint->thrust); -} - -/** - * @brief Send a roll_pitch_yaw_thrust_setpoint message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp in milliseconds since system boot - * @param roll Desired roll angle in radians - * @param pitch Desired pitch angle in radians - * @param yaw Desired yaw angle in radians - * @param thrust Collective thrust, normalized to 0 .. 1 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll); - _mav_put_float(buf, 8, pitch); - _mav_put_float(buf, 12, yaw); - _mav_put_float(buf, 16, thrust); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); -#endif -#else - mavlink_roll_pitch_yaw_thrust_setpoint_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.thrust = thrust; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); -#endif -#endif -} - -#endif - -// MESSAGE ROLL_PITCH_YAW_THRUST_SETPOINT UNPACKING - - -/** - * @brief Get field time_boot_ms from roll_pitch_yaw_thrust_setpoint message - * - * @return Timestamp in milliseconds since system boot - */ -static inline uint32_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field roll from roll_pitch_yaw_thrust_setpoint message - * - * @return Desired roll angle in radians - */ -static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field pitch from roll_pitch_yaw_thrust_setpoint message - * - * @return Desired pitch angle in radians - */ -static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yaw from roll_pitch_yaw_thrust_setpoint message - * - * @return Desired yaw angle in radians - */ -static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field thrust from roll_pitch_yaw_thrust_setpoint message - * - * @return Collective thrust, normalized to 0 .. 1 - */ -static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Decode a roll_pitch_yaw_thrust_setpoint message into a struct - * - * @param msg The message to decode - * @param roll_pitch_yaw_thrust_setpoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - roll_pitch_yaw_thrust_setpoint->time_boot_ms = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_boot_ms(msg); - roll_pitch_yaw_thrust_setpoint->roll = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(msg); - roll_pitch_yaw_thrust_setpoint->pitch = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(msg); - roll_pitch_yaw_thrust_setpoint->yaw = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(msg); - roll_pitch_yaw_thrust_setpoint->thrust = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(msg); -#else - memcpy(roll_pitch_yaw_thrust_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h index fcd54cb..0176dfc 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_safety_allowed_area_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); +#endif +#else + mavlink_safety_allowed_area_t *packet = (mavlink_safety_allowed_area_t *)msgbuf; + packet->p1x = p1x; + packet->p1y = p1y; + packet->p1z = p1z; + packet->p2x = p2x; + packet->p2y = p2y; + packet->p2z = p2z; + packet->frame = frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SAFETY_ALLOWED_AREA UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h index 61f6af1..dafbc26 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h @@ -234,6 +234,54 @@ static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t ch #endif } +#if MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_safety_set_allowed_area_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, target_system); + _mav_put_uint8_t(buf, 25, target_component); + _mav_put_uint8_t(buf, 26, frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); +#endif +#else + mavlink_safety_set_allowed_area_t *packet = (mavlink_safety_set_allowed_area_t *)msgbuf; + packet->p1x = p1x; + packet->p1y = p1y; + packet->p1z = p1z; + packet->p2x = p2x; + packet->p2y = p2y; + packet->p2z = p2z; + packet->target_system = target_system; + packet->target_component = target_component; + packet->frame = frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SAFETY_SET_ALLOWED_AREA UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h index 3010d05..1648a56 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint32_t #endif } +#if MAVLINK_MSG_ID_SCALED_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_scaled_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, MAVLINK_MSG_ID_SCALED_IMU_LEN); +#endif +#else + mavlink_scaled_imu_t *packet = (mavlink_scaled_imu_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SCALED_IMU UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h index ea4dbbf..1dea121 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_scaled_imu2_send(mavlink_channel_t chan, uint32_t #endif } +#if MAVLINK_MSG_ID_SCALED_IMU2_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_scaled_imu2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif +#else + mavlink_scaled_imu2_t *packet = (mavlink_scaled_imu2_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SCALED_IMU2 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h index 10324bc..e0d6d87 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_SCALED_PRESSURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_scaled_pressure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) +{ +#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, press_abs); + _mav_put_float(buf, 8, press_diff); + _mav_put_int16_t(buf, 12, temperature); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); +#endif +#else + mavlink_scaled_pressure_t *packet = (mavlink_scaled_pressure_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->press_abs = press_abs; + packet->press_diff = press_diff; + packet->temperature = temperature; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SCALED_PRESSURE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_serial_control.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_serial_control.h new file mode 100644 index 0000000..cbf72ba --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_serial_control.h @@ -0,0 +1,321 @@ +// MESSAGE SERIAL_CONTROL PACKING + +#define MAVLINK_MSG_ID_SERIAL_CONTROL 126 + +typedef struct __mavlink_serial_control_t +{ + uint32_t baudrate; ///< Baudrate of transfer. Zero means no change. + uint16_t timeout; ///< Timeout for reply data in milliseconds + uint8_t device; ///< See SERIAL_CONTROL_DEV enum + uint8_t flags; ///< See SERIAL_CONTROL_FLAG enum + uint8_t count; ///< how many bytes in this transfer + uint8_t data[70]; ///< serial data +} mavlink_serial_control_t; + +#define MAVLINK_MSG_ID_SERIAL_CONTROL_LEN 79 +#define MAVLINK_MSG_ID_126_LEN 79 + +#define MAVLINK_MSG_ID_SERIAL_CONTROL_CRC 220 +#define MAVLINK_MSG_ID_126_CRC 220 + +#define MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN 70 + +#define MAVLINK_MESSAGE_INFO_SERIAL_CONTROL { \ + "SERIAL_CONTROL", \ + 6, \ + { { "baudrate", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_serial_control_t, baudrate) }, \ + { "timeout", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_serial_control_t, timeout) }, \ + { "device", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_serial_control_t, device) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_serial_control_t, flags) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_serial_control_t, count) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 70, 9, offsetof(mavlink_serial_control_t, data) }, \ + } \ +} + + +/** + * @brief Pack a serial_control message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param device See SERIAL_CONTROL_DEV enum + * @param flags See SERIAL_CONTROL_FLAG enum + * @param timeout Timeout for reply data in milliseconds + * @param baudrate Baudrate of transfer. Zero means no change. + * @param count how many bytes in this transfer + * @param data serial data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_serial_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN]; + _mav_put_uint32_t(buf, 0, baudrate); + _mav_put_uint16_t(buf, 4, timeout); + _mav_put_uint8_t(buf, 6, device); + _mav_put_uint8_t(buf, 7, flags); + _mav_put_uint8_t(buf, 8, count); + _mav_put_uint8_t_array(buf, 9, data, 70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#else + mavlink_serial_control_t packet; + packet.baudrate = baudrate; + packet.timeout = timeout; + packet.device = device; + packet.flags = flags; + packet.count = count; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SERIAL_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +} + +/** + * @brief Pack a serial_control message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param device See SERIAL_CONTROL_DEV enum + * @param flags See SERIAL_CONTROL_FLAG enum + * @param timeout Timeout for reply data in milliseconds + * @param baudrate Baudrate of transfer. Zero means no change. + * @param count how many bytes in this transfer + * @param data serial data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_serial_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t device,uint8_t flags,uint16_t timeout,uint32_t baudrate,uint8_t count,const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN]; + _mav_put_uint32_t(buf, 0, baudrate); + _mav_put_uint16_t(buf, 4, timeout); + _mav_put_uint8_t(buf, 6, device); + _mav_put_uint8_t(buf, 7, flags); + _mav_put_uint8_t(buf, 8, count); + _mav_put_uint8_t_array(buf, 9, data, 70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#else + mavlink_serial_control_t packet; + packet.baudrate = baudrate; + packet.timeout = timeout; + packet.device = device; + packet.flags = flags; + packet.count = count; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SERIAL_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +} + +/** + * @brief Encode a serial_control struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param serial_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_control_t* serial_control) +{ + return mavlink_msg_serial_control_pack(system_id, component_id, msg, serial_control->device, serial_control->flags, serial_control->timeout, serial_control->baudrate, serial_control->count, serial_control->data); +} + +/** + * @brief Encode a serial_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_control_t* serial_control) +{ + return mavlink_msg_serial_control_pack_chan(system_id, component_id, chan, msg, serial_control->device, serial_control->flags, serial_control->timeout, serial_control->baudrate, serial_control->count, serial_control->data); +} + +/** + * @brief Send a serial_control message + * @param chan MAVLink channel to send the message + * + * @param device See SERIAL_CONTROL_DEV enum + * @param flags See SERIAL_CONTROL_FLAG enum + * @param timeout Timeout for reply data in milliseconds + * @param baudrate Baudrate of transfer. Zero means no change. + * @param count how many bytes in this transfer + * @param data serial data + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_serial_control_send(mavlink_channel_t chan, uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN]; + _mav_put_uint32_t(buf, 0, baudrate); + _mav_put_uint16_t(buf, 4, timeout); + _mav_put_uint8_t(buf, 6, device); + _mav_put_uint8_t(buf, 7, flags); + _mav_put_uint8_t(buf, 8, count); + _mav_put_uint8_t_array(buf, 9, data, 70); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +#else + mavlink_serial_control_t packet; + packet.baudrate = baudrate; + packet.timeout = timeout; + packet.device = device; + packet.flags = flags; + packet.count = count; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SERIAL_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, baudrate); + _mav_put_uint16_t(buf, 4, timeout); + _mav_put_uint8_t(buf, 6, device); + _mav_put_uint8_t(buf, 7, flags); + _mav_put_uint8_t(buf, 8, count); + _mav_put_uint8_t_array(buf, 9, data, 70); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +#else + mavlink_serial_control_t *packet = (mavlink_serial_control_t *)msgbuf; + packet->baudrate = baudrate; + packet->timeout = timeout; + packet->device = device; + packet->flags = flags; + packet->count = count; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*70); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SERIAL_CONTROL UNPACKING + + +/** + * @brief Get field device from serial_control message + * + * @return See SERIAL_CONTROL_DEV enum + */ +static inline uint8_t mavlink_msg_serial_control_get_device(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + +/** + * @brief Get field flags from serial_control message + * + * @return See SERIAL_CONTROL_FLAG enum + */ +static inline uint8_t mavlink_msg_serial_control_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 7); +} + +/** + * @brief Get field timeout from serial_control message + * + * @return Timeout for reply data in milliseconds + */ +static inline uint16_t mavlink_msg_serial_control_get_timeout(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field baudrate from serial_control message + * + * @return Baudrate of transfer. Zero means no change. + */ +static inline uint32_t mavlink_msg_serial_control_get_baudrate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field count from serial_control message + * + * @return how many bytes in this transfer + */ +static inline uint8_t mavlink_msg_serial_control_get_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field data from serial_control message + * + * @return serial data + */ +static inline uint16_t mavlink_msg_serial_control_get_data(const mavlink_message_t* msg, uint8_t *data) +{ + return _MAV_RETURN_uint8_t_array(msg, data, 70, 9); +} + +/** + * @brief Decode a serial_control message into a struct + * + * @param msg The message to decode + * @param serial_control C-struct to decode the message contents into + */ +static inline void mavlink_msg_serial_control_decode(const mavlink_message_t* msg, mavlink_serial_control_t* serial_control) +{ +#if MAVLINK_NEED_BYTE_SWAP + serial_control->baudrate = mavlink_msg_serial_control_get_baudrate(msg); + serial_control->timeout = mavlink_msg_serial_control_get_timeout(msg); + serial_control->device = mavlink_msg_serial_control_get_device(msg); + serial_control->flags = mavlink_msg_serial_control_get_flags(msg); + serial_control->count = mavlink_msg_serial_control_get_count(msg); + mavlink_msg_serial_control_get_data(msg, serial_control->data); +#else + memcpy(serial_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h index 6a14e93..e840886 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uin #endif } +#if MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_servo_output_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 4, servo1_raw); + _mav_put_uint16_t(buf, 6, servo2_raw); + _mav_put_uint16_t(buf, 8, servo3_raw); + _mav_put_uint16_t(buf, 10, servo4_raw); + _mav_put_uint16_t(buf, 12, servo5_raw); + _mav_put_uint16_t(buf, 14, servo6_raw); + _mav_put_uint16_t(buf, 16, servo7_raw); + _mav_put_uint16_t(buf, 18, servo8_raw); + _mav_put_uint8_t(buf, 20, port); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); +#endif +#else + mavlink_servo_output_raw_t *packet = (mavlink_servo_output_raw_t *)msgbuf; + packet->time_usec = time_usec; + packet->servo1_raw = servo1_raw; + packet->servo2_raw = servo2_raw; + packet->servo3_raw = servo3_raw; + packet->servo4_raw = servo4_raw; + packet->servo5_raw = servo5_raw; + packet->servo6_raw = servo6_raw; + packet->servo7_raw = servo7_raw; + packet->servo8_raw = servo8_raw; + packet->port = port; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERVO_OUTPUT_RAW UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_attitude_target.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_attitude_target.h new file mode 100644 index 0000000..19ccc14 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_attitude_target.h @@ -0,0 +1,393 @@ +// MESSAGE SET_ATTITUDE_TARGET PACKING + +#define MAVLINK_MSG_ID_SET_ATTITUDE_TARGET 82 + +typedef struct __mavlink_set_attitude_target_t +{ + uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot + float q[4]; ///< Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + float body_roll_rate; ///< Body roll rate in radians per second + float body_pitch_rate; ///< Body roll rate in radians per second + float body_yaw_rate; ///< Body roll rate in radians per second + float thrust; ///< Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t type_mask; ///< Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude +} mavlink_set_attitude_target_t; + +#define MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN 39 +#define MAVLINK_MSG_ID_82_LEN 39 + +#define MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC 49 +#define MAVLINK_MSG_ID_82_CRC 49 + +#define MAVLINK_MSG_SET_ATTITUDE_TARGET_FIELD_Q_LEN 4 + +#define MAVLINK_MESSAGE_INFO_SET_ATTITUDE_TARGET { \ + "SET_ATTITUDE_TARGET", \ + 9, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_set_attitude_target_t, time_boot_ms) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 4, offsetof(mavlink_set_attitude_target_t, q) }, \ + { "body_roll_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_set_attitude_target_t, body_roll_rate) }, \ + { "body_pitch_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_set_attitude_target_t, body_pitch_rate) }, \ + { "body_yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_set_attitude_target_t, body_yaw_rate) }, \ + { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_set_attitude_target_t, thrust) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_set_attitude_target_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_set_attitude_target_t, target_component) }, \ + { "type_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 38, offsetof(mavlink_set_attitude_target_t, type_mask) }, \ + } \ +} + + +/** + * @brief Pack a set_attitude_target message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param target_system System ID + * @param target_component Component ID + * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude + * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param body_roll_rate Body roll rate in radians per second + * @param body_pitch_rate Body roll rate in radians per second + * @param body_yaw_rate Body roll rate in radians per second + * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_attitude_target_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, target_system); + _mav_put_uint8_t(buf, 37, target_component); + _mav_put_uint8_t(buf, 38, type_mask); + _mav_put_float_array(buf, 4, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#else + mavlink_set_attitude_target_t packet; + packet.time_boot_ms = time_boot_ms; + packet.body_roll_rate = body_roll_rate; + packet.body_pitch_rate = body_pitch_rate; + packet.body_yaw_rate = body_yaw_rate; + packet.thrust = thrust; + packet.target_system = target_system; + packet.target_component = target_component; + packet.type_mask = type_mask; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_ATTITUDE_TARGET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#endif +} + +/** + * @brief Pack a set_attitude_target message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param target_system System ID + * @param target_component Component ID + * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude + * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param body_roll_rate Body roll rate in radians per second + * @param body_pitch_rate Body roll rate in radians per second + * @param body_yaw_rate Body roll rate in radians per second + * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_attitude_target_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t target_system,uint8_t target_component,uint8_t type_mask,const float *q,float body_roll_rate,float body_pitch_rate,float body_yaw_rate,float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, target_system); + _mav_put_uint8_t(buf, 37, target_component); + _mav_put_uint8_t(buf, 38, type_mask); + _mav_put_float_array(buf, 4, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#else + mavlink_set_attitude_target_t packet; + packet.time_boot_ms = time_boot_ms; + packet.body_roll_rate = body_roll_rate; + packet.body_pitch_rate = body_pitch_rate; + packet.body_yaw_rate = body_yaw_rate; + packet.thrust = thrust; + packet.target_system = target_system; + packet.target_component = target_component; + packet.type_mask = type_mask; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_ATTITUDE_TARGET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#endif +} + +/** + * @brief Encode a set_attitude_target struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_attitude_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_attitude_target_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_attitude_target_t* set_attitude_target) +{ + return mavlink_msg_set_attitude_target_pack(system_id, component_id, msg, set_attitude_target->time_boot_ms, set_attitude_target->target_system, set_attitude_target->target_component, set_attitude_target->type_mask, set_attitude_target->q, set_attitude_target->body_roll_rate, set_attitude_target->body_pitch_rate, set_attitude_target->body_yaw_rate, set_attitude_target->thrust); +} + +/** + * @brief Encode a set_attitude_target struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_attitude_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_attitude_target_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_attitude_target_t* set_attitude_target) +{ + return mavlink_msg_set_attitude_target_pack_chan(system_id, component_id, chan, msg, set_attitude_target->time_boot_ms, set_attitude_target->target_system, set_attitude_target->target_component, set_attitude_target->type_mask, set_attitude_target->q, set_attitude_target->body_roll_rate, set_attitude_target->body_pitch_rate, set_attitude_target->body_yaw_rate, set_attitude_target->thrust); +} + +/** + * @brief Send a set_attitude_target message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param target_system System ID + * @param target_component Component ID + * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude + * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param body_roll_rate Body roll rate in radians per second + * @param body_pitch_rate Body roll rate in radians per second + * @param body_yaw_rate Body roll rate in radians per second + * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_set_attitude_target_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, target_system); + _mav_put_uint8_t(buf, 37, target_component); + _mav_put_uint8_t(buf, 38, type_mask); + _mav_put_float_array(buf, 4, q, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, buf, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, buf, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#endif +#else + mavlink_set_attitude_target_t packet; + packet.time_boot_ms = time_boot_ms; + packet.body_roll_rate = body_roll_rate; + packet.body_pitch_rate = body_pitch_rate; + packet.body_yaw_rate = body_yaw_rate; + packet.thrust = thrust; + packet.target_system = target_system; + packet.target_component = target_component; + packet.type_mask = type_mask; + mav_array_memcpy(packet.q, q, sizeof(float)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, (const char *)&packet, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, (const char *)&packet, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_attitude_target_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, target_system); + _mav_put_uint8_t(buf, 37, target_component); + _mav_put_uint8_t(buf, 38, type_mask); + _mav_put_float_array(buf, 4, q, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, buf, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, buf, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#endif +#else + mavlink_set_attitude_target_t *packet = (mavlink_set_attitude_target_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->body_roll_rate = body_roll_rate; + packet->body_pitch_rate = body_pitch_rate; + packet->body_yaw_rate = body_yaw_rate; + packet->thrust = thrust; + packet->target_system = target_system; + packet->target_component = target_component; + packet->type_mask = type_mask; + mav_array_memcpy(packet->q, q, sizeof(float)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, (const char *)packet, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, (const char *)packet, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SET_ATTITUDE_TARGET UNPACKING + + +/** + * @brief Get field time_boot_ms from set_attitude_target message + * + * @return Timestamp in milliseconds since system boot + */ +static inline uint32_t mavlink_msg_set_attitude_target_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field target_system from set_attitude_target message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_attitude_target_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 36); +} + +/** + * @brief Get field target_component from set_attitude_target message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_attitude_target_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 37); +} + +/** + * @brief Get field type_mask from set_attitude_target message + * + * @return Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude + */ +static inline uint8_t mavlink_msg_set_attitude_target_get_type_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 38); +} + +/** + * @brief Get field q from set_attitude_target message + * + * @return Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + */ +static inline uint16_t mavlink_msg_set_attitude_target_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 4); +} + +/** + * @brief Get field body_roll_rate from set_attitude_target message + * + * @return Body roll rate in radians per second + */ +static inline float mavlink_msg_set_attitude_target_get_body_roll_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field body_pitch_rate from set_attitude_target message + * + * @return Body roll rate in radians per second + */ +static inline float mavlink_msg_set_attitude_target_get_body_pitch_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field body_yaw_rate from set_attitude_target message + * + * @return Body roll rate in radians per second + */ +static inline float mavlink_msg_set_attitude_target_get_body_yaw_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field thrust from set_attitude_target message + * + * @return Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + */ +static inline float mavlink_msg_set_attitude_target_get_thrust(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Decode a set_attitude_target message into a struct + * + * @param msg The message to decode + * @param set_attitude_target C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_attitude_target_decode(const mavlink_message_t* msg, mavlink_set_attitude_target_t* set_attitude_target) +{ +#if MAVLINK_NEED_BYTE_SWAP + set_attitude_target->time_boot_ms = mavlink_msg_set_attitude_target_get_time_boot_ms(msg); + mavlink_msg_set_attitude_target_get_q(msg, set_attitude_target->q); + set_attitude_target->body_roll_rate = mavlink_msg_set_attitude_target_get_body_roll_rate(msg); + set_attitude_target->body_pitch_rate = mavlink_msg_set_attitude_target_get_body_pitch_rate(msg); + set_attitude_target->body_yaw_rate = mavlink_msg_set_attitude_target_get_body_yaw_rate(msg); + set_attitude_target->thrust = mavlink_msg_set_attitude_target_get_thrust(msg); + set_attitude_target->target_system = mavlink_msg_set_attitude_target_get_target_system(msg); + set_attitude_target->target_component = mavlink_msg_set_attitude_target_get_target_component(msg); + set_attitude_target->type_mask = mavlink_msg_set_attitude_target_get_type_mask(msg); +#else + memcpy(set_attitude_target, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h deleted file mode 100644 index 6f0d7a6..0000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h +++ /dev/null @@ -1,265 +0,0 @@ -// MESSAGE SET_GLOBAL_POSITION_SETPOINT_INT PACKING - -#define MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT 53 - -typedef struct __mavlink_set_global_position_setpoint_int_t -{ - int32_t latitude; ///< Latitude (WGS84), in degrees * 1E7 - int32_t longitude; ///< Longitude (WGS84), in degrees * 1E7 - int32_t altitude; ///< Altitude (WGS84), in meters * 1000 (positive for up) - int16_t yaw; ///< Desired yaw angle in degrees * 100 - uint8_t coordinate_frame; ///< Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT -} mavlink_set_global_position_setpoint_int_t; - -#define MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN 15 -#define MAVLINK_MSG_ID_53_LEN 15 - -#define MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_CRC 33 -#define MAVLINK_MSG_ID_53_CRC 33 - - - -#define MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT { \ - "SET_GLOBAL_POSITION_SETPOINT_INT", \ - 5, \ - { { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_set_global_position_setpoint_int_t, latitude) }, \ - { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_set_global_position_setpoint_int_t, longitude) }, \ - { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_set_global_position_setpoint_int_t, altitude) }, \ - { "yaw", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_set_global_position_setpoint_int_t, yaw) }, \ - { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_set_global_position_setpoint_int_t, coordinate_frame) }, \ - } \ -} - - -/** - * @brief Pack a set_global_position_setpoint_int message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT - * @param latitude Latitude (WGS84), in degrees * 1E7 - * @param longitude Longitude (WGS84), in degrees * 1E7 - * @param altitude Altitude (WGS84), in meters * 1000 (positive for up) - * @param yaw Desired yaw angle in degrees * 100 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_global_position_setpoint_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t coordinate_frame, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - _mav_put_int16_t(buf, 12, yaw); - _mav_put_uint8_t(buf, 14, coordinate_frame); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); -#else - mavlink_set_global_position_setpoint_int_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - packet.yaw = yaw; - packet.coordinate_frame = coordinate_frame; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -} - -/** - * @brief Pack a set_global_position_setpoint_int message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT - * @param latitude Latitude (WGS84), in degrees * 1E7 - * @param longitude Longitude (WGS84), in degrees * 1E7 - * @param altitude Altitude (WGS84), in meters * 1000 (positive for up) - * @param yaw Desired yaw angle in degrees * 100 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_global_position_setpoint_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t coordinate_frame,int32_t latitude,int32_t longitude,int32_t altitude,int16_t yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - _mav_put_int16_t(buf, 12, yaw); - _mav_put_uint8_t(buf, 14, coordinate_frame); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); -#else - mavlink_set_global_position_setpoint_int_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - packet.yaw = yaw; - packet.coordinate_frame = coordinate_frame; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -} - -/** - * @brief Encode a set_global_position_setpoint_int struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_global_position_setpoint_int C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_global_position_setpoint_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_global_position_setpoint_int_t* set_global_position_setpoint_int) -{ - return mavlink_msg_set_global_position_setpoint_int_pack(system_id, component_id, msg, set_global_position_setpoint_int->coordinate_frame, set_global_position_setpoint_int->latitude, set_global_position_setpoint_int->longitude, set_global_position_setpoint_int->altitude, set_global_position_setpoint_int->yaw); -} - -/** - * @brief Encode a set_global_position_setpoint_int struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_global_position_setpoint_int C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_global_position_setpoint_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_global_position_setpoint_int_t* set_global_position_setpoint_int) -{ - return mavlink_msg_set_global_position_setpoint_int_pack_chan(system_id, component_id, chan, msg, set_global_position_setpoint_int->coordinate_frame, set_global_position_setpoint_int->latitude, set_global_position_setpoint_int->longitude, set_global_position_setpoint_int->altitude, set_global_position_setpoint_int->yaw); -} - -/** - * @brief Send a set_global_position_setpoint_int message - * @param chan MAVLink channel to send the message - * - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT - * @param latitude Latitude (WGS84), in degrees * 1E7 - * @param longitude Longitude (WGS84), in degrees * 1E7 - * @param altitude Altitude (WGS84), in meters * 1000 (positive for up) - * @param yaw Desired yaw angle in degrees * 100 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_global_position_setpoint_int_send(mavlink_channel_t chan, uint8_t coordinate_frame, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - _mav_put_int16_t(buf, 12, yaw); - _mav_put_uint8_t(buf, 14, coordinate_frame); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -#else - mavlink_set_global_position_setpoint_int_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - packet.yaw = yaw; - packet.coordinate_frame = coordinate_frame; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, (const char *)&packet, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, (const char *)&packet, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SET_GLOBAL_POSITION_SETPOINT_INT UNPACKING - - -/** - * @brief Get field coordinate_frame from set_global_position_setpoint_int message - * - * @return Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT - */ -static inline uint8_t mavlink_msg_set_global_position_setpoint_int_get_coordinate_frame(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 14); -} - -/** - * @brief Get field latitude from set_global_position_setpoint_int message - * - * @return Latitude (WGS84), in degrees * 1E7 - */ -static inline int32_t mavlink_msg_set_global_position_setpoint_int_get_latitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field longitude from set_global_position_setpoint_int message - * - * @return Longitude (WGS84), in degrees * 1E7 - */ -static inline int32_t mavlink_msg_set_global_position_setpoint_int_get_longitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field altitude from set_global_position_setpoint_int message - * - * @return Altitude (WGS84), in meters * 1000 (positive for up) - */ -static inline int32_t mavlink_msg_set_global_position_setpoint_int_get_altitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Get field yaw from set_global_position_setpoint_int message - * - * @return Desired yaw angle in degrees * 100 - */ -static inline int16_t mavlink_msg_set_global_position_setpoint_int_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 12); -} - -/** - * @brief Decode a set_global_position_setpoint_int message into a struct - * - * @param msg The message to decode - * @param set_global_position_setpoint_int C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_global_position_setpoint_int_decode(const mavlink_message_t* msg, mavlink_set_global_position_setpoint_int_t* set_global_position_setpoint_int) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_global_position_setpoint_int->latitude = mavlink_msg_set_global_position_setpoint_int_get_latitude(msg); - set_global_position_setpoint_int->longitude = mavlink_msg_set_global_position_setpoint_int_get_longitude(msg); - set_global_position_setpoint_int->altitude = mavlink_msg_set_global_position_setpoint_int_get_altitude(msg); - set_global_position_setpoint_int->yaw = mavlink_msg_set_global_position_setpoint_int_get_yaw(msg); - set_global_position_setpoint_int->coordinate_frame = mavlink_msg_set_global_position_setpoint_int_get_coordinate_frame(msg); -#else - memcpy(set_global_position_setpoint_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h index c444d8d..1c018cb 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_set_gps_global_origin_send(mavlink_channel_t chan #endif } +#if MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_gps_global_origin_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint8_t(buf, 12, target_system); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); +#endif +#else + mavlink_set_gps_global_origin_t *packet = (mavlink_set_gps_global_origin_t *)msgbuf; + packet->latitude = latitude; + packet->longitude = longitude; + packet->altitude = altitude; + packet->target_system = target_system; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_GPS_GLOBAL_ORIGIN UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h deleted file mode 100644 index 6f2835e..0000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h +++ /dev/null @@ -1,309 +0,0 @@ -// MESSAGE SET_LOCAL_POSITION_SETPOINT PACKING - -#define MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT 50 - -typedef struct __mavlink_set_local_position_setpoint_t -{ - float x; ///< x position - float y; ///< y position - float z; ///< z position - float yaw; ///< Desired yaw angle - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t coordinate_frame; ///< Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU -} mavlink_set_local_position_setpoint_t; - -#define MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN 19 -#define MAVLINK_MSG_ID_50_LEN 19 - -#define MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_CRC 214 -#define MAVLINK_MSG_ID_50_CRC 214 - - - -#define MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT { \ - "SET_LOCAL_POSITION_SETPOINT", \ - 7, \ - { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_set_local_position_setpoint_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_set_local_position_setpoint_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_set_local_position_setpoint_t, z) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_set_local_position_setpoint_t, yaw) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_set_local_position_setpoint_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_set_local_position_setpoint_t, target_component) }, \ - { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_set_local_position_setpoint_t, coordinate_frame) }, \ - } \ -} - - -/** - * @brief Pack a set_local_position_setpoint message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU - * @param x x position - * @param y y position - * @param z z position - * @param yaw Desired yaw angle - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_local_position_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - _mav_put_uint8_t(buf, 18, coordinate_frame); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); -#else - mavlink_set_local_position_setpoint_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.target_system = target_system; - packet.target_component = target_component; - packet.coordinate_frame = coordinate_frame; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); -#endif -} - -/** - * @brief Pack a set_local_position_setpoint message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU - * @param x x position - * @param y y position - * @param z z position - * @param yaw Desired yaw angle - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_local_position_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t coordinate_frame,float x,float y,float z,float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - _mav_put_uint8_t(buf, 18, coordinate_frame); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); -#else - mavlink_set_local_position_setpoint_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.target_system = target_system; - packet.target_component = target_component; - packet.coordinate_frame = coordinate_frame; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); -#endif -} - -/** - * @brief Encode a set_local_position_setpoint struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_local_position_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_local_position_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_local_position_setpoint_t* set_local_position_setpoint) -{ - return mavlink_msg_set_local_position_setpoint_pack(system_id, component_id, msg, set_local_position_setpoint->target_system, set_local_position_setpoint->target_component, set_local_position_setpoint->coordinate_frame, set_local_position_setpoint->x, set_local_position_setpoint->y, set_local_position_setpoint->z, set_local_position_setpoint->yaw); -} - -/** - * @brief Encode a set_local_position_setpoint struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_local_position_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_local_position_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_local_position_setpoint_t* set_local_position_setpoint) -{ - return mavlink_msg_set_local_position_setpoint_pack_chan(system_id, component_id, chan, msg, set_local_position_setpoint->target_system, set_local_position_setpoint->target_component, set_local_position_setpoint->coordinate_frame, set_local_position_setpoint->x, set_local_position_setpoint->y, set_local_position_setpoint->z, set_local_position_setpoint->yaw); -} - -/** - * @brief Send a set_local_position_setpoint message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU - * @param x x position - * @param y y position - * @param z z position - * @param yaw Desired yaw angle - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_local_position_setpoint_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - _mav_put_uint8_t(buf, 18, coordinate_frame); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); -#endif -#else - mavlink_set_local_position_setpoint_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.target_system = target_system; - packet.target_component = target_component; - packet.coordinate_frame = coordinate_frame; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SET_LOCAL_POSITION_SETPOINT UNPACKING - - -/** - * @brief Get field target_system from set_local_position_setpoint message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_set_local_position_setpoint_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Get field target_component from set_local_position_setpoint message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_set_local_position_setpoint_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 17); -} - -/** - * @brief Get field coordinate_frame from set_local_position_setpoint message - * - * @return Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU - */ -static inline uint8_t mavlink_msg_set_local_position_setpoint_get_coordinate_frame(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 18); -} - -/** - * @brief Get field x from set_local_position_setpoint message - * - * @return x position - */ -static inline float mavlink_msg_set_local_position_setpoint_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field y from set_local_position_setpoint message - * - * @return y position - */ -static inline float mavlink_msg_set_local_position_setpoint_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field z from set_local_position_setpoint message - * - * @return z position - */ -static inline float mavlink_msg_set_local_position_setpoint_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yaw from set_local_position_setpoint message - * - * @return Desired yaw angle - */ -static inline float mavlink_msg_set_local_position_setpoint_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a set_local_position_setpoint message into a struct - * - * @param msg The message to decode - * @param set_local_position_setpoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_local_position_setpoint_decode(const mavlink_message_t* msg, mavlink_set_local_position_setpoint_t* set_local_position_setpoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_local_position_setpoint->x = mavlink_msg_set_local_position_setpoint_get_x(msg); - set_local_position_setpoint->y = mavlink_msg_set_local_position_setpoint_get_y(msg); - set_local_position_setpoint->z = mavlink_msg_set_local_position_setpoint_get_z(msg); - set_local_position_setpoint->yaw = mavlink_msg_set_local_position_setpoint_get_yaw(msg); - set_local_position_setpoint->target_system = mavlink_msg_set_local_position_setpoint_get_target_system(msg); - set_local_position_setpoint->target_component = mavlink_msg_set_local_position_setpoint_get_target_component(msg); - set_local_position_setpoint->coordinate_frame = mavlink_msg_set_local_position_setpoint_get_coordinate_frame(msg); -#else - memcpy(set_local_position_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h index 1aff42c..4b60a41 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t tar #endif } +#if MAVLINK_MSG_ID_SET_MODE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_mode_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t base_mode, uint32_t custom_mode) +{ +#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, target_system); + _mav_put_uint8_t(buf, 5, base_mode); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, buf, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, buf, MAVLINK_MSG_ID_SET_MODE_LEN); +#endif +#else + mavlink_set_mode_t *packet = (mavlink_set_mode_t *)msgbuf; + packet->custom_mode = custom_mode; + packet->target_system = target_system; + packet->base_mode = base_mode; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, (const char *)packet, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, (const char *)packet, MAVLINK_MSG_ID_SET_MODE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_MODE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_position_target_global_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_position_target_global_int.h new file mode 100644 index 0000000..4bb786d --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_position_target_global_int.h @@ -0,0 +1,569 @@ +// MESSAGE SET_POSITION_TARGET_GLOBAL_INT PACKING + +#define MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT 86 + +typedef struct __mavlink_set_position_target_global_int_t +{ + uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + int32_t lat_int; ///< X Position in WGS84 frame in 1e7 * meters + int32_t lon_int; ///< Y Position in WGS84 frame in 1e7 * meters + float alt; ///< Altitude in meters in WGS84 altitude, not AMSL if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + float vx; ///< X velocity in NED frame in meter / s + float vy; ///< Y velocity in NED frame in meter / s + float vz; ///< Z velocity in NED frame in meter / s + float afx; ///< X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float afy; ///< Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float afz; ///< Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float yaw; ///< yaw setpoint in rad + float yaw_rate; ///< yaw rate setpoint in rad/s + uint16_t type_mask; ///< Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t coordinate_frame; ///< Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 +} mavlink_set_position_target_global_int_t; + +#define MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN 53 +#define MAVLINK_MSG_ID_86_LEN 53 + +#define MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_CRC 5 +#define MAVLINK_MSG_ID_86_CRC 5 + + + +#define MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_GLOBAL_INT { \ + "SET_POSITION_TARGET_GLOBAL_INT", \ + 16, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_set_position_target_global_int_t, time_boot_ms) }, \ + { "lat_int", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_set_position_target_global_int_t, lat_int) }, \ + { "lon_int", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_set_position_target_global_int_t, lon_int) }, \ + { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_set_position_target_global_int_t, alt) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_set_position_target_global_int_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_set_position_target_global_int_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_set_position_target_global_int_t, vz) }, \ + { "afx", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_set_position_target_global_int_t, afx) }, \ + { "afy", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_set_position_target_global_int_t, afy) }, \ + { "afz", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_set_position_target_global_int_t, afz) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_set_position_target_global_int_t, yaw) }, \ + { "yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_set_position_target_global_int_t, yaw_rate) }, \ + { "type_mask", NULL, MAVLINK_TYPE_UINT16_T, 0, 48, offsetof(mavlink_set_position_target_global_int_t, type_mask) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 50, offsetof(mavlink_set_position_target_global_int_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 51, offsetof(mavlink_set_position_target_global_int_t, target_component) }, \ + { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_set_position_target_global_int_t, coordinate_frame) }, \ + } \ +} + + +/** + * @brief Pack a set_position_target_global_int message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + * @param target_system System ID + * @param target_component Component ID + * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param lat_int X Position in WGS84 frame in 1e7 * meters + * @param lon_int Y Position in WGS84 frame in 1e7 * meters + * @param alt Altitude in meters in WGS84 altitude, not AMSL if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_position_target_global_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t type_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#else + mavlink_set_position_target_global_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_int = lat_int; + packet.lon_int = lon_int; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.target_system = target_system; + packet.target_component = target_component; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +} + +/** + * @brief Pack a set_position_target_global_int message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + * @param target_system System ID + * @param target_component Component ID + * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param lat_int X Position in WGS84 frame in 1e7 * meters + * @param lon_int Y Position in WGS84 frame in 1e7 * meters + * @param alt Altitude in meters in WGS84 altitude, not AMSL if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_position_target_global_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t target_system,uint8_t target_component,uint8_t coordinate_frame,uint16_t type_mask,int32_t lat_int,int32_t lon_int,float alt,float vx,float vy,float vz,float afx,float afy,float afz,float yaw,float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#else + mavlink_set_position_target_global_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_int = lat_int; + packet.lon_int = lon_int; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.target_system = target_system; + packet.target_component = target_component; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +} + +/** + * @brief Encode a set_position_target_global_int struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_position_target_global_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_position_target_global_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_position_target_global_int_t* set_position_target_global_int) +{ + return mavlink_msg_set_position_target_global_int_pack(system_id, component_id, msg, set_position_target_global_int->time_boot_ms, set_position_target_global_int->target_system, set_position_target_global_int->target_component, set_position_target_global_int->coordinate_frame, set_position_target_global_int->type_mask, set_position_target_global_int->lat_int, set_position_target_global_int->lon_int, set_position_target_global_int->alt, set_position_target_global_int->vx, set_position_target_global_int->vy, set_position_target_global_int->vz, set_position_target_global_int->afx, set_position_target_global_int->afy, set_position_target_global_int->afz, set_position_target_global_int->yaw, set_position_target_global_int->yaw_rate); +} + +/** + * @brief Encode a set_position_target_global_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_position_target_global_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_position_target_global_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_position_target_global_int_t* set_position_target_global_int) +{ + return mavlink_msg_set_position_target_global_int_pack_chan(system_id, component_id, chan, msg, set_position_target_global_int->time_boot_ms, set_position_target_global_int->target_system, set_position_target_global_int->target_component, set_position_target_global_int->coordinate_frame, set_position_target_global_int->type_mask, set_position_target_global_int->lat_int, set_position_target_global_int->lon_int, set_position_target_global_int->alt, set_position_target_global_int->vx, set_position_target_global_int->vy, set_position_target_global_int->vz, set_position_target_global_int->afx, set_position_target_global_int->afy, set_position_target_global_int->afz, set_position_target_global_int->yaw, set_position_target_global_int->yaw_rate); +} + +/** + * @brief Send a set_position_target_global_int message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + * @param target_system System ID + * @param target_component Component ID + * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param lat_int X Position in WGS84 frame in 1e7 * meters + * @param lon_int Y Position in WGS84 frame in 1e7 * meters + * @param alt Altitude in meters in WGS84 altitude, not AMSL if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_set_position_target_global_int_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t type_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT, buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT, buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +#else + mavlink_set_position_target_global_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_int = lat_int; + packet.lon_int = lon_int; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.target_system = target_system; + packet.target_component = target_component; + packet.coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT, (const char *)&packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT, (const char *)&packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_position_target_global_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t type_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT, buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT, buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +#else + mavlink_set_position_target_global_int_t *packet = (mavlink_set_position_target_global_int_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->lat_int = lat_int; + packet->lon_int = lon_int; + packet->alt = alt; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->afx = afx; + packet->afy = afy; + packet->afz = afz; + packet->yaw = yaw; + packet->yaw_rate = yaw_rate; + packet->type_mask = type_mask; + packet->target_system = target_system; + packet->target_component = target_component; + packet->coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT, (const char *)packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT, (const char *)packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SET_POSITION_TARGET_GLOBAL_INT UNPACKING + + +/** + * @brief Get field time_boot_ms from set_position_target_global_int message + * + * @return Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + */ +static inline uint32_t mavlink_msg_set_position_target_global_int_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field target_system from set_position_target_global_int message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_position_target_global_int_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 50); +} + +/** + * @brief Get field target_component from set_position_target_global_int message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_position_target_global_int_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 51); +} + +/** + * @brief Get field coordinate_frame from set_position_target_global_int message + * + * @return Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + */ +static inline uint8_t mavlink_msg_set_position_target_global_int_get_coordinate_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 52); +} + +/** + * @brief Get field type_mask from set_position_target_global_int message + * + * @return Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + */ +static inline uint16_t mavlink_msg_set_position_target_global_int_get_type_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 48); +} + +/** + * @brief Get field lat_int from set_position_target_global_int message + * + * @return X Position in WGS84 frame in 1e7 * meters + */ +static inline int32_t mavlink_msg_set_position_target_global_int_get_lat_int(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field lon_int from set_position_target_global_int message + * + * @return Y Position in WGS84 frame in 1e7 * meters + */ +static inline int32_t mavlink_msg_set_position_target_global_int_get_lon_int(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field alt from set_position_target_global_int message + * + * @return Altitude in meters in WGS84 altitude, not AMSL if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + */ +static inline float mavlink_msg_set_position_target_global_int_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field vx from set_position_target_global_int message + * + * @return X velocity in NED frame in meter / s + */ +static inline float mavlink_msg_set_position_target_global_int_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field vy from set_position_target_global_int message + * + * @return Y velocity in NED frame in meter / s + */ +static inline float mavlink_msg_set_position_target_global_int_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field vz from set_position_target_global_int message + * + * @return Z velocity in NED frame in meter / s + */ +static inline float mavlink_msg_set_position_target_global_int_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field afx from set_position_target_global_int message + * + * @return X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_set_position_target_global_int_get_afx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field afy from set_position_target_global_int message + * + * @return Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_set_position_target_global_int_get_afy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field afz from set_position_target_global_int message + * + * @return Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_set_position_target_global_int_get_afz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field yaw from set_position_target_global_int message + * + * @return yaw setpoint in rad + */ +static inline float mavlink_msg_set_position_target_global_int_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field yaw_rate from set_position_target_global_int message + * + * @return yaw rate setpoint in rad/s + */ +static inline float mavlink_msg_set_position_target_global_int_get_yaw_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Decode a set_position_target_global_int message into a struct + * + * @param msg The message to decode + * @param set_position_target_global_int C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_position_target_global_int_decode(const mavlink_message_t* msg, mavlink_set_position_target_global_int_t* set_position_target_global_int) +{ +#if MAVLINK_NEED_BYTE_SWAP + set_position_target_global_int->time_boot_ms = mavlink_msg_set_position_target_global_int_get_time_boot_ms(msg); + set_position_target_global_int->lat_int = mavlink_msg_set_position_target_global_int_get_lat_int(msg); + set_position_target_global_int->lon_int = mavlink_msg_set_position_target_global_int_get_lon_int(msg); + set_position_target_global_int->alt = mavlink_msg_set_position_target_global_int_get_alt(msg); + set_position_target_global_int->vx = mavlink_msg_set_position_target_global_int_get_vx(msg); + set_position_target_global_int->vy = mavlink_msg_set_position_target_global_int_get_vy(msg); + set_position_target_global_int->vz = mavlink_msg_set_position_target_global_int_get_vz(msg); + set_position_target_global_int->afx = mavlink_msg_set_position_target_global_int_get_afx(msg); + set_position_target_global_int->afy = mavlink_msg_set_position_target_global_int_get_afy(msg); + set_position_target_global_int->afz = mavlink_msg_set_position_target_global_int_get_afz(msg); + set_position_target_global_int->yaw = mavlink_msg_set_position_target_global_int_get_yaw(msg); + set_position_target_global_int->yaw_rate = mavlink_msg_set_position_target_global_int_get_yaw_rate(msg); + set_position_target_global_int->type_mask = mavlink_msg_set_position_target_global_int_get_type_mask(msg); + set_position_target_global_int->target_system = mavlink_msg_set_position_target_global_int_get_target_system(msg); + set_position_target_global_int->target_component = mavlink_msg_set_position_target_global_int_get_target_component(msg); + set_position_target_global_int->coordinate_frame = mavlink_msg_set_position_target_global_int_get_coordinate_frame(msg); +#else + memcpy(set_position_target_global_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_position_target_local_ned.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_position_target_local_ned.h new file mode 100644 index 0000000..8fcc3cb --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_position_target_local_ned.h @@ -0,0 +1,569 @@ +// MESSAGE SET_POSITION_TARGET_LOCAL_NED PACKING + +#define MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED 84 + +typedef struct __mavlink_set_position_target_local_ned_t +{ + uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot + float x; ///< X Position in NED frame in meters + float y; ///< Y Position in NED frame in meters + float z; ///< Z Position in NED frame in meters (note, altitude is negative in NED) + float vx; ///< X velocity in NED frame in meter / s + float vy; ///< Y velocity in NED frame in meter / s + float vz; ///< Z velocity in NED frame in meter / s + float afx; ///< X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float afy; ///< Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float afz; ///< Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float yaw; ///< yaw setpoint in rad + float yaw_rate; ///< yaw rate setpoint in rad/s + uint16_t type_mask; ///< Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t coordinate_frame; ///< Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 +} mavlink_set_position_target_local_ned_t; + +#define MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN 53 +#define MAVLINK_MSG_ID_84_LEN 53 + +#define MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_CRC 143 +#define MAVLINK_MSG_ID_84_CRC 143 + + + +#define MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_LOCAL_NED { \ + "SET_POSITION_TARGET_LOCAL_NED", \ + 16, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_set_position_target_local_ned_t, time_boot_ms) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_set_position_target_local_ned_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_set_position_target_local_ned_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_set_position_target_local_ned_t, z) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_set_position_target_local_ned_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_set_position_target_local_ned_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_set_position_target_local_ned_t, vz) }, \ + { "afx", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_set_position_target_local_ned_t, afx) }, \ + { "afy", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_set_position_target_local_ned_t, afy) }, \ + { "afz", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_set_position_target_local_ned_t, afz) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_set_position_target_local_ned_t, yaw) }, \ + { "yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_set_position_target_local_ned_t, yaw_rate) }, \ + { "type_mask", NULL, MAVLINK_TYPE_UINT16_T, 0, 48, offsetof(mavlink_set_position_target_local_ned_t, type_mask) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 50, offsetof(mavlink_set_position_target_local_ned_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 51, offsetof(mavlink_set_position_target_local_ned_t, target_component) }, \ + { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_set_position_target_local_ned_t, coordinate_frame) }, \ + } \ +} + + +/** + * @brief Pack a set_position_target_local_ned message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param target_system System ID + * @param target_component Component ID + * @param coordinate_frame Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param x X Position in NED frame in meters + * @param y Y Position in NED frame in meters + * @param z Z Position in NED frame in meters (note, altitude is negative in NED) + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_position_target_local_ned_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t type_mask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#else + mavlink_set_position_target_local_ned_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.target_system = target_system; + packet.target_component = target_component; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#endif +} + +/** + * @brief Pack a set_position_target_local_ned message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param target_system System ID + * @param target_component Component ID + * @param coordinate_frame Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param x X Position in NED frame in meters + * @param y Y Position in NED frame in meters + * @param z Z Position in NED frame in meters (note, altitude is negative in NED) + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_position_target_local_ned_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t target_system,uint8_t target_component,uint8_t coordinate_frame,uint16_t type_mask,float x,float y,float z,float vx,float vy,float vz,float afx,float afy,float afz,float yaw,float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#else + mavlink_set_position_target_local_ned_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.target_system = target_system; + packet.target_component = target_component; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#endif +} + +/** + * @brief Encode a set_position_target_local_ned struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_position_target_local_ned C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_position_target_local_ned_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_position_target_local_ned_t* set_position_target_local_ned) +{ + return mavlink_msg_set_position_target_local_ned_pack(system_id, component_id, msg, set_position_target_local_ned->time_boot_ms, set_position_target_local_ned->target_system, set_position_target_local_ned->target_component, set_position_target_local_ned->coordinate_frame, set_position_target_local_ned->type_mask, set_position_target_local_ned->x, set_position_target_local_ned->y, set_position_target_local_ned->z, set_position_target_local_ned->vx, set_position_target_local_ned->vy, set_position_target_local_ned->vz, set_position_target_local_ned->afx, set_position_target_local_ned->afy, set_position_target_local_ned->afz, set_position_target_local_ned->yaw, set_position_target_local_ned->yaw_rate); +} + +/** + * @brief Encode a set_position_target_local_ned struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_position_target_local_ned C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_position_target_local_ned_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_position_target_local_ned_t* set_position_target_local_ned) +{ + return mavlink_msg_set_position_target_local_ned_pack_chan(system_id, component_id, chan, msg, set_position_target_local_ned->time_boot_ms, set_position_target_local_ned->target_system, set_position_target_local_ned->target_component, set_position_target_local_ned->coordinate_frame, set_position_target_local_ned->type_mask, set_position_target_local_ned->x, set_position_target_local_ned->y, set_position_target_local_ned->z, set_position_target_local_ned->vx, set_position_target_local_ned->vy, set_position_target_local_ned->vz, set_position_target_local_ned->afx, set_position_target_local_ned->afy, set_position_target_local_ned->afz, set_position_target_local_ned->yaw, set_position_target_local_ned->yaw_rate); +} + +/** + * @brief Send a set_position_target_local_ned message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param target_system System ID + * @param target_component Component ID + * @param coordinate_frame Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param x X Position in NED frame in meters + * @param y Y Position in NED frame in meters + * @param z Z Position in NED frame in meters (note, altitude is negative in NED) + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_set_position_target_local_ned_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t type_mask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED, buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED, buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#endif +#else + mavlink_set_position_target_local_ned_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.target_system = target_system; + packet.target_component = target_component; + packet.coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED, (const char *)&packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED, (const char *)&packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_position_target_local_ned_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t type_mask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#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, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED, buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED, buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#endif +#else + mavlink_set_position_target_local_ned_t *packet = (mavlink_set_position_target_local_ned_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->x = x; + packet->y = y; + packet->z = z; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->afx = afx; + packet->afy = afy; + packet->afz = afz; + packet->yaw = yaw; + packet->yaw_rate = yaw_rate; + packet->type_mask = type_mask; + packet->target_system = target_system; + packet->target_component = target_component; + packet->coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED, (const char *)packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED, (const char *)packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SET_POSITION_TARGET_LOCAL_NED UNPACKING + + +/** + * @brief Get field time_boot_ms from set_position_target_local_ned message + * + * @return Timestamp in milliseconds since system boot + */ +static inline uint32_t mavlink_msg_set_position_target_local_ned_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field target_system from set_position_target_local_ned message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_position_target_local_ned_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 50); +} + +/** + * @brief Get field target_component from set_position_target_local_ned message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_position_target_local_ned_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 51); +} + +/** + * @brief Get field coordinate_frame from set_position_target_local_ned message + * + * @return Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + */ +static inline uint8_t mavlink_msg_set_position_target_local_ned_get_coordinate_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 52); +} + +/** + * @brief Get field type_mask from set_position_target_local_ned message + * + * @return Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + */ +static inline uint16_t mavlink_msg_set_position_target_local_ned_get_type_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 48); +} + +/** + * @brief Get field x from set_position_target_local_ned message + * + * @return X Position in NED frame in meters + */ +static inline float mavlink_msg_set_position_target_local_ned_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field y from set_position_target_local_ned message + * + * @return Y Position in NED frame in meters + */ +static inline float mavlink_msg_set_position_target_local_ned_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field z from set_position_target_local_ned message + * + * @return Z Position in NED frame in meters (note, altitude is negative in NED) + */ +static inline float mavlink_msg_set_position_target_local_ned_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field vx from set_position_target_local_ned message + * + * @return X velocity in NED frame in meter / s + */ +static inline float mavlink_msg_set_position_target_local_ned_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field vy from set_position_target_local_ned message + * + * @return Y velocity in NED frame in meter / s + */ +static inline float mavlink_msg_set_position_target_local_ned_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field vz from set_position_target_local_ned message + * + * @return Z velocity in NED frame in meter / s + */ +static inline float mavlink_msg_set_position_target_local_ned_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field afx from set_position_target_local_ned message + * + * @return X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_set_position_target_local_ned_get_afx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field afy from set_position_target_local_ned message + * + * @return Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_set_position_target_local_ned_get_afy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field afz from set_position_target_local_ned message + * + * @return Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_set_position_target_local_ned_get_afz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field yaw from set_position_target_local_ned message + * + * @return yaw setpoint in rad + */ +static inline float mavlink_msg_set_position_target_local_ned_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field yaw_rate from set_position_target_local_ned message + * + * @return yaw rate setpoint in rad/s + */ +static inline float mavlink_msg_set_position_target_local_ned_get_yaw_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Decode a set_position_target_local_ned message into a struct + * + * @param msg The message to decode + * @param set_position_target_local_ned C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_position_target_local_ned_decode(const mavlink_message_t* msg, mavlink_set_position_target_local_ned_t* set_position_target_local_ned) +{ +#if MAVLINK_NEED_BYTE_SWAP + set_position_target_local_ned->time_boot_ms = mavlink_msg_set_position_target_local_ned_get_time_boot_ms(msg); + set_position_target_local_ned->x = mavlink_msg_set_position_target_local_ned_get_x(msg); + set_position_target_local_ned->y = mavlink_msg_set_position_target_local_ned_get_y(msg); + set_position_target_local_ned->z = mavlink_msg_set_position_target_local_ned_get_z(msg); + set_position_target_local_ned->vx = mavlink_msg_set_position_target_local_ned_get_vx(msg); + set_position_target_local_ned->vy = mavlink_msg_set_position_target_local_ned_get_vy(msg); + set_position_target_local_ned->vz = mavlink_msg_set_position_target_local_ned_get_vz(msg); + set_position_target_local_ned->afx = mavlink_msg_set_position_target_local_ned_get_afx(msg); + set_position_target_local_ned->afy = mavlink_msg_set_position_target_local_ned_get_afy(msg); + set_position_target_local_ned->afz = mavlink_msg_set_position_target_local_ned_get_afz(msg); + set_position_target_local_ned->yaw = mavlink_msg_set_position_target_local_ned_get_yaw(msg); + set_position_target_local_ned->yaw_rate = mavlink_msg_set_position_target_local_ned_get_yaw_rate(msg); + set_position_target_local_ned->type_mask = mavlink_msg_set_position_target_local_ned_get_type_mask(msg); + set_position_target_local_ned->target_system = mavlink_msg_set_position_target_local_ned_get_target_system(msg); + set_position_target_local_ned->target_component = mavlink_msg_set_position_target_local_ned_get_target_component(msg); + set_position_target_local_ned->coordinate_frame = mavlink_msg_set_position_target_local_ned_get_coordinate_frame(msg); +#else + memcpy(set_position_target_local_ned, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h deleted file mode 100644 index 8ceb888..0000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h +++ /dev/null @@ -1,265 +0,0 @@ -// MESSAGE SET_QUAD_MOTORS_SETPOINT PACKING - -#define MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT 60 - -typedef struct __mavlink_set_quad_motors_setpoint_t -{ - uint16_t motor_front_nw; ///< Front motor in + configuration, front left motor in x configuration - uint16_t motor_right_ne; ///< Right motor in + configuration, front right motor in x configuration - uint16_t motor_back_se; ///< Back motor in + configuration, back right motor in x configuration - uint16_t motor_left_sw; ///< Left motor in + configuration, back left motor in x configuration - uint8_t target_system; ///< System ID of the system that should set these motor commands -} mavlink_set_quad_motors_setpoint_t; - -#define MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN 9 -#define MAVLINK_MSG_ID_60_LEN 9 - -#define MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_CRC 30 -#define MAVLINK_MSG_ID_60_CRC 30 - - - -#define MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT { \ - "SET_QUAD_MOTORS_SETPOINT", \ - 5, \ - { { "motor_front_nw", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_set_quad_motors_setpoint_t, motor_front_nw) }, \ - { "motor_right_ne", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_set_quad_motors_setpoint_t, motor_right_ne) }, \ - { "motor_back_se", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_set_quad_motors_setpoint_t, motor_back_se) }, \ - { "motor_left_sw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_set_quad_motors_setpoint_t, motor_left_sw) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_set_quad_motors_setpoint_t, target_system) }, \ - } \ -} - - -/** - * @brief Pack a set_quad_motors_setpoint message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID of the system that should set these motor commands - * @param motor_front_nw Front motor in + configuration, front left motor in x configuration - * @param motor_right_ne Right motor in + configuration, front right motor in x configuration - * @param motor_back_se Back motor in + configuration, back right motor in x configuration - * @param motor_left_sw Left motor in + configuration, back left motor in x configuration - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_quad_motors_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint16_t motor_front_nw, uint16_t motor_right_ne, uint16_t motor_back_se, uint16_t motor_left_sw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN]; - _mav_put_uint16_t(buf, 0, motor_front_nw); - _mav_put_uint16_t(buf, 2, motor_right_ne); - _mav_put_uint16_t(buf, 4, motor_back_se); - _mav_put_uint16_t(buf, 6, motor_left_sw); - _mav_put_uint8_t(buf, 8, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); -#else - mavlink_set_quad_motors_setpoint_t packet; - packet.motor_front_nw = motor_front_nw; - packet.motor_right_ne = motor_right_ne; - packet.motor_back_se = motor_back_se; - packet.motor_left_sw = motor_left_sw; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); -#endif -} - -/** - * @brief Pack a set_quad_motors_setpoint message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID of the system that should set these motor commands - * @param motor_front_nw Front motor in + configuration, front left motor in x configuration - * @param motor_right_ne Right motor in + configuration, front right motor in x configuration - * @param motor_back_se Back motor in + configuration, back right motor in x configuration - * @param motor_left_sw Left motor in + configuration, back left motor in x configuration - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_quad_motors_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint16_t motor_front_nw,uint16_t motor_right_ne,uint16_t motor_back_se,uint16_t motor_left_sw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN]; - _mav_put_uint16_t(buf, 0, motor_front_nw); - _mav_put_uint16_t(buf, 2, motor_right_ne); - _mav_put_uint16_t(buf, 4, motor_back_se); - _mav_put_uint16_t(buf, 6, motor_left_sw); - _mav_put_uint8_t(buf, 8, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); -#else - mavlink_set_quad_motors_setpoint_t packet; - packet.motor_front_nw = motor_front_nw; - packet.motor_right_ne = motor_right_ne; - packet.motor_back_se = motor_back_se; - packet.motor_left_sw = motor_left_sw; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); -#endif -} - -/** - * @brief Encode a set_quad_motors_setpoint struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_quad_motors_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_quad_motors_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_quad_motors_setpoint_t* set_quad_motors_setpoint) -{ - return mavlink_msg_set_quad_motors_setpoint_pack(system_id, component_id, msg, set_quad_motors_setpoint->target_system, set_quad_motors_setpoint->motor_front_nw, set_quad_motors_setpoint->motor_right_ne, set_quad_motors_setpoint->motor_back_se, set_quad_motors_setpoint->motor_left_sw); -} - -/** - * @brief Encode a set_quad_motors_setpoint struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_quad_motors_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_quad_motors_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_quad_motors_setpoint_t* set_quad_motors_setpoint) -{ - return mavlink_msg_set_quad_motors_setpoint_pack_chan(system_id, component_id, chan, msg, set_quad_motors_setpoint->target_system, set_quad_motors_setpoint->motor_front_nw, set_quad_motors_setpoint->motor_right_ne, set_quad_motors_setpoint->motor_back_se, set_quad_motors_setpoint->motor_left_sw); -} - -/** - * @brief Send a set_quad_motors_setpoint message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID of the system that should set these motor commands - * @param motor_front_nw Front motor in + configuration, front left motor in x configuration - * @param motor_right_ne Right motor in + configuration, front right motor in x configuration - * @param motor_back_se Back motor in + configuration, back right motor in x configuration - * @param motor_left_sw Left motor in + configuration, back left motor in x configuration - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_quad_motors_setpoint_send(mavlink_channel_t chan, uint8_t target_system, uint16_t motor_front_nw, uint16_t motor_right_ne, uint16_t motor_back_se, uint16_t motor_left_sw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN]; - _mav_put_uint16_t(buf, 0, motor_front_nw); - _mav_put_uint16_t(buf, 2, motor_right_ne); - _mav_put_uint16_t(buf, 4, motor_back_se); - _mav_put_uint16_t(buf, 6, motor_left_sw); - _mav_put_uint8_t(buf, 8, target_system); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, buf, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, buf, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); -#endif -#else - mavlink_set_quad_motors_setpoint_t packet; - packet.motor_front_nw = motor_front_nw; - packet.motor_right_ne = motor_right_ne; - packet.motor_back_se = motor_back_se; - packet.motor_left_sw = motor_left_sw; - packet.target_system = target_system; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SET_QUAD_MOTORS_SETPOINT UNPACKING - - -/** - * @brief Get field target_system from set_quad_motors_setpoint message - * - * @return System ID of the system that should set these motor commands - */ -static inline uint8_t mavlink_msg_set_quad_motors_setpoint_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 8); -} - -/** - * @brief Get field motor_front_nw from set_quad_motors_setpoint message - * - * @return Front motor in + configuration, front left motor in x configuration - */ -static inline uint16_t mavlink_msg_set_quad_motors_setpoint_get_motor_front_nw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field motor_right_ne from set_quad_motors_setpoint message - * - * @return Right motor in + configuration, front right motor in x configuration - */ -static inline uint16_t mavlink_msg_set_quad_motors_setpoint_get_motor_right_ne(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Get field motor_back_se from set_quad_motors_setpoint message - * - * @return Back motor in + configuration, back right motor in x configuration - */ -static inline uint16_t mavlink_msg_set_quad_motors_setpoint_get_motor_back_se(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field motor_left_sw from set_quad_motors_setpoint message - * - * @return Left motor in + configuration, back left motor in x configuration - */ -static inline uint16_t mavlink_msg_set_quad_motors_setpoint_get_motor_left_sw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Decode a set_quad_motors_setpoint message into a struct - * - * @param msg The message to decode - * @param set_quad_motors_setpoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_quad_motors_setpoint_decode(const mavlink_message_t* msg, mavlink_set_quad_motors_setpoint_t* set_quad_motors_setpoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_quad_motors_setpoint->motor_front_nw = mavlink_msg_set_quad_motors_setpoint_get_motor_front_nw(msg); - set_quad_motors_setpoint->motor_right_ne = mavlink_msg_set_quad_motors_setpoint_get_motor_right_ne(msg); - set_quad_motors_setpoint->motor_back_se = mavlink_msg_set_quad_motors_setpoint_get_motor_back_se(msg); - set_quad_motors_setpoint->motor_left_sw = mavlink_msg_set_quad_motors_setpoint_get_motor_left_sw(msg); - set_quad_motors_setpoint->target_system = mavlink_msg_set_quad_motors_setpoint_get_target_system(msg); -#else - memcpy(set_quad_motors_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h deleted file mode 100644 index 9ef294c..0000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST PACKING - -#define MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST 63 - -typedef struct __mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t -{ - int16_t roll[4]; ///< Desired roll angle in radians +-PI (+-INT16_MAX) - int16_t pitch[4]; ///< Desired pitch angle in radians +-PI (+-INT16_MAX) - int16_t yaw[4]; ///< Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) - uint16_t thrust[4]; ///< Collective thrust, scaled to uint16 (0..UINT16_MAX) - uint8_t group; ///< ID of the quadrotor group (0 - 255, up to 256 groups supported) - uint8_t mode; ///< ID of the flight mode (0 - 255, up to 256 modes supported) - uint8_t led_red[4]; ///< RGB red channel (0-255) - uint8_t led_blue[4]; ///< RGB green channel (0-255) - uint8_t led_green[4]; ///< RGB blue channel (0-255) -} mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t; - -#define MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN 46 -#define MAVLINK_MSG_ID_63_LEN 46 - -#define MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_CRC 130 -#define MAVLINK_MSG_ID_63_CRC 130 - -#define MAVLINK_MSG_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_FIELD_ROLL_LEN 4 -#define MAVLINK_MSG_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_FIELD_PITCH_LEN 4 -#define MAVLINK_MSG_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_FIELD_YAW_LEN 4 -#define MAVLINK_MSG_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_FIELD_THRUST_LEN 4 -#define MAVLINK_MSG_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_FIELD_LED_RED_LEN 4 -#define MAVLINK_MSG_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_FIELD_LED_BLUE_LEN 4 -#define MAVLINK_MSG_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_FIELD_LED_GREEN_LEN 4 - -#define MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST { \ - "SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST", \ - 9, \ - { { "roll", NULL, MAVLINK_TYPE_INT16_T, 4, 0, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_INT16_T, 4, 8, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_INT16_T, 4, 16, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, yaw) }, \ - { "thrust", NULL, MAVLINK_TYPE_UINT16_T, 4, 24, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, thrust) }, \ - { "group", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, group) }, \ - { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, mode) }, \ - { "led_red", NULL, MAVLINK_TYPE_UINT8_T, 4, 34, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, led_red) }, \ - { "led_blue", NULL, MAVLINK_TYPE_UINT8_T, 4, 38, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, led_blue) }, \ - { "led_green", NULL, MAVLINK_TYPE_UINT8_T, 4, 42, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, led_green) }, \ - } \ -} - - -/** - * @brief Pack a set_quad_swarm_led_roll_pitch_yaw_thrust message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) - * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) - * @param led_red RGB red channel (0-255) - * @param led_blue RGB green channel (0-255) - * @param led_green RGB blue channel (0-255) - * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) - * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) - * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t group, uint8_t mode, const uint8_t *led_red, const uint8_t *led_blue, const uint8_t *led_green, const int16_t *roll, const int16_t *pitch, const int16_t *yaw, const uint16_t *thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN]; - _mav_put_uint8_t(buf, 32, group); - _mav_put_uint8_t(buf, 33, mode); - _mav_put_int16_t_array(buf, 0, roll, 4); - _mav_put_int16_t_array(buf, 8, pitch, 4); - _mav_put_int16_t_array(buf, 16, yaw, 4); - _mav_put_uint16_t_array(buf, 24, thrust, 4); - _mav_put_uint8_t_array(buf, 34, led_red, 4); - _mav_put_uint8_t_array(buf, 38, led_blue, 4); - _mav_put_uint8_t_array(buf, 42, led_green, 4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); -#else - mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t packet; - packet.group = group; - packet.mode = mode; - mav_array_memcpy(packet.roll, roll, sizeof(int16_t)*4); - mav_array_memcpy(packet.pitch, pitch, sizeof(int16_t)*4); - mav_array_memcpy(packet.yaw, yaw, sizeof(int16_t)*4); - mav_array_memcpy(packet.thrust, thrust, sizeof(uint16_t)*4); - mav_array_memcpy(packet.led_red, led_red, sizeof(uint8_t)*4); - mav_array_memcpy(packet.led_blue, led_blue, sizeof(uint8_t)*4); - mav_array_memcpy(packet.led_green, led_green, sizeof(uint8_t)*4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); -#endif -} - -/** - * @brief Pack a set_quad_swarm_led_roll_pitch_yaw_thrust message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) - * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) - * @param led_red RGB red channel (0-255) - * @param led_blue RGB green channel (0-255) - * @param led_green RGB blue channel (0-255) - * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) - * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) - * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t group,uint8_t mode,const uint8_t *led_red,const uint8_t *led_blue,const uint8_t *led_green,const int16_t *roll,const int16_t *pitch,const int16_t *yaw,const uint16_t *thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN]; - _mav_put_uint8_t(buf, 32, group); - _mav_put_uint8_t(buf, 33, mode); - _mav_put_int16_t_array(buf, 0, roll, 4); - _mav_put_int16_t_array(buf, 8, pitch, 4); - _mav_put_int16_t_array(buf, 16, yaw, 4); - _mav_put_uint16_t_array(buf, 24, thrust, 4); - _mav_put_uint8_t_array(buf, 34, led_red, 4); - _mav_put_uint8_t_array(buf, 38, led_blue, 4); - _mav_put_uint8_t_array(buf, 42, led_green, 4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); -#else - mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t packet; - packet.group = group; - packet.mode = mode; - mav_array_memcpy(packet.roll, roll, sizeof(int16_t)*4); - mav_array_memcpy(packet.pitch, pitch, sizeof(int16_t)*4); - mav_array_memcpy(packet.yaw, yaw, sizeof(int16_t)*4); - mav_array_memcpy(packet.thrust, thrust, sizeof(uint16_t)*4); - mav_array_memcpy(packet.led_red, led_red, sizeof(uint8_t)*4); - mav_array_memcpy(packet.led_blue, led_blue, sizeof(uint8_t)*4); - mav_array_memcpy(packet.led_green, led_green, sizeof(uint8_t)*4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); -#endif -} - -/** - * @brief Encode a set_quad_swarm_led_roll_pitch_yaw_thrust struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_quad_swarm_led_roll_pitch_yaw_thrust C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t* set_quad_swarm_led_roll_pitch_yaw_thrust) -{ - return mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack(system_id, component_id, msg, set_quad_swarm_led_roll_pitch_yaw_thrust->group, set_quad_swarm_led_roll_pitch_yaw_thrust->mode, set_quad_swarm_led_roll_pitch_yaw_thrust->led_red, set_quad_swarm_led_roll_pitch_yaw_thrust->led_blue, set_quad_swarm_led_roll_pitch_yaw_thrust->led_green, set_quad_swarm_led_roll_pitch_yaw_thrust->roll, set_quad_swarm_led_roll_pitch_yaw_thrust->pitch, set_quad_swarm_led_roll_pitch_yaw_thrust->yaw, set_quad_swarm_led_roll_pitch_yaw_thrust->thrust); -} - -/** - * @brief Encode a set_quad_swarm_led_roll_pitch_yaw_thrust struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_quad_swarm_led_roll_pitch_yaw_thrust C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t* set_quad_swarm_led_roll_pitch_yaw_thrust) -{ - return mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, chan, msg, set_quad_swarm_led_roll_pitch_yaw_thrust->group, set_quad_swarm_led_roll_pitch_yaw_thrust->mode, set_quad_swarm_led_roll_pitch_yaw_thrust->led_red, set_quad_swarm_led_roll_pitch_yaw_thrust->led_blue, set_quad_swarm_led_roll_pitch_yaw_thrust->led_green, set_quad_swarm_led_roll_pitch_yaw_thrust->roll, set_quad_swarm_led_roll_pitch_yaw_thrust->pitch, set_quad_swarm_led_roll_pitch_yaw_thrust->yaw, set_quad_swarm_led_roll_pitch_yaw_thrust->thrust); -} - -/** - * @brief Send a set_quad_swarm_led_roll_pitch_yaw_thrust message - * @param chan MAVLink channel to send the message - * - * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) - * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) - * @param led_red RGB red channel (0-255) - * @param led_blue RGB green channel (0-255) - * @param led_green RGB blue channel (0-255) - * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) - * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) - * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_send(mavlink_channel_t chan, uint8_t group, uint8_t mode, const uint8_t *led_red, const uint8_t *led_blue, const uint8_t *led_green, const int16_t *roll, const int16_t *pitch, const int16_t *yaw, const uint16_t *thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN]; - _mav_put_uint8_t(buf, 32, group); - _mav_put_uint8_t(buf, 33, mode); - _mav_put_int16_t_array(buf, 0, roll, 4); - _mav_put_int16_t_array(buf, 8, pitch, 4); - _mav_put_int16_t_array(buf, 16, yaw, 4); - _mav_put_uint16_t_array(buf, 24, thrust, 4); - _mav_put_uint8_t_array(buf, 34, led_red, 4); - _mav_put_uint8_t_array(buf, 38, led_blue, 4); - _mav_put_uint8_t_array(buf, 42, led_green, 4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); -#endif -#else - mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t packet; - packet.group = group; - packet.mode = mode; - mav_array_memcpy(packet.roll, roll, sizeof(int16_t)*4); - mav_array_memcpy(packet.pitch, pitch, sizeof(int16_t)*4); - mav_array_memcpy(packet.yaw, yaw, sizeof(int16_t)*4); - mav_array_memcpy(packet.thrust, thrust, sizeof(uint16_t)*4); - mav_array_memcpy(packet.led_red, led_red, sizeof(uint8_t)*4); - mav_array_memcpy(packet.led_blue, led_blue, sizeof(uint8_t)*4); - mav_array_memcpy(packet.led_green, led_green, sizeof(uint8_t)*4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, (const char *)&packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, (const char *)&packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST UNPACKING - - -/** - * @brief Get field group from set_quad_swarm_led_roll_pitch_yaw_thrust message - * - * @return ID of the quadrotor group (0 - 255, up to 256 groups supported) - */ -static inline uint8_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_group(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 32); -} - -/** - * @brief Get field mode from set_quad_swarm_led_roll_pitch_yaw_thrust message - * - * @return ID of the flight mode (0 - 255, up to 256 modes supported) - */ -static inline uint8_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 33); -} - -/** - * @brief Get field led_red from set_quad_swarm_led_roll_pitch_yaw_thrust message - * - * @return RGB red channel (0-255) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_led_red(const mavlink_message_t* msg, uint8_t *led_red) -{ - return _MAV_RETURN_uint8_t_array(msg, led_red, 4, 34); -} - -/** - * @brief Get field led_blue from set_quad_swarm_led_roll_pitch_yaw_thrust message - * - * @return RGB green channel (0-255) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_led_blue(const mavlink_message_t* msg, uint8_t *led_blue) -{ - return _MAV_RETURN_uint8_t_array(msg, led_blue, 4, 38); -} - -/** - * @brief Get field led_green from set_quad_swarm_led_roll_pitch_yaw_thrust message - * - * @return RGB blue channel (0-255) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_led_green(const mavlink_message_t* msg, uint8_t *led_green) -{ - return _MAV_RETURN_uint8_t_array(msg, led_green, 4, 42); -} - -/** - * @brief Get field roll from set_quad_swarm_led_roll_pitch_yaw_thrust message - * - * @return Desired roll angle in radians +-PI (+-INT16_MAX) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg, int16_t *roll) -{ - return _MAV_RETURN_int16_t_array(msg, roll, 4, 0); -} - -/** - * @brief Get field pitch from set_quad_swarm_led_roll_pitch_yaw_thrust message - * - * @return Desired pitch angle in radians +-PI (+-INT16_MAX) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg, int16_t *pitch) -{ - return _MAV_RETURN_int16_t_array(msg, pitch, 4, 8); -} - -/** - * @brief Get field yaw from set_quad_swarm_led_roll_pitch_yaw_thrust message - * - * @return Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg, int16_t *yaw) -{ - return _MAV_RETURN_int16_t_array(msg, yaw, 4, 16); -} - -/** - * @brief Get field thrust from set_quad_swarm_led_roll_pitch_yaw_thrust message - * - * @return Collective thrust, scaled to uint16 (0..UINT16_MAX) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg, uint16_t *thrust) -{ - return _MAV_RETURN_uint16_t_array(msg, thrust, 4, 24); -} - -/** - * @brief Decode a set_quad_swarm_led_roll_pitch_yaw_thrust message into a struct - * - * @param msg The message to decode - * @param set_quad_swarm_led_roll_pitch_yaw_thrust C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_decode(const mavlink_message_t* msg, mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t* set_quad_swarm_led_roll_pitch_yaw_thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_roll(msg, set_quad_swarm_led_roll_pitch_yaw_thrust->roll); - mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_pitch(msg, set_quad_swarm_led_roll_pitch_yaw_thrust->pitch); - mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_yaw(msg, set_quad_swarm_led_roll_pitch_yaw_thrust->yaw); - mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_thrust(msg, set_quad_swarm_led_roll_pitch_yaw_thrust->thrust); - set_quad_swarm_led_roll_pitch_yaw_thrust->group = mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_group(msg); - set_quad_swarm_led_roll_pitch_yaw_thrust->mode = mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_mode(msg); - mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_led_red(msg, set_quad_swarm_led_roll_pitch_yaw_thrust->led_red); - mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_led_blue(msg, set_quad_swarm_led_roll_pitch_yaw_thrust->led_blue); - mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_led_green(msg, set_quad_swarm_led_roll_pitch_yaw_thrust->led_green); -#else - memcpy(set_quad_swarm_led_roll_pitch_yaw_thrust, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h deleted file mode 100644 index 7d8d526..0000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h +++ /dev/null @@ -1,284 +0,0 @@ -// MESSAGE SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST PACKING - -#define MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST 61 - -typedef struct __mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t -{ - int16_t roll[4]; ///< Desired roll angle in radians +-PI (+-INT16_MAX) - int16_t pitch[4]; ///< Desired pitch angle in radians +-PI (+-INT16_MAX) - int16_t yaw[4]; ///< Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) - uint16_t thrust[4]; ///< Collective thrust, scaled to uint16 (0..UINT16_MAX) - uint8_t group; ///< ID of the quadrotor group (0 - 255, up to 256 groups supported) - uint8_t mode; ///< ID of the flight mode (0 - 255, up to 256 modes supported) -} mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t; - -#define MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN 34 -#define MAVLINK_MSG_ID_61_LEN 34 - -#define MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_CRC 240 -#define MAVLINK_MSG_ID_61_CRC 240 - -#define MAVLINK_MSG_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_FIELD_ROLL_LEN 4 -#define MAVLINK_MSG_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_FIELD_PITCH_LEN 4 -#define MAVLINK_MSG_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_FIELD_YAW_LEN 4 -#define MAVLINK_MSG_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_FIELD_THRUST_LEN 4 - -#define MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST { \ - "SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST", \ - 6, \ - { { "roll", NULL, MAVLINK_TYPE_INT16_T, 4, 0, offsetof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_INT16_T, 4, 8, offsetof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_INT16_T, 4, 16, offsetof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t, yaw) }, \ - { "thrust", NULL, MAVLINK_TYPE_UINT16_T, 4, 24, offsetof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t, thrust) }, \ - { "group", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t, group) }, \ - { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t, mode) }, \ - } \ -} - - -/** - * @brief Pack a set_quad_swarm_roll_pitch_yaw_thrust message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) - * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) - * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) - * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) - * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t group, uint8_t mode, const int16_t *roll, const int16_t *pitch, const int16_t *yaw, const uint16_t *thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN]; - _mav_put_uint8_t(buf, 32, group); - _mav_put_uint8_t(buf, 33, mode); - _mav_put_int16_t_array(buf, 0, roll, 4); - _mav_put_int16_t_array(buf, 8, pitch, 4); - _mav_put_int16_t_array(buf, 16, yaw, 4); - _mav_put_uint16_t_array(buf, 24, thrust, 4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); -#else - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t packet; - packet.group = group; - packet.mode = mode; - mav_array_memcpy(packet.roll, roll, sizeof(int16_t)*4); - mav_array_memcpy(packet.pitch, pitch, sizeof(int16_t)*4); - mav_array_memcpy(packet.yaw, yaw, sizeof(int16_t)*4); - mav_array_memcpy(packet.thrust, thrust, sizeof(uint16_t)*4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); -#endif -} - -/** - * @brief Pack a set_quad_swarm_roll_pitch_yaw_thrust message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) - * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) - * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) - * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) - * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t group,uint8_t mode,const int16_t *roll,const int16_t *pitch,const int16_t *yaw,const uint16_t *thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN]; - _mav_put_uint8_t(buf, 32, group); - _mav_put_uint8_t(buf, 33, mode); - _mav_put_int16_t_array(buf, 0, roll, 4); - _mav_put_int16_t_array(buf, 8, pitch, 4); - _mav_put_int16_t_array(buf, 16, yaw, 4); - _mav_put_uint16_t_array(buf, 24, thrust, 4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); -#else - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t packet; - packet.group = group; - packet.mode = mode; - mav_array_memcpy(packet.roll, roll, sizeof(int16_t)*4); - mav_array_memcpy(packet.pitch, pitch, sizeof(int16_t)*4); - mav_array_memcpy(packet.yaw, yaw, sizeof(int16_t)*4); - mav_array_memcpy(packet.thrust, thrust, sizeof(uint16_t)*4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); -#endif -} - -/** - * @brief Encode a set_quad_swarm_roll_pitch_yaw_thrust struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_quad_swarm_roll_pitch_yaw_thrust C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t* set_quad_swarm_roll_pitch_yaw_thrust) -{ - return mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack(system_id, component_id, msg, set_quad_swarm_roll_pitch_yaw_thrust->group, set_quad_swarm_roll_pitch_yaw_thrust->mode, set_quad_swarm_roll_pitch_yaw_thrust->roll, set_quad_swarm_roll_pitch_yaw_thrust->pitch, set_quad_swarm_roll_pitch_yaw_thrust->yaw, set_quad_swarm_roll_pitch_yaw_thrust->thrust); -} - -/** - * @brief Encode a set_quad_swarm_roll_pitch_yaw_thrust struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_quad_swarm_roll_pitch_yaw_thrust C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t* set_quad_swarm_roll_pitch_yaw_thrust) -{ - return mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, chan, msg, set_quad_swarm_roll_pitch_yaw_thrust->group, set_quad_swarm_roll_pitch_yaw_thrust->mode, set_quad_swarm_roll_pitch_yaw_thrust->roll, set_quad_swarm_roll_pitch_yaw_thrust->pitch, set_quad_swarm_roll_pitch_yaw_thrust->yaw, set_quad_swarm_roll_pitch_yaw_thrust->thrust); -} - -/** - * @brief Send a set_quad_swarm_roll_pitch_yaw_thrust message - * @param chan MAVLink channel to send the message - * - * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) - * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) - * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) - * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) - * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_send(mavlink_channel_t chan, uint8_t group, uint8_t mode, const int16_t *roll, const int16_t *pitch, const int16_t *yaw, const uint16_t *thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN]; - _mav_put_uint8_t(buf, 32, group); - _mav_put_uint8_t(buf, 33, mode); - _mav_put_int16_t_array(buf, 0, roll, 4); - _mav_put_int16_t_array(buf, 8, pitch, 4); - _mav_put_int16_t_array(buf, 16, yaw, 4); - _mav_put_uint16_t_array(buf, 24, thrust, 4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); -#endif -#else - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t packet; - packet.group = group; - packet.mode = mode; - mav_array_memcpy(packet.roll, roll, sizeof(int16_t)*4); - mav_array_memcpy(packet.pitch, pitch, sizeof(int16_t)*4); - mav_array_memcpy(packet.yaw, yaw, sizeof(int16_t)*4); - mav_array_memcpy(packet.thrust, thrust, sizeof(uint16_t)*4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, (const char *)&packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, (const char *)&packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST UNPACKING - - -/** - * @brief Get field group from set_quad_swarm_roll_pitch_yaw_thrust message - * - * @return ID of the quadrotor group (0 - 255, up to 256 groups supported) - */ -static inline uint8_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_group(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 32); -} - -/** - * @brief Get field mode from set_quad_swarm_roll_pitch_yaw_thrust message - * - * @return ID of the flight mode (0 - 255, up to 256 modes supported) - */ -static inline uint8_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 33); -} - -/** - * @brief Get field roll from set_quad_swarm_roll_pitch_yaw_thrust message - * - * @return Desired roll angle in radians +-PI (+-INT16_MAX) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg, int16_t *roll) -{ - return _MAV_RETURN_int16_t_array(msg, roll, 4, 0); -} - -/** - * @brief Get field pitch from set_quad_swarm_roll_pitch_yaw_thrust message - * - * @return Desired pitch angle in radians +-PI (+-INT16_MAX) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg, int16_t *pitch) -{ - return _MAV_RETURN_int16_t_array(msg, pitch, 4, 8); -} - -/** - * @brief Get field yaw from set_quad_swarm_roll_pitch_yaw_thrust message - * - * @return Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg, int16_t *yaw) -{ - return _MAV_RETURN_int16_t_array(msg, yaw, 4, 16); -} - -/** - * @brief Get field thrust from set_quad_swarm_roll_pitch_yaw_thrust message - * - * @return Collective thrust, scaled to uint16 (0..UINT16_MAX) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg, uint16_t *thrust) -{ - return _MAV_RETURN_uint16_t_array(msg, thrust, 4, 24); -} - -/** - * @brief Decode a set_quad_swarm_roll_pitch_yaw_thrust message into a struct - * - * @param msg The message to decode - * @param set_quad_swarm_roll_pitch_yaw_thrust C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(const mavlink_message_t* msg, mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t* set_quad_swarm_roll_pitch_yaw_thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_roll(msg, set_quad_swarm_roll_pitch_yaw_thrust->roll); - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_pitch(msg, set_quad_swarm_roll_pitch_yaw_thrust->pitch); - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_yaw(msg, set_quad_swarm_roll_pitch_yaw_thrust->yaw); - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_thrust(msg, set_quad_swarm_roll_pitch_yaw_thrust->thrust); - set_quad_swarm_roll_pitch_yaw_thrust->group = mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_group(msg); - set_quad_swarm_roll_pitch_yaw_thrust->mode = mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_mode(msg); -#else - memcpy(set_quad_swarm_roll_pitch_yaw_thrust, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h deleted file mode 100644 index 5846ba4..0000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h +++ /dev/null @@ -1,287 +0,0 @@ -// MESSAGE SET_ROLL_PITCH_YAW_SPEED_THRUST PACKING - -#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST 57 - -typedef struct __mavlink_set_roll_pitch_yaw_speed_thrust_t -{ - float roll_speed; ///< Desired roll angular speed in rad/s - float pitch_speed; ///< Desired pitch angular speed in rad/s - float yaw_speed; ///< Desired yaw angular speed in rad/s - float thrust; ///< Collective thrust, normalized to 0 .. 1 - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_set_roll_pitch_yaw_speed_thrust_t; - -#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN 18 -#define MAVLINK_MSG_ID_57_LEN 18 - -#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_CRC 24 -#define MAVLINK_MSG_ID_57_CRC 24 - - - -#define MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST { \ - "SET_ROLL_PITCH_YAW_SPEED_THRUST", \ - 6, \ - { { "roll_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, roll_speed) }, \ - { "pitch_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, pitch_speed) }, \ - { "yaw_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, yaw_speed) }, \ - { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, thrust) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a set_roll_pitch_yaw_speed_thrust message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param roll_speed Desired roll angular speed in rad/s - * @param pitch_speed Desired pitch angular speed in rad/s - * @param yaw_speed Desired yaw angular speed in rad/s - * @param thrust Collective thrust, normalized to 0 .. 1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN]; - _mav_put_float(buf, 0, roll_speed); - _mav_put_float(buf, 4, pitch_speed); - _mav_put_float(buf, 8, yaw_speed); - _mav_put_float(buf, 12, thrust); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); -#else - mavlink_set_roll_pitch_yaw_speed_thrust_t packet; - packet.roll_speed = roll_speed; - packet.pitch_speed = pitch_speed; - packet.yaw_speed = yaw_speed; - packet.thrust = thrust; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); -#endif -} - -/** - * @brief Pack a set_roll_pitch_yaw_speed_thrust message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param roll_speed Desired roll angular speed in rad/s - * @param pitch_speed Desired pitch angular speed in rad/s - * @param yaw_speed Desired yaw angular speed in rad/s - * @param thrust Collective thrust, normalized to 0 .. 1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_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,float roll_speed,float pitch_speed,float yaw_speed,float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN]; - _mav_put_float(buf, 0, roll_speed); - _mav_put_float(buf, 4, pitch_speed); - _mav_put_float(buf, 8, yaw_speed); - _mav_put_float(buf, 12, thrust); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); -#else - mavlink_set_roll_pitch_yaw_speed_thrust_t packet; - packet.roll_speed = roll_speed; - packet.pitch_speed = pitch_speed; - packet.yaw_speed = yaw_speed; - packet.thrust = thrust; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); -#endif -} - -/** - * @brief Encode a set_roll_pitch_yaw_speed_thrust struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_roll_pitch_yaw_speed_thrust C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust) -{ - return mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(system_id, component_id, msg, set_roll_pitch_yaw_speed_thrust->target_system, set_roll_pitch_yaw_speed_thrust->target_component, set_roll_pitch_yaw_speed_thrust->roll_speed, set_roll_pitch_yaw_speed_thrust->pitch_speed, set_roll_pitch_yaw_speed_thrust->yaw_speed, set_roll_pitch_yaw_speed_thrust->thrust); -} - -/** - * @brief Encode a set_roll_pitch_yaw_speed_thrust struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_roll_pitch_yaw_speed_thrust C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust) -{ - return mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(system_id, component_id, chan, msg, set_roll_pitch_yaw_speed_thrust->target_system, set_roll_pitch_yaw_speed_thrust->target_component, set_roll_pitch_yaw_speed_thrust->roll_speed, set_roll_pitch_yaw_speed_thrust->pitch_speed, set_roll_pitch_yaw_speed_thrust->yaw_speed, set_roll_pitch_yaw_speed_thrust->thrust); -} - -/** - * @brief Send a set_roll_pitch_yaw_speed_thrust message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param roll_speed Desired roll angular speed in rad/s - * @param pitch_speed Desired pitch angular speed in rad/s - * @param yaw_speed Desired yaw angular speed in rad/s - * @param thrust Collective thrust, normalized to 0 .. 1 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN]; - _mav_put_float(buf, 0, roll_speed); - _mav_put_float(buf, 4, pitch_speed); - _mav_put_float(buf, 8, yaw_speed); - _mav_put_float(buf, 12, thrust); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); -#endif -#else - mavlink_set_roll_pitch_yaw_speed_thrust_t packet; - packet.roll_speed = roll_speed; - packet.pitch_speed = pitch_speed; - packet.yaw_speed = yaw_speed; - packet.thrust = thrust; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, (const char *)&packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, (const char *)&packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SET_ROLL_PITCH_YAW_SPEED_THRUST UNPACKING - - -/** - * @brief Get field target_system from set_roll_pitch_yaw_speed_thrust message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Get field target_component from set_roll_pitch_yaw_speed_thrust message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 17); -} - -/** - * @brief Get field roll_speed from set_roll_pitch_yaw_speed_thrust message - * - * @return Desired roll angular speed in rad/s - */ -static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field pitch_speed from set_roll_pitch_yaw_speed_thrust message - * - * @return Desired pitch angular speed in rad/s - */ -static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field yaw_speed from set_roll_pitch_yaw_speed_thrust message - * - * @return Desired yaw angular speed in rad/s - */ -static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field thrust from set_roll_pitch_yaw_speed_thrust message - * - * @return Collective thrust, normalized to 0 .. 1 - */ -static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_thrust(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a set_roll_pitch_yaw_speed_thrust message into a struct - * - * @param msg The message to decode - * @param set_roll_pitch_yaw_speed_thrust C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_roll_pitch_yaw_speed_thrust->roll_speed = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(msg); - set_roll_pitch_yaw_speed_thrust->pitch_speed = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed(msg); - set_roll_pitch_yaw_speed_thrust->yaw_speed = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(msg); - set_roll_pitch_yaw_speed_thrust->thrust = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_thrust(msg); - set_roll_pitch_yaw_speed_thrust->target_system = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_system(msg); - set_roll_pitch_yaw_speed_thrust->target_component = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_component(msg); -#else - memcpy(set_roll_pitch_yaw_speed_thrust, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h deleted file mode 100644 index 334fd39..0000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h +++ /dev/null @@ -1,287 +0,0 @@ -// MESSAGE SET_ROLL_PITCH_YAW_THRUST PACKING - -#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST 56 - -typedef struct __mavlink_set_roll_pitch_yaw_thrust_t -{ - float roll; ///< Desired roll angle in radians - float pitch; ///< Desired pitch angle in radians - float yaw; ///< Desired yaw angle in radians - float thrust; ///< Collective thrust, normalized to 0 .. 1 - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_set_roll_pitch_yaw_thrust_t; - -#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN 18 -#define MAVLINK_MSG_ID_56_LEN 18 - -#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_CRC 100 -#define MAVLINK_MSG_ID_56_CRC 100 - - - -#define MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST { \ - "SET_ROLL_PITCH_YAW_THRUST", \ - 6, \ - { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, yaw) }, \ - { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, thrust) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a set_roll_pitch_yaw_thrust message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param roll Desired roll angle in radians - * @param pitch Desired pitch angle in radians - * @param yaw Desired yaw angle in radians - * @param thrust Collective thrust, normalized to 0 .. 1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, thrust); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); -#else - mavlink_set_roll_pitch_yaw_thrust_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.thrust = thrust; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); -#endif -} - -/** - * @brief Pack a set_roll_pitch_yaw_thrust message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param roll Desired roll angle in radians - * @param pitch Desired pitch angle in radians - * @param yaw Desired yaw angle in radians - * @param thrust Collective thrust, normalized to 0 .. 1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_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,float roll,float pitch,float yaw,float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, thrust); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); -#else - mavlink_set_roll_pitch_yaw_thrust_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.thrust = thrust; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); -#endif -} - -/** - * @brief Encode a set_roll_pitch_yaw_thrust struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_roll_pitch_yaw_thrust C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust) -{ - return mavlink_msg_set_roll_pitch_yaw_thrust_pack(system_id, component_id, msg, set_roll_pitch_yaw_thrust->target_system, set_roll_pitch_yaw_thrust->target_component, set_roll_pitch_yaw_thrust->roll, set_roll_pitch_yaw_thrust->pitch, set_roll_pitch_yaw_thrust->yaw, set_roll_pitch_yaw_thrust->thrust); -} - -/** - * @brief Encode a set_roll_pitch_yaw_thrust struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_roll_pitch_yaw_thrust C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust) -{ - return mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, chan, msg, set_roll_pitch_yaw_thrust->target_system, set_roll_pitch_yaw_thrust->target_component, set_roll_pitch_yaw_thrust->roll, set_roll_pitch_yaw_thrust->pitch, set_roll_pitch_yaw_thrust->yaw, set_roll_pitch_yaw_thrust->thrust); -} - -/** - * @brief Send a set_roll_pitch_yaw_thrust message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param roll Desired roll angle in radians - * @param pitch Desired pitch angle in radians - * @param yaw Desired yaw angle in radians - * @param thrust Collective thrust, normalized to 0 .. 1 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_roll_pitch_yaw_thrust_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, thrust); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); -#endif -#else - mavlink_set_roll_pitch_yaw_thrust_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.thrust = thrust; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, (const char *)&packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, (const char *)&packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SET_ROLL_PITCH_YAW_THRUST UNPACKING - - -/** - * @brief Get field target_system from set_roll_pitch_yaw_thrust message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Get field target_component from set_roll_pitch_yaw_thrust message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 17); -} - -/** - * @brief Get field roll from set_roll_pitch_yaw_thrust message - * - * @return Desired roll angle in radians - */ -static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field pitch from set_roll_pitch_yaw_thrust message - * - * @return Desired pitch angle in radians - */ -static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field yaw from set_roll_pitch_yaw_thrust message - * - * @return Desired yaw angle in radians - */ -static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field thrust from set_roll_pitch_yaw_thrust message - * - * @return Collective thrust, normalized to 0 .. 1 - */ -static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a set_roll_pitch_yaw_thrust message into a struct - * - * @param msg The message to decode - * @param set_roll_pitch_yaw_thrust C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_roll_pitch_yaw_thrust_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_roll_pitch_yaw_thrust->roll = mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(msg); - set_roll_pitch_yaw_thrust->pitch = mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(msg); - set_roll_pitch_yaw_thrust->yaw = mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(msg); - set_roll_pitch_yaw_thrust->thrust = mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(msg); - set_roll_pitch_yaw_thrust->target_system = mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(msg); - set_roll_pitch_yaw_thrust->target_component = mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component(msg); -#else - memcpy(set_roll_pitch_yaw_thrust, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h deleted file mode 100644 index 93ce345..0000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h +++ /dev/null @@ -1,309 +0,0 @@ -// MESSAGE SETPOINT_6DOF PACKING - -#define MAVLINK_MSG_ID_SETPOINT_6DOF 149 - -typedef struct __mavlink_setpoint_6dof_t -{ - float trans_x; ///< Translational Component in x - float trans_y; ///< Translational Component in y - float trans_z; ///< Translational Component in z - float rot_x; ///< Rotational Component in x - float rot_y; ///< Rotational Component in y - float rot_z; ///< Rotational Component in z - uint8_t target_system; ///< System ID -} mavlink_setpoint_6dof_t; - -#define MAVLINK_MSG_ID_SETPOINT_6DOF_LEN 25 -#define MAVLINK_MSG_ID_149_LEN 25 - -#define MAVLINK_MSG_ID_SETPOINT_6DOF_CRC 15 -#define MAVLINK_MSG_ID_149_CRC 15 - - - -#define MAVLINK_MESSAGE_INFO_SETPOINT_6DOF { \ - "SETPOINT_6DOF", \ - 7, \ - { { "trans_x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_setpoint_6dof_t, trans_x) }, \ - { "trans_y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_setpoint_6dof_t, trans_y) }, \ - { "trans_z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_setpoint_6dof_t, trans_z) }, \ - { "rot_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_setpoint_6dof_t, rot_x) }, \ - { "rot_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_setpoint_6dof_t, rot_y) }, \ - { "rot_z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_setpoint_6dof_t, rot_z) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_setpoint_6dof_t, target_system) }, \ - } \ -} - - -/** - * @brief Pack a setpoint_6dof message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param trans_x Translational Component in x - * @param trans_y Translational Component in y - * @param trans_z Translational Component in z - * @param rot_x Rotational Component in x - * @param rot_y Rotational Component in y - * @param rot_z Rotational Component in z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_setpoint_6dof_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, float trans_x, float trans_y, float trans_z, float rot_x, float rot_y, float rot_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SETPOINT_6DOF_LEN]; - _mav_put_float(buf, 0, trans_x); - _mav_put_float(buf, 4, trans_y); - _mav_put_float(buf, 8, trans_z); - _mav_put_float(buf, 12, rot_x); - _mav_put_float(buf, 16, rot_y); - _mav_put_float(buf, 20, rot_z); - _mav_put_uint8_t(buf, 24, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); -#else - mavlink_setpoint_6dof_t packet; - packet.trans_x = trans_x; - packet.trans_y = trans_y; - packet.trans_z = trans_z; - packet.rot_x = rot_x; - packet.rot_y = rot_y; - packet.rot_z = rot_z; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SETPOINT_6DOF; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN, MAVLINK_MSG_ID_SETPOINT_6DOF_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); -#endif -} - -/** - * @brief Pack a setpoint_6dof message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param trans_x Translational Component in x - * @param trans_y Translational Component in y - * @param trans_z Translational Component in z - * @param rot_x Rotational Component in x - * @param rot_y Rotational Component in y - * @param rot_z Rotational Component in z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_setpoint_6dof_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,float trans_x,float trans_y,float trans_z,float rot_x,float rot_y,float rot_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SETPOINT_6DOF_LEN]; - _mav_put_float(buf, 0, trans_x); - _mav_put_float(buf, 4, trans_y); - _mav_put_float(buf, 8, trans_z); - _mav_put_float(buf, 12, rot_x); - _mav_put_float(buf, 16, rot_y); - _mav_put_float(buf, 20, rot_z); - _mav_put_uint8_t(buf, 24, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); -#else - mavlink_setpoint_6dof_t packet; - packet.trans_x = trans_x; - packet.trans_y = trans_y; - packet.trans_z = trans_z; - packet.rot_x = rot_x; - packet.rot_y = rot_y; - packet.rot_z = rot_z; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SETPOINT_6DOF; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN, MAVLINK_MSG_ID_SETPOINT_6DOF_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); -#endif -} - -/** - * @brief Encode a setpoint_6dof struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param setpoint_6dof C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_setpoint_6dof_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_setpoint_6dof_t* setpoint_6dof) -{ - return mavlink_msg_setpoint_6dof_pack(system_id, component_id, msg, setpoint_6dof->target_system, setpoint_6dof->trans_x, setpoint_6dof->trans_y, setpoint_6dof->trans_z, setpoint_6dof->rot_x, setpoint_6dof->rot_y, setpoint_6dof->rot_z); -} - -/** - * @brief Encode a setpoint_6dof struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param setpoint_6dof C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_setpoint_6dof_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_setpoint_6dof_t* setpoint_6dof) -{ - return mavlink_msg_setpoint_6dof_pack_chan(system_id, component_id, chan, msg, setpoint_6dof->target_system, setpoint_6dof->trans_x, setpoint_6dof->trans_y, setpoint_6dof->trans_z, setpoint_6dof->rot_x, setpoint_6dof->rot_y, setpoint_6dof->rot_z); -} - -/** - * @brief Send a setpoint_6dof message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param trans_x Translational Component in x - * @param trans_y Translational Component in y - * @param trans_z Translational Component in z - * @param rot_x Rotational Component in x - * @param rot_y Rotational Component in y - * @param rot_z Rotational Component in z - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_setpoint_6dof_send(mavlink_channel_t chan, uint8_t target_system, float trans_x, float trans_y, float trans_z, float rot_x, float rot_y, float rot_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SETPOINT_6DOF_LEN]; - _mav_put_float(buf, 0, trans_x); - _mav_put_float(buf, 4, trans_y); - _mav_put_float(buf, 8, trans_z); - _mav_put_float(buf, 12, rot_x); - _mav_put_float(buf, 16, rot_y); - _mav_put_float(buf, 20, rot_z); - _mav_put_uint8_t(buf, 24, target_system); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, buf, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN, MAVLINK_MSG_ID_SETPOINT_6DOF_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, buf, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); -#endif -#else - mavlink_setpoint_6dof_t packet; - packet.trans_x = trans_x; - packet.trans_y = trans_y; - packet.trans_z = trans_z; - packet.rot_x = rot_x; - packet.rot_y = rot_y; - packet.rot_z = rot_z; - packet.target_system = target_system; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, (const char *)&packet, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN, MAVLINK_MSG_ID_SETPOINT_6DOF_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, (const char *)&packet, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SETPOINT_6DOF UNPACKING - - -/** - * @brief Get field target_system from setpoint_6dof message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_setpoint_6dof_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 24); -} - -/** - * @brief Get field trans_x from setpoint_6dof message - * - * @return Translational Component in x - */ -static inline float mavlink_msg_setpoint_6dof_get_trans_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field trans_y from setpoint_6dof message - * - * @return Translational Component in y - */ -static inline float mavlink_msg_setpoint_6dof_get_trans_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field trans_z from setpoint_6dof message - * - * @return Translational Component in z - */ -static inline float mavlink_msg_setpoint_6dof_get_trans_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field rot_x from setpoint_6dof message - * - * @return Rotational Component in x - */ -static inline float mavlink_msg_setpoint_6dof_get_rot_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field rot_y from setpoint_6dof message - * - * @return Rotational Component in y - */ -static inline float mavlink_msg_setpoint_6dof_get_rot_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field rot_z from setpoint_6dof message - * - * @return Rotational Component in z - */ -static inline float mavlink_msg_setpoint_6dof_get_rot_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Decode a setpoint_6dof message into a struct - * - * @param msg The message to decode - * @param setpoint_6dof C-struct to decode the message contents into - */ -static inline void mavlink_msg_setpoint_6dof_decode(const mavlink_message_t* msg, mavlink_setpoint_6dof_t* setpoint_6dof) -{ -#if MAVLINK_NEED_BYTE_SWAP - setpoint_6dof->trans_x = mavlink_msg_setpoint_6dof_get_trans_x(msg); - setpoint_6dof->trans_y = mavlink_msg_setpoint_6dof_get_trans_y(msg); - setpoint_6dof->trans_z = mavlink_msg_setpoint_6dof_get_trans_z(msg); - setpoint_6dof->rot_x = mavlink_msg_setpoint_6dof_get_rot_x(msg); - setpoint_6dof->rot_y = mavlink_msg_setpoint_6dof_get_rot_y(msg); - setpoint_6dof->rot_z = mavlink_msg_setpoint_6dof_get_rot_z(msg); - setpoint_6dof->target_system = mavlink_msg_setpoint_6dof_get_target_system(msg); -#else - memcpy(setpoint_6dof, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h deleted file mode 100644 index de80e46..0000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE SETPOINT_8DOF PACKING - -#define MAVLINK_MSG_ID_SETPOINT_8DOF 148 - -typedef struct __mavlink_setpoint_8dof_t -{ - float val1; ///< Value 1 - float val2; ///< Value 2 - float val3; ///< Value 3 - float val4; ///< Value 4 - float val5; ///< Value 5 - float val6; ///< Value 6 - float val7; ///< Value 7 - float val8; ///< Value 8 - uint8_t target_system; ///< System ID -} mavlink_setpoint_8dof_t; - -#define MAVLINK_MSG_ID_SETPOINT_8DOF_LEN 33 -#define MAVLINK_MSG_ID_148_LEN 33 - -#define MAVLINK_MSG_ID_SETPOINT_8DOF_CRC 241 -#define MAVLINK_MSG_ID_148_CRC 241 - - - -#define MAVLINK_MESSAGE_INFO_SETPOINT_8DOF { \ - "SETPOINT_8DOF", \ - 9, \ - { { "val1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_setpoint_8dof_t, val1) }, \ - { "val2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_setpoint_8dof_t, val2) }, \ - { "val3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_setpoint_8dof_t, val3) }, \ - { "val4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_setpoint_8dof_t, val4) }, \ - { "val5", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_setpoint_8dof_t, val5) }, \ - { "val6", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_setpoint_8dof_t, val6) }, \ - { "val7", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_setpoint_8dof_t, val7) }, \ - { "val8", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_setpoint_8dof_t, val8) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_setpoint_8dof_t, target_system) }, \ - } \ -} - - -/** - * @brief Pack a setpoint_8dof message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param val1 Value 1 - * @param val2 Value 2 - * @param val3 Value 3 - * @param val4 Value 4 - * @param val5 Value 5 - * @param val6 Value 6 - * @param val7 Value 7 - * @param val8 Value 8 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_setpoint_8dof_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, float val1, float val2, float val3, float val4, float val5, float val6, float val7, float val8) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SETPOINT_8DOF_LEN]; - _mav_put_float(buf, 0, val1); - _mav_put_float(buf, 4, val2); - _mav_put_float(buf, 8, val3); - _mav_put_float(buf, 12, val4); - _mav_put_float(buf, 16, val5); - _mav_put_float(buf, 20, val6); - _mav_put_float(buf, 24, val7); - _mav_put_float(buf, 28, val8); - _mav_put_uint8_t(buf, 32, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); -#else - mavlink_setpoint_8dof_t packet; - packet.val1 = val1; - packet.val2 = val2; - packet.val3 = val3; - packet.val4 = val4; - packet.val5 = val5; - packet.val6 = val6; - packet.val7 = val7; - packet.val8 = val8; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SETPOINT_8DOF; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN, MAVLINK_MSG_ID_SETPOINT_8DOF_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); -#endif -} - -/** - * @brief Pack a setpoint_8dof message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param val1 Value 1 - * @param val2 Value 2 - * @param val3 Value 3 - * @param val4 Value 4 - * @param val5 Value 5 - * @param val6 Value 6 - * @param val7 Value 7 - * @param val8 Value 8 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_setpoint_8dof_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,float val1,float val2,float val3,float val4,float val5,float val6,float val7,float val8) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SETPOINT_8DOF_LEN]; - _mav_put_float(buf, 0, val1); - _mav_put_float(buf, 4, val2); - _mav_put_float(buf, 8, val3); - _mav_put_float(buf, 12, val4); - _mav_put_float(buf, 16, val5); - _mav_put_float(buf, 20, val6); - _mav_put_float(buf, 24, val7); - _mav_put_float(buf, 28, val8); - _mav_put_uint8_t(buf, 32, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); -#else - mavlink_setpoint_8dof_t packet; - packet.val1 = val1; - packet.val2 = val2; - packet.val3 = val3; - packet.val4 = val4; - packet.val5 = val5; - packet.val6 = val6; - packet.val7 = val7; - packet.val8 = val8; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SETPOINT_8DOF; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN, MAVLINK_MSG_ID_SETPOINT_8DOF_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); -#endif -} - -/** - * @brief Encode a setpoint_8dof struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param setpoint_8dof C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_setpoint_8dof_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_setpoint_8dof_t* setpoint_8dof) -{ - return mavlink_msg_setpoint_8dof_pack(system_id, component_id, msg, setpoint_8dof->target_system, setpoint_8dof->val1, setpoint_8dof->val2, setpoint_8dof->val3, setpoint_8dof->val4, setpoint_8dof->val5, setpoint_8dof->val6, setpoint_8dof->val7, setpoint_8dof->val8); -} - -/** - * @brief Encode a setpoint_8dof struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param setpoint_8dof C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_setpoint_8dof_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_setpoint_8dof_t* setpoint_8dof) -{ - return mavlink_msg_setpoint_8dof_pack_chan(system_id, component_id, chan, msg, setpoint_8dof->target_system, setpoint_8dof->val1, setpoint_8dof->val2, setpoint_8dof->val3, setpoint_8dof->val4, setpoint_8dof->val5, setpoint_8dof->val6, setpoint_8dof->val7, setpoint_8dof->val8); -} - -/** - * @brief Send a setpoint_8dof message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param val1 Value 1 - * @param val2 Value 2 - * @param val3 Value 3 - * @param val4 Value 4 - * @param val5 Value 5 - * @param val6 Value 6 - * @param val7 Value 7 - * @param val8 Value 8 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_setpoint_8dof_send(mavlink_channel_t chan, uint8_t target_system, float val1, float val2, float val3, float val4, float val5, float val6, float val7, float val8) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SETPOINT_8DOF_LEN]; - _mav_put_float(buf, 0, val1); - _mav_put_float(buf, 4, val2); - _mav_put_float(buf, 8, val3); - _mav_put_float(buf, 12, val4); - _mav_put_float(buf, 16, val5); - _mav_put_float(buf, 20, val6); - _mav_put_float(buf, 24, val7); - _mav_put_float(buf, 28, val8); - _mav_put_uint8_t(buf, 32, target_system); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, buf, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN, MAVLINK_MSG_ID_SETPOINT_8DOF_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, buf, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); -#endif -#else - mavlink_setpoint_8dof_t packet; - packet.val1 = val1; - packet.val2 = val2; - packet.val3 = val3; - packet.val4 = val4; - packet.val5 = val5; - packet.val6 = val6; - packet.val7 = val7; - packet.val8 = val8; - packet.target_system = target_system; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, (const char *)&packet, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN, MAVLINK_MSG_ID_SETPOINT_8DOF_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, (const char *)&packet, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SETPOINT_8DOF UNPACKING - - -/** - * @brief Get field target_system from setpoint_8dof message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_setpoint_8dof_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 32); -} - -/** - * @brief Get field val1 from setpoint_8dof message - * - * @return Value 1 - */ -static inline float mavlink_msg_setpoint_8dof_get_val1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field val2 from setpoint_8dof message - * - * @return Value 2 - */ -static inline float mavlink_msg_setpoint_8dof_get_val2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field val3 from setpoint_8dof message - * - * @return Value 3 - */ -static inline float mavlink_msg_setpoint_8dof_get_val3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field val4 from setpoint_8dof message - * - * @return Value 4 - */ -static inline float mavlink_msg_setpoint_8dof_get_val4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field val5 from setpoint_8dof message - * - * @return Value 5 - */ -static inline float mavlink_msg_setpoint_8dof_get_val5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field val6 from setpoint_8dof message - * - * @return Value 6 - */ -static inline float mavlink_msg_setpoint_8dof_get_val6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field val7 from setpoint_8dof message - * - * @return Value 7 - */ -static inline float mavlink_msg_setpoint_8dof_get_val7(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field val8 from setpoint_8dof message - * - * @return Value 8 - */ -static inline float mavlink_msg_setpoint_8dof_get_val8(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Decode a setpoint_8dof message into a struct - * - * @param msg The message to decode - * @param setpoint_8dof C-struct to decode the message contents into - */ -static inline void mavlink_msg_setpoint_8dof_decode(const mavlink_message_t* msg, mavlink_setpoint_8dof_t* setpoint_8dof) -{ -#if MAVLINK_NEED_BYTE_SWAP - setpoint_8dof->val1 = mavlink_msg_setpoint_8dof_get_val1(msg); - setpoint_8dof->val2 = mavlink_msg_setpoint_8dof_get_val2(msg); - setpoint_8dof->val3 = mavlink_msg_setpoint_8dof_get_val3(msg); - setpoint_8dof->val4 = mavlink_msg_setpoint_8dof_get_val4(msg); - setpoint_8dof->val5 = mavlink_msg_setpoint_8dof_get_val5(msg); - setpoint_8dof->val6 = mavlink_msg_setpoint_8dof_get_val6(msg); - setpoint_8dof->val7 = mavlink_msg_setpoint_8dof_get_val7(msg); - setpoint_8dof->val8 = mavlink_msg_setpoint_8dof_get_val8(msg); - setpoint_8dof->target_system = mavlink_msg_setpoint_8dof_get_target_system(msg); -#else - memcpy(setpoint_8dof, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h index ebd657c..c47fc8c 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h @@ -4,10 +4,10 @@ typedef struct __mavlink_sim_state_t { - float q1; ///< True attitude quaternion component 1 - float q2; ///< True attitude quaternion component 2 - float q3; ///< True attitude quaternion component 3 - float q4; ///< True attitude quaternion component 4 + float q1; ///< True attitude quaternion component 1, w (1 in null-rotation) + float q2; ///< True attitude quaternion component 2, x (0 in null-rotation) + float q3; ///< True attitude quaternion component 3, y (0 in null-rotation) + float q4; ///< True attitude quaternion component 4, z (0 in null-rotation) float roll; ///< Attitude roll expressed as Euler angles, not recommended except for human-readable outputs float pitch; ///< Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs float yaw; ///< Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs @@ -69,10 +69,10 @@ typedef struct __mavlink_sim_state_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param q1 True attitude quaternion component 1 - * @param q2 True attitude quaternion component 2 - * @param q3 True attitude quaternion component 3 - * @param q4 True attitude quaternion component 4 + * @param q1 True attitude quaternion component 1, w (1 in null-rotation) + * @param q2 True attitude quaternion component 2, x (0 in null-rotation) + * @param q3 True attitude quaternion component 3, y (0 in null-rotation) + * @param q4 True attitude quaternion component 4, z (0 in null-rotation) * @param roll Attitude roll expressed as Euler angles, not recommended except for human-readable outputs * @param pitch Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs * @param yaw Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs @@ -161,10 +161,10 @@ static inline uint16_t mavlink_msg_sim_state_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 q1 True attitude quaternion component 1 - * @param q2 True attitude quaternion component 2 - * @param q3 True attitude quaternion component 3 - * @param q4 True attitude quaternion component 4 + * @param q1 True attitude quaternion component 1, w (1 in null-rotation) + * @param q2 True attitude quaternion component 2, x (0 in null-rotation) + * @param q3 True attitude quaternion component 3, y (0 in null-rotation) + * @param q4 True attitude quaternion component 4, z (0 in null-rotation) * @param roll Attitude roll expressed as Euler angles, not recommended except for human-readable outputs * @param pitch Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs * @param yaw Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs @@ -279,10 +279,10 @@ static inline uint16_t mavlink_msg_sim_state_encode_chan(uint8_t system_id, uint * @brief Send a sim_state message * @param chan MAVLink channel to send the message * - * @param q1 True attitude quaternion component 1 - * @param q2 True attitude quaternion component 2 - * @param q3 True attitude quaternion component 3 - * @param q4 True attitude quaternion component 4 + * @param q1 True attitude quaternion component 1, w (1 in null-rotation) + * @param q2 True attitude quaternion component 2, x (0 in null-rotation) + * @param q3 True attitude quaternion component 3, y (0 in null-rotation) + * @param q4 True attitude quaternion component 4, z (0 in null-rotation) * @param roll Attitude roll expressed as Euler angles, not recommended except for human-readable outputs * @param pitch Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs * @param yaw Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs @@ -366,6 +366,78 @@ static inline void mavlink_msg_sim_state_send(mavlink_channel_t chan, float q1, #endif } +#if MAVLINK_MSG_ID_SIM_STATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_sim_state_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float q1, float q2, float q3, float q4, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lon, float alt, float std_dev_horz, float std_dev_vert, float vn, float ve, float vd) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, q1); + _mav_put_float(buf, 4, q2); + _mav_put_float(buf, 8, q3); + _mav_put_float(buf, 12, q4); + _mav_put_float(buf, 16, roll); + _mav_put_float(buf, 20, pitch); + _mav_put_float(buf, 24, yaw); + _mav_put_float(buf, 28, xacc); + _mav_put_float(buf, 32, yacc); + _mav_put_float(buf, 36, zacc); + _mav_put_float(buf, 40, xgyro); + _mav_put_float(buf, 44, ygyro); + _mav_put_float(buf, 48, zgyro); + _mav_put_float(buf, 52, lat); + _mav_put_float(buf, 56, lon); + _mav_put_float(buf, 60, alt); + _mav_put_float(buf, 64, std_dev_horz); + _mav_put_float(buf, 68, std_dev_vert); + _mav_put_float(buf, 72, vn); + _mav_put_float(buf, 76, ve); + _mav_put_float(buf, 80, vd); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, buf, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, buf, MAVLINK_MSG_ID_SIM_STATE_LEN); +#endif +#else + mavlink_sim_state_t *packet = (mavlink_sim_state_t *)msgbuf; + packet->q1 = q1; + packet->q2 = q2; + packet->q3 = q3; + packet->q4 = q4; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->std_dev_horz = std_dev_horz; + packet->std_dev_vert = std_dev_vert; + packet->vn = vn; + packet->ve = ve; + packet->vd = vd; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)packet, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)packet, MAVLINK_MSG_ID_SIM_STATE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SIM_STATE UNPACKING @@ -374,7 +446,7 @@ static inline void mavlink_msg_sim_state_send(mavlink_channel_t chan, float q1, /** * @brief Get field q1 from sim_state message * - * @return True attitude quaternion component 1 + * @return True attitude quaternion component 1, w (1 in null-rotation) */ static inline float mavlink_msg_sim_state_get_q1(const mavlink_message_t* msg) { @@ -384,7 +456,7 @@ static inline float mavlink_msg_sim_state_get_q1(const mavlink_message_t* msg) /** * @brief Get field q2 from sim_state message * - * @return True attitude quaternion component 2 + * @return True attitude quaternion component 2, x (0 in null-rotation) */ static inline float mavlink_msg_sim_state_get_q2(const mavlink_message_t* msg) { @@ -394,7 +466,7 @@ static inline float mavlink_msg_sim_state_get_q2(const mavlink_message_t* msg) /** * @brief Get field q3 from sim_state message * - * @return True attitude quaternion component 3 + * @return True attitude quaternion component 3, y (0 in null-rotation) */ static inline float mavlink_msg_sim_state_get_q3(const mavlink_message_t* msg) { @@ -404,7 +476,7 @@ static inline float mavlink_msg_sim_state_get_q3(const mavlink_message_t* msg) /** * @brief Get field q4 from sim_state message * - * @return True attitude quaternion component 4 + * @return True attitude quaternion component 4, z (0 in null-rotation) */ static inline float mavlink_msg_sim_state_get_q4(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h deleted file mode 100644 index 2db627b..0000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE STATE_CORRECTION PACKING - -#define MAVLINK_MSG_ID_STATE_CORRECTION 64 - -typedef struct __mavlink_state_correction_t -{ - float xErr; ///< x position error - float yErr; ///< y position error - float zErr; ///< z position error - float rollErr; ///< roll error (radians) - float pitchErr; ///< pitch error (radians) - float yawErr; ///< yaw error (radians) - float vxErr; ///< x velocity - float vyErr; ///< y velocity - float vzErr; ///< z velocity -} mavlink_state_correction_t; - -#define MAVLINK_MSG_ID_STATE_CORRECTION_LEN 36 -#define MAVLINK_MSG_ID_64_LEN 36 - -#define MAVLINK_MSG_ID_STATE_CORRECTION_CRC 130 -#define MAVLINK_MSG_ID_64_CRC 130 - - - -#define MAVLINK_MESSAGE_INFO_STATE_CORRECTION { \ - "STATE_CORRECTION", \ - 9, \ - { { "xErr", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_state_correction_t, xErr) }, \ - { "yErr", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_state_correction_t, yErr) }, \ - { "zErr", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_state_correction_t, zErr) }, \ - { "rollErr", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_state_correction_t, rollErr) }, \ - { "pitchErr", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_state_correction_t, pitchErr) }, \ - { "yawErr", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_state_correction_t, yawErr) }, \ - { "vxErr", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_state_correction_t, vxErr) }, \ - { "vyErr", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_state_correction_t, vyErr) }, \ - { "vzErr", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_state_correction_t, vzErr) }, \ - } \ -} - - -/** - * @brief Pack a state_correction message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param xErr x position error - * @param yErr y position error - * @param zErr z position error - * @param rollErr roll error (radians) - * @param pitchErr pitch error (radians) - * @param yawErr yaw error (radians) - * @param vxErr x velocity - * @param vyErr y velocity - * @param vzErr z velocity - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_state_correction_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_STATE_CORRECTION_LEN]; - _mav_put_float(buf, 0, xErr); - _mav_put_float(buf, 4, yErr); - _mav_put_float(buf, 8, zErr); - _mav_put_float(buf, 12, rollErr); - _mav_put_float(buf, 16, pitchErr); - _mav_put_float(buf, 20, yawErr); - _mav_put_float(buf, 24, vxErr); - _mav_put_float(buf, 28, vyErr); - _mav_put_float(buf, 32, vzErr); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); -#else - mavlink_state_correction_t packet; - packet.xErr = xErr; - packet.yErr = yErr; - packet.zErr = zErr; - packet.rollErr = rollErr; - packet.pitchErr = pitchErr; - packet.yawErr = yawErr; - packet.vxErr = vxErr; - packet.vyErr = vyErr; - packet.vzErr = vzErr; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATE_CORRECTION_LEN, MAVLINK_MSG_ID_STATE_CORRECTION_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); -#endif -} - -/** - * @brief Pack a state_correction message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param xErr x position error - * @param yErr y position error - * @param zErr z position error - * @param rollErr roll error (radians) - * @param pitchErr pitch error (radians) - * @param yawErr yaw error (radians) - * @param vxErr x velocity - * @param vyErr y velocity - * @param vzErr z velocity - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_state_correction_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float xErr,float yErr,float zErr,float rollErr,float pitchErr,float yawErr,float vxErr,float vyErr,float vzErr) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_STATE_CORRECTION_LEN]; - _mav_put_float(buf, 0, xErr); - _mav_put_float(buf, 4, yErr); - _mav_put_float(buf, 8, zErr); - _mav_put_float(buf, 12, rollErr); - _mav_put_float(buf, 16, pitchErr); - _mav_put_float(buf, 20, yawErr); - _mav_put_float(buf, 24, vxErr); - _mav_put_float(buf, 28, vyErr); - _mav_put_float(buf, 32, vzErr); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); -#else - mavlink_state_correction_t packet; - packet.xErr = xErr; - packet.yErr = yErr; - packet.zErr = zErr; - packet.rollErr = rollErr; - packet.pitchErr = pitchErr; - packet.yawErr = yawErr; - packet.vxErr = vxErr; - packet.vyErr = vyErr; - packet.vzErr = vzErr; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATE_CORRECTION_LEN, MAVLINK_MSG_ID_STATE_CORRECTION_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); -#endif -} - -/** - * @brief Encode a state_correction struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param state_correction C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_state_correction_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_state_correction_t* state_correction) -{ - return mavlink_msg_state_correction_pack(system_id, component_id, msg, state_correction->xErr, state_correction->yErr, state_correction->zErr, state_correction->rollErr, state_correction->pitchErr, state_correction->yawErr, state_correction->vxErr, state_correction->vyErr, state_correction->vzErr); -} - -/** - * @brief Encode a state_correction struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param state_correction C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_state_correction_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_state_correction_t* state_correction) -{ - return mavlink_msg_state_correction_pack_chan(system_id, component_id, chan, msg, state_correction->xErr, state_correction->yErr, state_correction->zErr, state_correction->rollErr, state_correction->pitchErr, state_correction->yawErr, state_correction->vxErr, state_correction->vyErr, state_correction->vzErr); -} - -/** - * @brief Send a state_correction message - * @param chan MAVLink channel to send the message - * - * @param xErr x position error - * @param yErr y position error - * @param zErr z position error - * @param rollErr roll error (radians) - * @param pitchErr pitch error (radians) - * @param yawErr yaw error (radians) - * @param vxErr x velocity - * @param vyErr y velocity - * @param vzErr z velocity - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_STATE_CORRECTION_LEN]; - _mav_put_float(buf, 0, xErr); - _mav_put_float(buf, 4, yErr); - _mav_put_float(buf, 8, zErr); - _mav_put_float(buf, 12, rollErr); - _mav_put_float(buf, 16, pitchErr); - _mav_put_float(buf, 20, yawErr); - _mav_put_float(buf, 24, vxErr); - _mav_put_float(buf, 28, vyErr); - _mav_put_float(buf, 32, vzErr); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, buf, MAVLINK_MSG_ID_STATE_CORRECTION_LEN, MAVLINK_MSG_ID_STATE_CORRECTION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, buf, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); -#endif -#else - mavlink_state_correction_t packet; - packet.xErr = xErr; - packet.yErr = yErr; - packet.zErr = zErr; - packet.rollErr = rollErr; - packet.pitchErr = pitchErr; - packet.yawErr = yawErr; - packet.vxErr = vxErr; - packet.vyErr = vyErr; - packet.vzErr = vzErr; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, (const char *)&packet, MAVLINK_MSG_ID_STATE_CORRECTION_LEN, MAVLINK_MSG_ID_STATE_CORRECTION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, (const char *)&packet, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); -#endif -#endif -} - -#endif - -// MESSAGE STATE_CORRECTION UNPACKING - - -/** - * @brief Get field xErr from state_correction message - * - * @return x position error - */ -static inline float mavlink_msg_state_correction_get_xErr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field yErr from state_correction message - * - * @return y position error - */ -static inline float mavlink_msg_state_correction_get_yErr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field zErr from state_correction message - * - * @return z position error - */ -static inline float mavlink_msg_state_correction_get_zErr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field rollErr from state_correction message - * - * @return roll error (radians) - */ -static inline float mavlink_msg_state_correction_get_rollErr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field pitchErr from state_correction message - * - * @return pitch error (radians) - */ -static inline float mavlink_msg_state_correction_get_pitchErr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field yawErr from state_correction message - * - * @return yaw error (radians) - */ -static inline float mavlink_msg_state_correction_get_yawErr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field vxErr from state_correction message - * - * @return x velocity - */ -static inline float mavlink_msg_state_correction_get_vxErr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field vyErr from state_correction message - * - * @return y velocity - */ -static inline float mavlink_msg_state_correction_get_vyErr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Get field vzErr from state_correction message - * - * @return z velocity - */ -static inline float mavlink_msg_state_correction_get_vzErr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 32); -} - -/** - * @brief Decode a state_correction message into a struct - * - * @param msg The message to decode - * @param state_correction C-struct to decode the message contents into - */ -static inline void mavlink_msg_state_correction_decode(const mavlink_message_t* msg, mavlink_state_correction_t* state_correction) -{ -#if MAVLINK_NEED_BYTE_SWAP - state_correction->xErr = mavlink_msg_state_correction_get_xErr(msg); - state_correction->yErr = mavlink_msg_state_correction_get_yErr(msg); - state_correction->zErr = mavlink_msg_state_correction_get_zErr(msg); - state_correction->rollErr = mavlink_msg_state_correction_get_rollErr(msg); - state_correction->pitchErr = mavlink_msg_state_correction_get_pitchErr(msg); - state_correction->yawErr = mavlink_msg_state_correction_get_yawErr(msg); - state_correction->vxErr = mavlink_msg_state_correction_get_vxErr(msg); - state_correction->vyErr = mavlink_msg_state_correction_get_vyErr(msg); - state_correction->vzErr = mavlink_msg_state_correction_get_vzErr(msg); -#else - memcpy(state_correction, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_STATE_CORRECTION_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h index 536ca06..c8c2547 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h @@ -151,6 +151,38 @@ static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t s #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 + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_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); +#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 +#endif +} +#endif + #endif // MESSAGE STATUSTEXT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h index 101b366..1b1730d 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h @@ -278,6 +278,62 @@ static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint32_t #endif } +#if MAVLINK_MSG_ID_SYS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_sys_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, onboard_control_sensors_present); + _mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled); + _mav_put_uint32_t(buf, 8, onboard_control_sensors_health); + _mav_put_uint16_t(buf, 12, load); + _mav_put_uint16_t(buf, 14, voltage_battery); + _mav_put_int16_t(buf, 16, current_battery); + _mav_put_uint16_t(buf, 18, drop_rate_comm); + _mav_put_uint16_t(buf, 20, errors_comm); + _mav_put_uint16_t(buf, 22, errors_count1); + _mav_put_uint16_t(buf, 24, errors_count2); + _mav_put_uint16_t(buf, 26, errors_count3); + _mav_put_uint16_t(buf, 28, errors_count4); + _mav_put_int8_t(buf, 30, battery_remaining); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_LEN); +#endif +#else + mavlink_sys_status_t *packet = (mavlink_sys_status_t *)msgbuf; + packet->onboard_control_sensors_present = onboard_control_sensors_present; + packet->onboard_control_sensors_enabled = onboard_control_sensors_enabled; + packet->onboard_control_sensors_health = onboard_control_sensors_health; + packet->load = load; + packet->voltage_battery = voltage_battery; + packet->current_battery = current_battery; + packet->drop_rate_comm = drop_rate_comm; + packet->errors_comm = errors_comm; + packet->errors_count1 = errors_count1; + packet->errors_count2 = errors_count2; + packet->errors_count3 = errors_count3; + packet->errors_count4 = errors_count4; + packet->battery_remaining = battery_remaining; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)packet, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)packet, MAVLINK_MSG_ID_SYS_STATUS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SYS_STATUS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h index 1807567..988c669 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t #endif } +#if MAVLINK_MSG_ID_SYSTEM_TIME_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_system_time_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_unix_usec, uint32_t time_boot_ms) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_unix_usec); + _mav_put_uint32_t(buf, 8, time_boot_ms); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); +#endif +#else + mavlink_system_time_t *packet = (mavlink_system_time_t *)msgbuf; + packet->time_unix_usec = time_unix_usec; + packet->time_boot_ms = time_boot_ms; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, (const char *)packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, (const char *)packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SYSTEM_TIME UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_terrain_check.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_terrain_check.h new file mode 100644 index 0000000..ebdf71a --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_terrain_check.h @@ -0,0 +1,233 @@ +// MESSAGE TERRAIN_CHECK PACKING + +#define MAVLINK_MSG_ID_TERRAIN_CHECK 135 + +typedef struct __mavlink_terrain_check_t +{ + int32_t lat; ///< Latitude (degrees *10^7) + int32_t lon; ///< Longitude (degrees *10^7) +} mavlink_terrain_check_t; + +#define MAVLINK_MSG_ID_TERRAIN_CHECK_LEN 8 +#define MAVLINK_MSG_ID_135_LEN 8 + +#define MAVLINK_MSG_ID_TERRAIN_CHECK_CRC 203 +#define MAVLINK_MSG_ID_135_CRC 203 + + + +#define MAVLINK_MESSAGE_INFO_TERRAIN_CHECK { \ + "TERRAIN_CHECK", \ + 2, \ + { { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_terrain_check_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_terrain_check_t, lon) }, \ + } \ +} + + +/** + * @brief Pack a terrain_check message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param lat Latitude (degrees *10^7) + * @param lon Longitude (degrees *10^7) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_terrain_check_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + int32_t lat, int32_t lon) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_CHECK_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#else + mavlink_terrain_check_t packet; + packet.lat = lat; + packet.lon = lon; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TERRAIN_CHECK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#endif +} + +/** + * @brief Pack a terrain_check message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param lat Latitude (degrees *10^7) + * @param lon Longitude (degrees *10^7) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_terrain_check_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + int32_t lat,int32_t lon) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_CHECK_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#else + mavlink_terrain_check_t packet; + packet.lat = lat; + packet.lon = lon; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TERRAIN_CHECK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#endif +} + +/** + * @brief Encode a terrain_check struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param terrain_check C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_terrain_check_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_terrain_check_t* terrain_check) +{ + return mavlink_msg_terrain_check_pack(system_id, component_id, msg, terrain_check->lat, terrain_check->lon); +} + +/** + * @brief Encode a terrain_check struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param terrain_check C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_terrain_check_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_terrain_check_t* terrain_check) +{ + return mavlink_msg_terrain_check_pack_chan(system_id, component_id, chan, msg, terrain_check->lat, terrain_check->lon); +} + +/** + * @brief Send a terrain_check message + * @param chan MAVLink channel to send the message + * + * @param lat Latitude (degrees *10^7) + * @param lon Longitude (degrees *10^7) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_terrain_check_send(mavlink_channel_t chan, int32_t lat, int32_t lon) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_CHECK_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_CHECK, buf, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_CHECK, buf, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#endif +#else + mavlink_terrain_check_t packet; + packet.lat = lat; + packet.lon = lon; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_CHECK, (const char *)&packet, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_CHECK, (const char *)&packet, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_TERRAIN_CHECK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_terrain_check_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t lat, int32_t lon) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_CHECK, buf, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_CHECK, buf, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#endif +#else + mavlink_terrain_check_t *packet = (mavlink_terrain_check_t *)msgbuf; + packet->lat = lat; + packet->lon = lon; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_CHECK, (const char *)packet, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_CHECK, (const char *)packet, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE TERRAIN_CHECK UNPACKING + + +/** + * @brief Get field lat from terrain_check message + * + * @return Latitude (degrees *10^7) + */ +static inline int32_t mavlink_msg_terrain_check_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field lon from terrain_check message + * + * @return Longitude (degrees *10^7) + */ +static inline int32_t mavlink_msg_terrain_check_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Decode a terrain_check message into a struct + * + * @param msg The message to decode + * @param terrain_check C-struct to decode the message contents into + */ +static inline void mavlink_msg_terrain_check_decode(const mavlink_message_t* msg, mavlink_terrain_check_t* terrain_check) +{ +#if MAVLINK_NEED_BYTE_SWAP + terrain_check->lat = mavlink_msg_terrain_check_get_lat(msg); + terrain_check->lon = mavlink_msg_terrain_check_get_lon(msg); +#else + memcpy(terrain_check, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_terrain_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_terrain_data.h new file mode 100644 index 0000000..8fc0b43 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_terrain_data.h @@ -0,0 +1,297 @@ +// MESSAGE TERRAIN_DATA PACKING + +#define MAVLINK_MSG_ID_TERRAIN_DATA 134 + +typedef struct __mavlink_terrain_data_t +{ + int32_t lat; ///< Latitude of SW corner of first grid (degrees *10^7) + int32_t lon; ///< Longitude of SW corner of first grid (in degrees *10^7) + uint16_t grid_spacing; ///< Grid spacing in meters + int16_t data[16]; ///< Terrain data in meters AMSL + uint8_t gridbit; ///< bit within the terrain request mask +} mavlink_terrain_data_t; + +#define MAVLINK_MSG_ID_TERRAIN_DATA_LEN 43 +#define MAVLINK_MSG_ID_134_LEN 43 + +#define MAVLINK_MSG_ID_TERRAIN_DATA_CRC 229 +#define MAVLINK_MSG_ID_134_CRC 229 + +#define MAVLINK_MSG_TERRAIN_DATA_FIELD_DATA_LEN 16 + +#define MAVLINK_MESSAGE_INFO_TERRAIN_DATA { \ + "TERRAIN_DATA", \ + 5, \ + { { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_terrain_data_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_terrain_data_t, lon) }, \ + { "grid_spacing", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_terrain_data_t, grid_spacing) }, \ + { "data", NULL, MAVLINK_TYPE_INT16_T, 16, 10, offsetof(mavlink_terrain_data_t, data) }, \ + { "gridbit", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_terrain_data_t, gridbit) }, \ + } \ +} + + +/** + * @brief Pack a terrain_data message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param lat Latitude of SW corner of first grid (degrees *10^7) + * @param lon Longitude of SW corner of first grid (in degrees *10^7) + * @param grid_spacing Grid spacing in meters + * @param gridbit bit within the terrain request mask + * @param data Terrain data in meters AMSL + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_terrain_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + int32_t lat, int32_t lon, uint16_t grid_spacing, uint8_t gridbit, const int16_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_DATA_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_uint16_t(buf, 8, grid_spacing); + _mav_put_uint8_t(buf, 42, gridbit); + _mav_put_int16_t_array(buf, 10, data, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#else + mavlink_terrain_data_t packet; + packet.lat = lat; + packet.lon = lon; + packet.grid_spacing = grid_spacing; + packet.gridbit = gridbit; + mav_array_memcpy(packet.data, data, sizeof(int16_t)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TERRAIN_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#endif +} + +/** + * @brief Pack a terrain_data message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param lat Latitude of SW corner of first grid (degrees *10^7) + * @param lon Longitude of SW corner of first grid (in degrees *10^7) + * @param grid_spacing Grid spacing in meters + * @param gridbit bit within the terrain request mask + * @param data Terrain data in meters AMSL + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_terrain_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + int32_t lat,int32_t lon,uint16_t grid_spacing,uint8_t gridbit,const int16_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_DATA_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_uint16_t(buf, 8, grid_spacing); + _mav_put_uint8_t(buf, 42, gridbit); + _mav_put_int16_t_array(buf, 10, data, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#else + mavlink_terrain_data_t packet; + packet.lat = lat; + packet.lon = lon; + packet.grid_spacing = grid_spacing; + packet.gridbit = gridbit; + mav_array_memcpy(packet.data, data, sizeof(int16_t)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TERRAIN_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#endif +} + +/** + * @brief Encode a terrain_data struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param terrain_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_terrain_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_terrain_data_t* terrain_data) +{ + return mavlink_msg_terrain_data_pack(system_id, component_id, msg, terrain_data->lat, terrain_data->lon, terrain_data->grid_spacing, terrain_data->gridbit, terrain_data->data); +} + +/** + * @brief Encode a terrain_data struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param terrain_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_terrain_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_terrain_data_t* terrain_data) +{ + return mavlink_msg_terrain_data_pack_chan(system_id, component_id, chan, msg, terrain_data->lat, terrain_data->lon, terrain_data->grid_spacing, terrain_data->gridbit, terrain_data->data); +} + +/** + * @brief Send a terrain_data message + * @param chan MAVLink channel to send the message + * + * @param lat Latitude of SW corner of first grid (degrees *10^7) + * @param lon Longitude of SW corner of first grid (in degrees *10^7) + * @param grid_spacing Grid spacing in meters + * @param gridbit bit within the terrain request mask + * @param data Terrain data in meters AMSL + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_terrain_data_send(mavlink_channel_t chan, int32_t lat, int32_t lon, uint16_t grid_spacing, uint8_t gridbit, const int16_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_DATA_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_uint16_t(buf, 8, grid_spacing); + _mav_put_uint8_t(buf, 42, gridbit); + _mav_put_int16_t_array(buf, 10, data, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, buf, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, buf, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#endif +#else + mavlink_terrain_data_t packet; + packet.lat = lat; + packet.lon = lon; + packet.grid_spacing = grid_spacing; + packet.gridbit = gridbit; + mav_array_memcpy(packet.data, data, sizeof(int16_t)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, (const char *)&packet, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, (const char *)&packet, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_TERRAIN_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_terrain_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t lat, int32_t lon, uint16_t grid_spacing, uint8_t gridbit, const int16_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_uint16_t(buf, 8, grid_spacing); + _mav_put_uint8_t(buf, 42, gridbit); + _mav_put_int16_t_array(buf, 10, data, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, buf, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, buf, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#endif +#else + mavlink_terrain_data_t *packet = (mavlink_terrain_data_t *)msgbuf; + packet->lat = lat; + packet->lon = lon; + packet->grid_spacing = grid_spacing; + packet->gridbit = gridbit; + mav_array_memcpy(packet->data, data, sizeof(int16_t)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, (const char *)packet, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, (const char *)packet, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE TERRAIN_DATA UNPACKING + + +/** + * @brief Get field lat from terrain_data message + * + * @return Latitude of SW corner of first grid (degrees *10^7) + */ +static inline int32_t mavlink_msg_terrain_data_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field lon from terrain_data message + * + * @return Longitude of SW corner of first grid (in degrees *10^7) + */ +static inline int32_t mavlink_msg_terrain_data_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field grid_spacing from terrain_data message + * + * @return Grid spacing in meters + */ +static inline uint16_t mavlink_msg_terrain_data_get_grid_spacing(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field gridbit from terrain_data message + * + * @return bit within the terrain request mask + */ +static inline uint8_t mavlink_msg_terrain_data_get_gridbit(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 42); +} + +/** + * @brief Get field data from terrain_data message + * + * @return Terrain data in meters AMSL + */ +static inline uint16_t mavlink_msg_terrain_data_get_data(const mavlink_message_t* msg, int16_t *data) +{ + return _MAV_RETURN_int16_t_array(msg, data, 16, 10); +} + +/** + * @brief Decode a terrain_data message into a struct + * + * @param msg The message to decode + * @param terrain_data C-struct to decode the message contents into + */ +static inline void mavlink_msg_terrain_data_decode(const mavlink_message_t* msg, mavlink_terrain_data_t* terrain_data) +{ +#if MAVLINK_NEED_BYTE_SWAP + terrain_data->lat = mavlink_msg_terrain_data_get_lat(msg); + terrain_data->lon = mavlink_msg_terrain_data_get_lon(msg); + terrain_data->grid_spacing = mavlink_msg_terrain_data_get_grid_spacing(msg); + mavlink_msg_terrain_data_get_data(msg, terrain_data->data); + terrain_data->gridbit = mavlink_msg_terrain_data_get_gridbit(msg); +#else + memcpy(terrain_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_terrain_report.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_terrain_report.h new file mode 100644 index 0000000..1b61dd4 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_terrain_report.h @@ -0,0 +1,353 @@ +// MESSAGE TERRAIN_REPORT PACKING + +#define MAVLINK_MSG_ID_TERRAIN_REPORT 136 + +typedef struct __mavlink_terrain_report_t +{ + int32_t lat; ///< Latitude (degrees *10^7) + int32_t lon; ///< Longitude (degrees *10^7) + float terrain_height; ///< Terrain height in meters AMSL + float current_height; ///< Current vehicle height above lat/lon terrain height (meters) + uint16_t spacing; ///< grid spacing (zero if terrain at this location unavailable) + uint16_t pending; ///< Number of 4x4 terrain blocks waiting to be received or read from disk + uint16_t loaded; ///< Number of 4x4 terrain blocks in memory +} mavlink_terrain_report_t; + +#define MAVLINK_MSG_ID_TERRAIN_REPORT_LEN 22 +#define MAVLINK_MSG_ID_136_LEN 22 + +#define MAVLINK_MSG_ID_TERRAIN_REPORT_CRC 1 +#define MAVLINK_MSG_ID_136_CRC 1 + + + +#define MAVLINK_MESSAGE_INFO_TERRAIN_REPORT { \ + "TERRAIN_REPORT", \ + 7, \ + { { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_terrain_report_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_terrain_report_t, lon) }, \ + { "terrain_height", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_terrain_report_t, terrain_height) }, \ + { "current_height", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_terrain_report_t, current_height) }, \ + { "spacing", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_terrain_report_t, spacing) }, \ + { "pending", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_terrain_report_t, pending) }, \ + { "loaded", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_terrain_report_t, loaded) }, \ + } \ +} + + +/** + * @brief Pack a terrain_report message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param lat Latitude (degrees *10^7) + * @param lon Longitude (degrees *10^7) + * @param spacing grid spacing (zero if terrain at this location unavailable) + * @param terrain_height Terrain height in meters AMSL + * @param current_height Current vehicle height above lat/lon terrain height (meters) + * @param pending Number of 4x4 terrain blocks waiting to be received or read from disk + * @param loaded Number of 4x4 terrain blocks in memory + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_terrain_report_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + int32_t lat, int32_t lon, uint16_t spacing, float terrain_height, float current_height, uint16_t pending, uint16_t loaded) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_REPORT_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_float(buf, 8, terrain_height); + _mav_put_float(buf, 12, current_height); + _mav_put_uint16_t(buf, 16, spacing); + _mav_put_uint16_t(buf, 18, pending); + _mav_put_uint16_t(buf, 20, loaded); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#else + mavlink_terrain_report_t packet; + packet.lat = lat; + packet.lon = lon; + packet.terrain_height = terrain_height; + packet.current_height = current_height; + packet.spacing = spacing; + packet.pending = pending; + packet.loaded = loaded; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TERRAIN_REPORT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN, MAVLINK_MSG_ID_TERRAIN_REPORT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#endif +} + +/** + * @brief Pack a terrain_report message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param lat Latitude (degrees *10^7) + * @param lon Longitude (degrees *10^7) + * @param spacing grid spacing (zero if terrain at this location unavailable) + * @param terrain_height Terrain height in meters AMSL + * @param current_height Current vehicle height above lat/lon terrain height (meters) + * @param pending Number of 4x4 terrain blocks waiting to be received or read from disk + * @param loaded Number of 4x4 terrain blocks in memory + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_terrain_report_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + int32_t lat,int32_t lon,uint16_t spacing,float terrain_height,float current_height,uint16_t pending,uint16_t loaded) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_REPORT_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_float(buf, 8, terrain_height); + _mav_put_float(buf, 12, current_height); + _mav_put_uint16_t(buf, 16, spacing); + _mav_put_uint16_t(buf, 18, pending); + _mav_put_uint16_t(buf, 20, loaded); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#else + mavlink_terrain_report_t packet; + packet.lat = lat; + packet.lon = lon; + packet.terrain_height = terrain_height; + packet.current_height = current_height; + packet.spacing = spacing; + packet.pending = pending; + packet.loaded = loaded; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TERRAIN_REPORT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN, MAVLINK_MSG_ID_TERRAIN_REPORT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#endif +} + +/** + * @brief Encode a terrain_report struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param terrain_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_terrain_report_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_terrain_report_t* terrain_report) +{ + return mavlink_msg_terrain_report_pack(system_id, component_id, msg, terrain_report->lat, terrain_report->lon, terrain_report->spacing, terrain_report->terrain_height, terrain_report->current_height, terrain_report->pending, terrain_report->loaded); +} + +/** + * @brief Encode a terrain_report struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param terrain_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_terrain_report_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_terrain_report_t* terrain_report) +{ + return mavlink_msg_terrain_report_pack_chan(system_id, component_id, chan, msg, terrain_report->lat, terrain_report->lon, terrain_report->spacing, terrain_report->terrain_height, terrain_report->current_height, terrain_report->pending, terrain_report->loaded); +} + +/** + * @brief Send a terrain_report message + * @param chan MAVLink channel to send the message + * + * @param lat Latitude (degrees *10^7) + * @param lon Longitude (degrees *10^7) + * @param spacing grid spacing (zero if terrain at this location unavailable) + * @param terrain_height Terrain height in meters AMSL + * @param current_height Current vehicle height above lat/lon terrain height (meters) + * @param pending Number of 4x4 terrain blocks waiting to be received or read from disk + * @param loaded Number of 4x4 terrain blocks in memory + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_terrain_report_send(mavlink_channel_t chan, int32_t lat, int32_t lon, uint16_t spacing, float terrain_height, float current_height, uint16_t pending, uint16_t loaded) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_REPORT_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_float(buf, 8, terrain_height); + _mav_put_float(buf, 12, current_height); + _mav_put_uint16_t(buf, 16, spacing); + _mav_put_uint16_t(buf, 18, pending); + _mav_put_uint16_t(buf, 20, loaded); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REPORT, buf, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN, MAVLINK_MSG_ID_TERRAIN_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REPORT, buf, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#endif +#else + mavlink_terrain_report_t packet; + packet.lat = lat; + packet.lon = lon; + packet.terrain_height = terrain_height; + packet.current_height = current_height; + packet.spacing = spacing; + packet.pending = pending; + packet.loaded = loaded; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REPORT, (const char *)&packet, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN, MAVLINK_MSG_ID_TERRAIN_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REPORT, (const char *)&packet, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_TERRAIN_REPORT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_terrain_report_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t lat, int32_t lon, uint16_t spacing, float terrain_height, float current_height, uint16_t pending, uint16_t loaded) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_float(buf, 8, terrain_height); + _mav_put_float(buf, 12, current_height); + _mav_put_uint16_t(buf, 16, spacing); + _mav_put_uint16_t(buf, 18, pending); + _mav_put_uint16_t(buf, 20, loaded); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REPORT, buf, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN, MAVLINK_MSG_ID_TERRAIN_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REPORT, buf, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#endif +#else + mavlink_terrain_report_t *packet = (mavlink_terrain_report_t *)msgbuf; + packet->lat = lat; + packet->lon = lon; + packet->terrain_height = terrain_height; + packet->current_height = current_height; + packet->spacing = spacing; + packet->pending = pending; + packet->loaded = loaded; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REPORT, (const char *)packet, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN, MAVLINK_MSG_ID_TERRAIN_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REPORT, (const char *)packet, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE TERRAIN_REPORT UNPACKING + + +/** + * @brief Get field lat from terrain_report message + * + * @return Latitude (degrees *10^7) + */ +static inline int32_t mavlink_msg_terrain_report_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field lon from terrain_report message + * + * @return Longitude (degrees *10^7) + */ +static inline int32_t mavlink_msg_terrain_report_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field spacing from terrain_report message + * + * @return grid spacing (zero if terrain at this location unavailable) + */ +static inline uint16_t mavlink_msg_terrain_report_get_spacing(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field terrain_height from terrain_report message + * + * @return Terrain height in meters AMSL + */ +static inline float mavlink_msg_terrain_report_get_terrain_height(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field current_height from terrain_report message + * + * @return Current vehicle height above lat/lon terrain height (meters) + */ +static inline float mavlink_msg_terrain_report_get_current_height(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field pending from terrain_report message + * + * @return Number of 4x4 terrain blocks waiting to be received or read from disk + */ +static inline uint16_t mavlink_msg_terrain_report_get_pending(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 18); +} + +/** + * @brief Get field loaded from terrain_report message + * + * @return Number of 4x4 terrain blocks in memory + */ +static inline uint16_t mavlink_msg_terrain_report_get_loaded(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 20); +} + +/** + * @brief Decode a terrain_report message into a struct + * + * @param msg The message to decode + * @param terrain_report C-struct to decode the message contents into + */ +static inline void mavlink_msg_terrain_report_decode(const mavlink_message_t* msg, mavlink_terrain_report_t* terrain_report) +{ +#if MAVLINK_NEED_BYTE_SWAP + terrain_report->lat = mavlink_msg_terrain_report_get_lat(msg); + terrain_report->lon = mavlink_msg_terrain_report_get_lon(msg); + terrain_report->terrain_height = mavlink_msg_terrain_report_get_terrain_height(msg); + terrain_report->current_height = mavlink_msg_terrain_report_get_current_height(msg); + terrain_report->spacing = mavlink_msg_terrain_report_get_spacing(msg); + terrain_report->pending = mavlink_msg_terrain_report_get_pending(msg); + terrain_report->loaded = mavlink_msg_terrain_report_get_loaded(msg); +#else + memcpy(terrain_report, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_terrain_request.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_terrain_request.h new file mode 100644 index 0000000..3a85b85 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_terrain_request.h @@ -0,0 +1,281 @@ +// MESSAGE TERRAIN_REQUEST PACKING + +#define MAVLINK_MSG_ID_TERRAIN_REQUEST 133 + +typedef struct __mavlink_terrain_request_t +{ + uint64_t mask; ///< Bitmask of requested 4x4 grids (row major 8x7 array of grids, 56 bits) + int32_t lat; ///< Latitude of SW corner of first grid (degrees *10^7) + int32_t lon; ///< Longitude of SW corner of first grid (in degrees *10^7) + uint16_t grid_spacing; ///< Grid spacing in meters +} mavlink_terrain_request_t; + +#define MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN 18 +#define MAVLINK_MSG_ID_133_LEN 18 + +#define MAVLINK_MSG_ID_TERRAIN_REQUEST_CRC 6 +#define MAVLINK_MSG_ID_133_CRC 6 + + + +#define MAVLINK_MESSAGE_INFO_TERRAIN_REQUEST { \ + "TERRAIN_REQUEST", \ + 4, \ + { { "mask", "0x%07x", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_terrain_request_t, mask) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_terrain_request_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_terrain_request_t, lon) }, \ + { "grid_spacing", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_terrain_request_t, grid_spacing) }, \ + } \ +} + + +/** + * @brief Pack a terrain_request message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param lat Latitude of SW corner of first grid (degrees *10^7) + * @param lon Longitude of SW corner of first grid (in degrees *10^7) + * @param grid_spacing Grid spacing in meters + * @param mask Bitmask of requested 4x4 grids (row major 8x7 array of grids, 56 bits) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_terrain_request_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + int32_t lat, int32_t lon, uint16_t grid_spacing, uint64_t mask) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN]; + _mav_put_uint64_t(buf, 0, mask); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_uint16_t(buf, 16, grid_spacing); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#else + mavlink_terrain_request_t packet; + packet.mask = mask; + packet.lat = lat; + packet.lon = lon; + packet.grid_spacing = grid_spacing; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TERRAIN_REQUEST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN, MAVLINK_MSG_ID_TERRAIN_REQUEST_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#endif +} + +/** + * @brief Pack a terrain_request message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param lat Latitude of SW corner of first grid (degrees *10^7) + * @param lon Longitude of SW corner of first grid (in degrees *10^7) + * @param grid_spacing Grid spacing in meters + * @param mask Bitmask of requested 4x4 grids (row major 8x7 array of grids, 56 bits) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_terrain_request_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + int32_t lat,int32_t lon,uint16_t grid_spacing,uint64_t mask) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN]; + _mav_put_uint64_t(buf, 0, mask); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_uint16_t(buf, 16, grid_spacing); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#else + mavlink_terrain_request_t packet; + packet.mask = mask; + packet.lat = lat; + packet.lon = lon; + packet.grid_spacing = grid_spacing; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TERRAIN_REQUEST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN, MAVLINK_MSG_ID_TERRAIN_REQUEST_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#endif +} + +/** + * @brief Encode a terrain_request struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param terrain_request C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_terrain_request_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_terrain_request_t* terrain_request) +{ + return mavlink_msg_terrain_request_pack(system_id, component_id, msg, terrain_request->lat, terrain_request->lon, terrain_request->grid_spacing, terrain_request->mask); +} + +/** + * @brief Encode a terrain_request struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param terrain_request C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_terrain_request_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_terrain_request_t* terrain_request) +{ + return mavlink_msg_terrain_request_pack_chan(system_id, component_id, chan, msg, terrain_request->lat, terrain_request->lon, terrain_request->grid_spacing, terrain_request->mask); +} + +/** + * @brief Send a terrain_request message + * @param chan MAVLink channel to send the message + * + * @param lat Latitude of SW corner of first grid (degrees *10^7) + * @param lon Longitude of SW corner of first grid (in degrees *10^7) + * @param grid_spacing Grid spacing in meters + * @param mask Bitmask of requested 4x4 grids (row major 8x7 array of grids, 56 bits) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_terrain_request_send(mavlink_channel_t chan, int32_t lat, int32_t lon, uint16_t grid_spacing, uint64_t mask) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN]; + _mav_put_uint64_t(buf, 0, mask); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_uint16_t(buf, 16, grid_spacing); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REQUEST, buf, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN, MAVLINK_MSG_ID_TERRAIN_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REQUEST, buf, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#endif +#else + mavlink_terrain_request_t packet; + packet.mask = mask; + packet.lat = lat; + packet.lon = lon; + packet.grid_spacing = grid_spacing; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REQUEST, (const char *)&packet, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN, MAVLINK_MSG_ID_TERRAIN_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REQUEST, (const char *)&packet, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_terrain_request_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t lat, int32_t lon, uint16_t grid_spacing, uint64_t mask) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, mask); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_uint16_t(buf, 16, grid_spacing); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REQUEST, buf, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN, MAVLINK_MSG_ID_TERRAIN_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REQUEST, buf, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#endif +#else + mavlink_terrain_request_t *packet = (mavlink_terrain_request_t *)msgbuf; + packet->mask = mask; + packet->lat = lat; + packet->lon = lon; + packet->grid_spacing = grid_spacing; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REQUEST, (const char *)packet, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN, MAVLINK_MSG_ID_TERRAIN_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REQUEST, (const char *)packet, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE TERRAIN_REQUEST UNPACKING + + +/** + * @brief Get field lat from terrain_request message + * + * @return Latitude of SW corner of first grid (degrees *10^7) + */ +static inline int32_t mavlink_msg_terrain_request_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field lon from terrain_request message + * + * @return Longitude of SW corner of first grid (in degrees *10^7) + */ +static inline int32_t mavlink_msg_terrain_request_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field grid_spacing from terrain_request message + * + * @return Grid spacing in meters + */ +static inline uint16_t mavlink_msg_terrain_request_get_grid_spacing(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field mask from terrain_request message + * + * @return Bitmask of requested 4x4 grids (row major 8x7 array of grids, 56 bits) + */ +static inline uint64_t mavlink_msg_terrain_request_get_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Decode a terrain_request message into a struct + * + * @param msg The message to decode + * @param terrain_request C-struct to decode the message contents into + */ +static inline void mavlink_msg_terrain_request_decode(const mavlink_message_t* msg, mavlink_terrain_request_t* terrain_request) +{ +#if MAVLINK_NEED_BYTE_SWAP + terrain_request->mask = mavlink_msg_terrain_request_get_mask(msg); + terrain_request->lat = mavlink_msg_terrain_request_get_lat(msg); + terrain_request->lon = mavlink_msg_terrain_request_get_lon(msg); + terrain_request->grid_spacing = mavlink_msg_terrain_request_get_grid_spacing(msg); +#else + memcpy(terrain_request, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_v2_extension.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_v2_extension.h new file mode 100644 index 0000000..59a0ba0 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_v2_extension.h @@ -0,0 +1,297 @@ +// MESSAGE V2_EXTENSION PACKING + +#define MAVLINK_MSG_ID_V2_EXTENSION 248 + +typedef struct __mavlink_v2_extension_t +{ + uint16_t message_type; ///< A code that identifies the software component that understands this message (analogous to usb device classes or mime type strings). If this code is less than 32768, it is considered a 'registered' protocol extension and the corresponding entry should be added to https://github.com/mavlink/mavlink/extension-message-ids.xml. Software creators can register blocks of message IDs as needed (useful for GCS specific metadata, etc...). Message_types greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + uint8_t target_network; ///< Network ID (0 for broadcast) + uint8_t target_system; ///< System ID (0 for broadcast) + uint8_t target_component; ///< Component ID (0 for broadcast) + uint8_t payload[249]; ///< Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. +} mavlink_v2_extension_t; + +#define MAVLINK_MSG_ID_V2_EXTENSION_LEN 254 +#define MAVLINK_MSG_ID_248_LEN 254 + +#define MAVLINK_MSG_ID_V2_EXTENSION_CRC 8 +#define MAVLINK_MSG_ID_248_CRC 8 + +#define MAVLINK_MSG_V2_EXTENSION_FIELD_PAYLOAD_LEN 249 + +#define MAVLINK_MESSAGE_INFO_V2_EXTENSION { \ + "V2_EXTENSION", \ + 5, \ + { { "message_type", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_v2_extension_t, message_type) }, \ + { "target_network", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_v2_extension_t, target_network) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_v2_extension_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_v2_extension_t, target_component) }, \ + { "payload", NULL, MAVLINK_TYPE_UINT8_T, 249, 5, offsetof(mavlink_v2_extension_t, payload) }, \ + } \ +} + + +/** + * @brief Pack a v2_extension message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_network Network ID (0 for broadcast) + * @param target_system System ID (0 for broadcast) + * @param target_component Component ID (0 for broadcast) + * @param message_type A code that identifies the software component that understands this message (analogous to usb device classes or mime type strings). If this code is less than 32768, it is considered a 'registered' protocol extension and the corresponding entry should be added to https://github.com/mavlink/mavlink/extension-message-ids.xml. Software creators can register blocks of message IDs as needed (useful for GCS specific metadata, etc...). Message_types greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + * @param payload Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_v2_extension_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_network, uint8_t target_system, uint8_t target_component, uint16_t message_type, const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_V2_EXTENSION_LEN]; + _mav_put_uint16_t(buf, 0, message_type); + _mav_put_uint8_t(buf, 2, target_network); + _mav_put_uint8_t(buf, 3, target_system); + _mav_put_uint8_t(buf, 4, target_component); + _mav_put_uint8_t_array(buf, 5, payload, 249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#else + mavlink_v2_extension_t packet; + packet.message_type = message_type; + packet.target_network = target_network; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_V2_EXTENSION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_V2_EXTENSION_LEN, MAVLINK_MSG_ID_V2_EXTENSION_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#endif +} + +/** + * @brief Pack a v2_extension message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_network Network ID (0 for broadcast) + * @param target_system System ID (0 for broadcast) + * @param target_component Component ID (0 for broadcast) + * @param message_type A code that identifies the software component that understands this message (analogous to usb device classes or mime type strings). If this code is less than 32768, it is considered a 'registered' protocol extension and the corresponding entry should be added to https://github.com/mavlink/mavlink/extension-message-ids.xml. Software creators can register blocks of message IDs as needed (useful for GCS specific metadata, etc...). Message_types greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + * @param payload Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_v2_extension_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_network,uint8_t target_system,uint8_t target_component,uint16_t message_type,const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_V2_EXTENSION_LEN]; + _mav_put_uint16_t(buf, 0, message_type); + _mav_put_uint8_t(buf, 2, target_network); + _mav_put_uint8_t(buf, 3, target_system); + _mav_put_uint8_t(buf, 4, target_component); + _mav_put_uint8_t_array(buf, 5, payload, 249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#else + mavlink_v2_extension_t packet; + packet.message_type = message_type; + packet.target_network = target_network; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_V2_EXTENSION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_V2_EXTENSION_LEN, MAVLINK_MSG_ID_V2_EXTENSION_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#endif +} + +/** + * @brief Encode a v2_extension struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param v2_extension C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_v2_extension_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_v2_extension_t* v2_extension) +{ + return mavlink_msg_v2_extension_pack(system_id, component_id, msg, v2_extension->target_network, v2_extension->target_system, v2_extension->target_component, v2_extension->message_type, v2_extension->payload); +} + +/** + * @brief Encode a v2_extension struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param v2_extension C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_v2_extension_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_v2_extension_t* v2_extension) +{ + return mavlink_msg_v2_extension_pack_chan(system_id, component_id, chan, msg, v2_extension->target_network, v2_extension->target_system, v2_extension->target_component, v2_extension->message_type, v2_extension->payload); +} + +/** + * @brief Send a v2_extension message + * @param chan MAVLink channel to send the message + * + * @param target_network Network ID (0 for broadcast) + * @param target_system System ID (0 for broadcast) + * @param target_component Component ID (0 for broadcast) + * @param message_type A code that identifies the software component that understands this message (analogous to usb device classes or mime type strings). If this code is less than 32768, it is considered a 'registered' protocol extension and the corresponding entry should be added to https://github.com/mavlink/mavlink/extension-message-ids.xml. Software creators can register blocks of message IDs as needed (useful for GCS specific metadata, etc...). Message_types greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + * @param payload Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_v2_extension_send(mavlink_channel_t chan, uint8_t target_network, uint8_t target_system, uint8_t target_component, uint16_t message_type, const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_V2_EXTENSION_LEN]; + _mav_put_uint16_t(buf, 0, message_type); + _mav_put_uint8_t(buf, 2, target_network); + _mav_put_uint8_t(buf, 3, target_system); + _mav_put_uint8_t(buf, 4, target_component); + _mav_put_uint8_t_array(buf, 5, payload, 249); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_V2_EXTENSION, buf, MAVLINK_MSG_ID_V2_EXTENSION_LEN, MAVLINK_MSG_ID_V2_EXTENSION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_V2_EXTENSION, buf, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#endif +#else + mavlink_v2_extension_t packet; + packet.message_type = message_type; + packet.target_network = target_network; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*249); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_V2_EXTENSION, (const char *)&packet, MAVLINK_MSG_ID_V2_EXTENSION_LEN, MAVLINK_MSG_ID_V2_EXTENSION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_V2_EXTENSION, (const char *)&packet, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_V2_EXTENSION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_v2_extension_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_network, uint8_t target_system, uint8_t target_component, uint16_t message_type, const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, message_type); + _mav_put_uint8_t(buf, 2, target_network); + _mav_put_uint8_t(buf, 3, target_system); + _mav_put_uint8_t(buf, 4, target_component); + _mav_put_uint8_t_array(buf, 5, payload, 249); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_V2_EXTENSION, buf, MAVLINK_MSG_ID_V2_EXTENSION_LEN, MAVLINK_MSG_ID_V2_EXTENSION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_V2_EXTENSION, buf, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#endif +#else + mavlink_v2_extension_t *packet = (mavlink_v2_extension_t *)msgbuf; + packet->message_type = message_type; + packet->target_network = target_network; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->payload, payload, sizeof(uint8_t)*249); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_V2_EXTENSION, (const char *)packet, MAVLINK_MSG_ID_V2_EXTENSION_LEN, MAVLINK_MSG_ID_V2_EXTENSION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_V2_EXTENSION, (const char *)packet, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE V2_EXTENSION UNPACKING + + +/** + * @brief Get field target_network from v2_extension message + * + * @return Network ID (0 for broadcast) + */ +static inline uint8_t mavlink_msg_v2_extension_get_target_network(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field target_system from v2_extension message + * + * @return System ID (0 for broadcast) + */ +static inline uint8_t mavlink_msg_v2_extension_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field target_component from v2_extension message + * + * @return Component ID (0 for broadcast) + */ +static inline uint8_t mavlink_msg_v2_extension_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field message_type from v2_extension message + * + * @return A code that identifies the software component that understands this message (analogous to usb device classes or mime type strings). If this code is less than 32768, it is considered a 'registered' protocol extension and the corresponding entry should be added to https://github.com/mavlink/mavlink/extension-message-ids.xml. Software creators can register blocks of message IDs as needed (useful for GCS specific metadata, etc...). Message_types greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + */ +static inline uint16_t mavlink_msg_v2_extension_get_message_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field payload from v2_extension message + * + * @return Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + */ +static inline uint16_t mavlink_msg_v2_extension_get_payload(const mavlink_message_t* msg, uint8_t *payload) +{ + return _MAV_RETURN_uint8_t_array(msg, payload, 249, 5); +} + +/** + * @brief Decode a v2_extension message into a struct + * + * @param msg The message to decode + * @param v2_extension C-struct to decode the message contents into + */ +static inline void mavlink_msg_v2_extension_decode(const mavlink_message_t* msg, mavlink_v2_extension_t* v2_extension) +{ +#if MAVLINK_NEED_BYTE_SWAP + v2_extension->message_type = mavlink_msg_v2_extension_get_message_type(msg); + v2_extension->target_network = mavlink_msg_v2_extension_get_target_network(msg); + v2_extension->target_system = mavlink_msg_v2_extension_get_target_system(msg); + v2_extension->target_component = mavlink_msg_v2_extension_get_target_component(msg); + mavlink_msg_v2_extension_get_payload(msg, v2_extension->payload); +#else + memcpy(v2_extension, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h index 5b1093a..b130ee5 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspe #endif } +#if MAVLINK_MSG_ID_VFR_HUD_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_vfr_hud_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, airspeed); + _mav_put_float(buf, 4, groundspeed); + _mav_put_float(buf, 8, alt); + _mav_put_float(buf, 12, climb); + _mav_put_int16_t(buf, 16, heading); + _mav_put_uint16_t(buf, 18, throttle); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, buf, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, buf, MAVLINK_MSG_ID_VFR_HUD_LEN); +#endif +#else + mavlink_vfr_hud_t *packet = (mavlink_vfr_hud_t *)msgbuf; + packet->airspeed = airspeed; + packet->groundspeed = groundspeed; + packet->alt = alt; + packet->climb = climb; + packet->heading = heading; + packet->throttle = throttle; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, (const char *)packet, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, (const char *)packet, MAVLINK_MSG_ID_VFR_HUD_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE VFR_HUD UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h index a254202..b3fa7bc 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t ch #endif } +#if MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_vicon_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); +#endif +#else + mavlink_vicon_position_estimate_t *packet = (mavlink_vicon_position_estimate_t *)msgbuf; + packet->usec = usec; + packet->x = x; + packet->y = y; + packet->z = z; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE VICON_POSITION_ESTIMATE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h index f7a741b..8f82fb6 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t c #endif } +#if MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_vision_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); +#endif +#else + mavlink_vision_position_estimate_t *packet = (mavlink_vision_position_estimate_t *)msgbuf; + packet->usec = usec; + packet->x = x; + packet->y = y; + packet->z = z; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE VISION_POSITION_ESTIMATE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h index 6602251..7528014 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan #endif } +#if MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_vision_speed_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); +#endif +#else + mavlink_vision_speed_estimate_t *packet = (mavlink_vision_speed_estimate_t *)msgbuf; + packet->usec = usec; + packet->x = x; + packet->y = y; + packet->z = z; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE VISION_SPEED_ESTIMATE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/testsuite.h b/mavlink/include/mavlink/v1.0/common/testsuite.h index c5aa9dd..4c51037 100644 --- a/mavlink/include/mavlink/v1.0/common/testsuite.h +++ b/mavlink/include/mavlink/v1.0/common/testsuite.h @@ -30,13 +30,8 @@ static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id, mavl uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_heartbeat_t packet_in = { - 963497464, - }17, - }84, - }151, - }218, - }3, - }; + 963497464,17,84,151,218,3 + }; mavlink_heartbeat_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.custom_mode = packet_in.custom_mode; @@ -83,20 +78,8 @@ static void mavlink_test_sys_status(uint8_t system_id, uint8_t component_id, mav uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_sys_status_t packet_in = { - 963497464, - }963497672, - }963497880, - }17859, - }17963, - }18067, - }18171, - }18275, - }18379, - }18483, - }18587, - }18691, - }223, - }; + 963497464,963497672,963497880,17859,17963,18067,18171,18275,18379,18483,18587,18691,223 + }; mavlink_sys_status_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.onboard_control_sensors_present = packet_in.onboard_control_sensors_present; @@ -150,9 +133,8 @@ static void mavlink_test_system_time(uint8_t system_id, uint8_t component_id, ma uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_system_time_t packet_in = { - 93372036854775807ULL, - }963497880, - }; + 93372036854775807ULL,963497880 + }; mavlink_system_time_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.time_unix_usec = packet_in.time_unix_usec; @@ -195,11 +177,8 @@ static void mavlink_test_ping(uint8_t system_id, uint8_t component_id, mavlink_m uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_ping_t packet_in = { - 93372036854775807ULL, - }963497880, - }41, - }108, - }; + 93372036854775807ULL,963497880,41,108 + }; mavlink_ping_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.time_usec = packet_in.time_usec; @@ -244,11 +223,8 @@ static void mavlink_test_change_operator_control(uint8_t system_id, uint8_t comp uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_change_operator_control_t packet_in = { - 5, - }72, - }139, - }"DEFGHIJKLMNOPQRSTUVWXYZA", - }; + 5,72,139,"DEFGHIJKLMNOPQRSTUVWXYZA" + }; mavlink_change_operator_control_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.target_system = packet_in.target_system; @@ -293,10 +269,8 @@ static void mavlink_test_change_operator_control_ack(uint8_t system_id, uint8_t uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_change_operator_control_ack_t packet_in = { - 5, - }72, - }139, - }; + 5,72,139 + }; mavlink_change_operator_control_ack_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.gcs_system_id = packet_in.gcs_system_id; @@ -340,8 +314,8 @@ static void mavlink_test_auth_key(uint8_t system_id, uint8_t component_id, mavli uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_auth_key_t packet_in = { - "ABCDEFGHIJKLMNOPQRSTUVWXYZABCDE", - }; + "ABCDEFGHIJKLMNOPQRSTUVWXYZABCDE" + }; mavlink_auth_key_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -383,10 +357,8 @@ static void mavlink_test_set_mode(uint8_t system_id, uint8_t component_id, mavli uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_set_mode_t packet_in = { - 963497464, - }17, - }84, - }; + 963497464,17,84 + }; mavlink_set_mode_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.custom_mode = packet_in.custom_mode; @@ -430,11 +402,8 @@ static void mavlink_test_param_request_read(uint8_t system_id, uint8_t component uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_param_request_read_t packet_in = { - 17235, - }139, - }206, - }"EFGHIJKLMNOPQRS", - }; + 17235,139,206,"EFGHIJKLMNOPQRS" + }; mavlink_param_request_read_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.param_index = packet_in.param_index; @@ -479,9 +448,8 @@ static void mavlink_test_param_request_list(uint8_t system_id, uint8_t component uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_param_request_list_t packet_in = { - 5, - }72, - }; + 5,72 + }; mavlink_param_request_list_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.target_system = packet_in.target_system; @@ -524,12 +492,8 @@ static void mavlink_test_param_value(uint8_t system_id, uint8_t component_id, ma uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_param_value_t packet_in = { - 17.0, - }17443, - }17547, - }"IJKLMNOPQRSTUVW", - }77, - }; + 17.0,17443,17547,"IJKLMNOPQRSTUVW",77 + }; mavlink_param_value_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.param_value = packet_in.param_value; @@ -575,12 +539,8 @@ static void mavlink_test_param_set(uint8_t system_id, uint8_t component_id, mavl uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_param_set_t packet_in = { - 17.0, - }17, - }84, - }"GHIJKLMNOPQRSTU", - }199, - }; + 17.0,17,84,"GHIJKLMNOPQRSTU",199 + }; mavlink_param_set_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.param_value = packet_in.param_value; @@ -626,17 +586,8 @@ static void mavlink_test_gps_raw_int(uint8_t system_id, uint8_t component_id, ma uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_gps_raw_int_t packet_in = { - 93372036854775807ULL, - }963497880, - }963498088, - }963498296, - }18275, - }18379, - }18483, - }18587, - }89, - }156, - }; + 93372036854775807ULL,963497880,963498088,963498296,18275,18379,18483,18587,89,156 + }; mavlink_gps_raw_int_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.time_usec = packet_in.time_usec; @@ -687,13 +638,8 @@ static void mavlink_test_gps_status(uint8_t system_id, uint8_t component_id, mav uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_gps_status_t packet_in = { - 5, - }{ 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91 }, - }{ 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151 }, - }{ 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211 }, - }{ 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 }, - }{ 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75 }, - }; + 5,{ 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91 },{ 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151 },{ 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211 },{ 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 },{ 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75 } + }; mavlink_gps_status_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.satellites_visible = packet_in.satellites_visible; @@ -740,17 +686,8 @@ static void mavlink_test_scaled_imu(uint8_t system_id, uint8_t component_id, mav uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_scaled_imu_t packet_in = { - 963497464, - }17443, - }17547, - }17651, - }17755, - }17859, - }17963, - }18067, - }18171, - }18275, - }; + 963497464,17443,17547,17651,17755,17859,17963,18067,18171,18275 + }; mavlink_scaled_imu_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.time_boot_ms = packet_in.time_boot_ms; @@ -801,17 +738,8 @@ static void mavlink_test_raw_imu(uint8_t system_id, uint8_t component_id, mavlin uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_raw_imu_t packet_in = { - 93372036854775807ULL, - }17651, - }17755, - }17859, - }17963, - }18067, - }18171, - }18275, - }18379, - }18483, - }; + 93372036854775807ULL,17651,17755,17859,17963,18067,18171,18275,18379,18483 + }; mavlink_raw_imu_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.time_usec = packet_in.time_usec; @@ -862,12 +790,8 @@ static void mavlink_test_raw_pressure(uint8_t system_id, uint8_t component_id, m uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_raw_pressure_t packet_in = { - 93372036854775807ULL, - }17651, - }17755, - }17859, - }17963, - }; + 93372036854775807ULL,17651,17755,17859,17963 + }; mavlink_raw_pressure_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.time_usec = packet_in.time_usec; @@ -913,11 +837,8 @@ static void mavlink_test_scaled_pressure(uint8_t system_id, uint8_t component_id uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_scaled_pressure_t packet_in = { - 963497464, - }45.0, - }73.0, - }17859, - }; + 963497464,45.0,73.0,17859 + }; mavlink_scaled_pressure_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.time_boot_ms = packet_in.time_boot_ms; @@ -962,14 +883,8 @@ static void mavlink_test_attitude(uint8_t system_id, uint8_t component_id, mavli uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_attitude_t packet_in = { - 963497464, - }45.0, - }73.0, - }101.0, - }129.0, - }157.0, - }185.0, - }; + 963497464,45.0,73.0,101.0,129.0,157.0,185.0 + }; mavlink_attitude_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.time_boot_ms = packet_in.time_boot_ms; @@ -1017,15 +932,8 @@ static void mavlink_test_attitude_quaternion(uint8_t system_id, uint8_t componen 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, - }; + 963497464,45.0,73.0,101.0,129.0,157.0,185.0,213.0 + }; mavlink_attitude_quaternion_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.time_boot_ms = packet_in.time_boot_ms; @@ -1074,14 +982,8 @@ static void mavlink_test_local_position_ned(uint8_t system_id, uint8_t component uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_local_position_ned_t packet_in = { - 963497464, - }45.0, - }73.0, - }101.0, - }129.0, - }157.0, - }185.0, - }; + 963497464,45.0,73.0,101.0,129.0,157.0,185.0 + }; mavlink_local_position_ned_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.time_boot_ms = packet_in.time_boot_ms; @@ -1129,16 +1031,8 @@ static void mavlink_test_global_position_int(uint8_t system_id, uint8_t componen uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_global_position_int_t packet_in = { - 963497464, - }963497672, - }963497880, - }963498088, - }963498296, - }18275, - }18379, - }18483, - }18587, - }; + 963497464,963497672,963497880,963498088,963498296,18275,18379,18483,18587 + }; mavlink_global_position_int_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.time_boot_ms = packet_in.time_boot_ms; @@ -1188,18 +1082,8 @@ static void mavlink_test_rc_channels_scaled(uint8_t system_id, uint8_t component uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_rc_channels_scaled_t packet_in = { - 963497464, - }17443, - }17547, - }17651, - }17755, - }17859, - }17963, - }18067, - }18171, - }65, - }132, - }; + 963497464,17443,17547,17651,17755,17859,17963,18067,18171,65,132 + }; mavlink_rc_channels_scaled_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.time_boot_ms = packet_in.time_boot_ms; @@ -1251,18 +1135,8 @@ static void mavlink_test_rc_channels_raw(uint8_t system_id, uint8_t component_id uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_rc_channels_raw_t packet_in = { - 963497464, - }17443, - }17547, - }17651, - }17755, - }17859, - }17963, - }18067, - }18171, - }65, - }132, - }; + 963497464,17443,17547,17651,17755,17859,17963,18067,18171,65,132 + }; mavlink_rc_channels_raw_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.time_boot_ms = packet_in.time_boot_ms; @@ -1314,17 +1188,8 @@ static void mavlink_test_servo_output_raw(uint8_t system_id, uint8_t component_i uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_servo_output_raw_t packet_in = { - 963497464, - }17443, - }17547, - }17651, - }17755, - }17859, - }17963, - }18067, - }18171, - }65, - }; + 963497464,17443,17547,17651,17755,17859,17963,18067,18171,65 + }; mavlink_servo_output_raw_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.time_usec = packet_in.time_usec; @@ -1375,11 +1240,8 @@ static void mavlink_test_mission_request_partial_list(uint8_t system_id, uint8_t uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_request_partial_list_t packet_in = { - 17235, - }17339, - }17, - }84, - }; + 17235,17339,17,84 + }; mavlink_mission_request_partial_list_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.start_index = packet_in.start_index; @@ -1424,11 +1286,8 @@ static void mavlink_test_mission_write_partial_list(uint8_t system_id, uint8_t c uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_write_partial_list_t packet_in = { - 17235, - }17339, - }17, - }84, - }; + 17235,17339,17,84 + }; mavlink_mission_write_partial_list_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.start_index = packet_in.start_index; @@ -1473,21 +1332,8 @@ static void mavlink_test_mission_item(uint8_t system_id, uint8_t component_id, m uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_item_t packet_in = { - 17.0, - }45.0, - }73.0, - }101.0, - }129.0, - }157.0, - }185.0, - }18691, - }18795, - }101, - }168, - }235, - }46, - }113, - }; + 17.0,45.0,73.0,101.0,129.0,157.0,185.0,18691,18795,101,168,235,46,113 + }; mavlink_mission_item_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.param1 = packet_in.param1; @@ -1542,10 +1388,8 @@ static void mavlink_test_mission_request(uint8_t system_id, uint8_t component_id uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_request_t packet_in = { - 17235, - }139, - }206, - }; + 17235,139,206 + }; mavlink_mission_request_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.seq = packet_in.seq; @@ -1589,10 +1433,8 @@ static void mavlink_test_mission_set_current(uint8_t system_id, uint8_t componen uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_set_current_t packet_in = { - 17235, - }139, - }206, - }; + 17235,139,206 + }; mavlink_mission_set_current_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.seq = packet_in.seq; @@ -1636,8 +1478,8 @@ static void mavlink_test_mission_current(uint8_t system_id, uint8_t component_id uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_current_t packet_in = { - 17235, - }; + 17235 + }; mavlink_mission_current_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.seq = packet_in.seq; @@ -1679,9 +1521,8 @@ static void mavlink_test_mission_request_list(uint8_t system_id, uint8_t compone uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_request_list_t packet_in = { - 5, - }72, - }; + 5,72 + }; mavlink_mission_request_list_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.target_system = packet_in.target_system; @@ -1724,10 +1565,8 @@ static void mavlink_test_mission_count(uint8_t system_id, uint8_t component_id, uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_count_t packet_in = { - 17235, - }139, - }206, - }; + 17235,139,206 + }; mavlink_mission_count_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.count = packet_in.count; @@ -1771,9 +1610,8 @@ static void mavlink_test_mission_clear_all(uint8_t system_id, uint8_t component_ uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_clear_all_t packet_in = { - 5, - }72, - }; + 5,72 + }; mavlink_mission_clear_all_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.target_system = packet_in.target_system; @@ -1816,8 +1654,8 @@ static void mavlink_test_mission_item_reached(uint8_t system_id, uint8_t compone uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_item_reached_t packet_in = { - 17235, - }; + 17235 + }; mavlink_mission_item_reached_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.seq = packet_in.seq; @@ -1859,10 +1697,8 @@ static void mavlink_test_mission_ack(uint8_t system_id, uint8_t component_id, ma uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_ack_t packet_in = { - 5, - }72, - }139, - }; + 5,72,139 + }; mavlink_mission_ack_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.target_system = packet_in.target_system; @@ -1906,11 +1742,8 @@ static void mavlink_test_set_gps_global_origin(uint8_t system_id, uint8_t compon uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_set_gps_global_origin_t packet_in = { - 963497464, - }963497672, - }963497880, - }41, - }; + 963497464,963497672,963497880,41 + }; mavlink_set_gps_global_origin_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.latitude = packet_in.latitude; @@ -1955,10 +1788,8 @@ static void mavlink_test_gps_global_origin(uint8_t system_id, uint8_t component_ uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_gps_global_origin_t packet_in = { - 963497464, - }963497672, - }963497880, - }; + 963497464,963497672,963497880 + }; mavlink_gps_global_origin_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.latitude = packet_in.latitude; @@ -1996,45 +1827,41 @@ static void mavlink_test_gps_global_origin(uint8_t system_id, uint8_t component_ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } -static void mavlink_test_set_local_position_setpoint(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +static void mavlink_test_safety_set_allowed_area(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_set_local_position_setpoint_t packet_in = { - 17.0, - }45.0, - }73.0, - }101.0, - }53, - }120, - }187, - }; - mavlink_set_local_position_setpoint_t packet1, packet2; + mavlink_safety_set_allowed_area_t packet_in = { + 17.0,45.0,73.0,101.0,129.0,157.0,77,144,211 + }; + mavlink_safety_set_allowed_area_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.x = packet_in.x; - packet1.y = packet_in.y; - packet1.z = packet_in.z; - packet1.yaw = packet_in.yaw; + packet1.p1x = packet_in.p1x; + packet1.p1y = packet_in.p1y; + packet1.p1z = packet_in.p1z; + packet1.p2x = packet_in.p2x; + packet1.p2y = packet_in.p2y; + packet1.p2z = packet_in.p2z; packet1.target_system = packet_in.target_system; packet1.target_component = packet_in.target_component; - packet1.coordinate_frame = packet_in.coordinate_frame; + packet1.frame = packet_in.frame; memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_set_local_position_setpoint_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_set_local_position_setpoint_decode(&msg, &packet2); + mavlink_msg_safety_set_allowed_area_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_safety_set_allowed_area_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_set_local_position_setpoint_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.coordinate_frame , packet1.x , packet1.y , packet1.z , packet1.yaw ); - mavlink_msg_set_local_position_setpoint_decode(&msg, &packet2); + mavlink_msg_safety_set_allowed_area_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.frame , packet1.p1x , packet1.p1y , packet1.p1z , packet1.p2x , packet1.p2y , packet1.p2z ); + mavlink_msg_safety_set_allowed_area_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_set_local_position_setpoint_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.coordinate_frame , packet1.x , packet1.y , packet1.z , packet1.yaw ); - mavlink_msg_set_local_position_setpoint_decode(&msg, &packet2); + mavlink_msg_safety_set_allowed_area_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.frame , packet1.p1x , packet1.p1y , packet1.p1z , packet1.p2x , packet1.p2y , packet1.p2z ); + mavlink_msg_safety_set_allowed_area_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); @@ -2042,50 +1869,48 @@ static void mavlink_test_set_local_position_setpoint(uint8_t system_id, uint8_t for (i=0; i 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ - MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ - MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ - MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ - MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ - MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ - MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ - MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - MAV_CMD_ENUM_END=501, /* | */ -}; -#endif - -#include "../common/common.h" - -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -// MESSAGE DEFINITIONS -#include "./mavlink_msg_flexifunction_set.h" -#include "./mavlink_msg_flexifunction_read_req.h" -#include "./mavlink_msg_flexifunction_buffer_function.h" -#include "./mavlink_msg_flexifunction_buffer_function_ack.h" -#include "./mavlink_msg_flexifunction_directory.h" -#include "./mavlink_msg_flexifunction_directory_ack.h" -#include "./mavlink_msg_flexifunction_command.h" -#include "./mavlink_msg_flexifunction_command_ack.h" -#include "./mavlink_msg_serial_udb_extra_f2_a.h" -#include "./mavlink_msg_serial_udb_extra_f2_b.h" -#include "./mavlink_msg_serial_udb_extra_f4.h" -#include "./mavlink_msg_serial_udb_extra_f5.h" -#include "./mavlink_msg_serial_udb_extra_f6.h" -#include "./mavlink_msg_serial_udb_extra_f7.h" -#include "./mavlink_msg_serial_udb_extra_f8.h" -#include "./mavlink_msg_serial_udb_extra_f13.h" -#include "./mavlink_msg_serial_udb_extra_f14.h" -#include "./mavlink_msg_serial_udb_extra_f15.h" -#include "./mavlink_msg_serial_udb_extra_f16.h" -#include "./mavlink_msg_altitudes.h" -#include "./mavlink_msg_airspeeds.h" - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // MATRIXPILOT_H diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink.h deleted file mode 100644 index 7e62a97..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink.h +++ /dev/null @@ -1,27 +0,0 @@ -/** @file - * @brief MAVLink comm protocol built from matrixpilot.xml - * @see http://pixhawk.ethz.ch/software/mavlink - */ -#ifndef MAVLINK_H -#define MAVLINK_H - -#ifndef MAVLINK_STX -#define MAVLINK_STX 254 -#endif - -#ifndef MAVLINK_ENDIAN -#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN -#endif - -#ifndef MAVLINK_ALIGNED_FIELDS -#define MAVLINK_ALIGNED_FIELDS 1 -#endif - -#ifndef MAVLINK_CRC_EXTRA -#define MAVLINK_CRC_EXTRA 1 -#endif - -#include "version.h" -#include "matrixpilot.h" - -#endif // MAVLINK_H diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h deleted file mode 100644 index bc47a54..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h +++ /dev/null @@ -1,309 +0,0 @@ -// MESSAGE AIRSPEEDS PACKING - -#define MAVLINK_MSG_ID_AIRSPEEDS 182 - -typedef struct __mavlink_airspeeds_t -{ - uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - int16_t airspeed_imu; ///< Airspeed estimate from IMU, cm/s - int16_t airspeed_pitot; ///< Pitot measured forward airpseed, cm/s - int16_t airspeed_hot_wire; ///< Hot wire anenometer measured airspeed, cm/s - int16_t airspeed_ultrasonic; ///< Ultrasonic measured airspeed, cm/s - int16_t aoa; ///< Angle of attack sensor, degrees * 10 - int16_t aoy; ///< Yaw angle sensor, degrees * 10 -} mavlink_airspeeds_t; - -#define MAVLINK_MSG_ID_AIRSPEEDS_LEN 16 -#define MAVLINK_MSG_ID_182_LEN 16 - -#define MAVLINK_MSG_ID_AIRSPEEDS_CRC 154 -#define MAVLINK_MSG_ID_182_CRC 154 - - - -#define MAVLINK_MESSAGE_INFO_AIRSPEEDS { \ - "AIRSPEEDS", \ - 7, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_airspeeds_t, time_boot_ms) }, \ - { "airspeed_imu", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_airspeeds_t, airspeed_imu) }, \ - { "airspeed_pitot", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_airspeeds_t, airspeed_pitot) }, \ - { "airspeed_hot_wire", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_airspeeds_t, airspeed_hot_wire) }, \ - { "airspeed_ultrasonic", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_airspeeds_t, airspeed_ultrasonic) }, \ - { "aoa", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_airspeeds_t, aoa) }, \ - { "aoy", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_airspeeds_t, aoy) }, \ - } \ -} - - -/** - * @brief Pack a airspeeds message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param airspeed_imu Airspeed estimate from IMU, cm/s - * @param airspeed_pitot Pitot measured forward airpseed, cm/s - * @param airspeed_hot_wire Hot wire anenometer measured airspeed, cm/s - * @param airspeed_ultrasonic Ultrasonic measured airspeed, cm/s - * @param aoa Angle of attack sensor, degrees * 10 - * @param aoy Yaw angle sensor, degrees * 10 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_airspeeds_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, int16_t airspeed_imu, int16_t airspeed_pitot, int16_t airspeed_hot_wire, int16_t airspeed_ultrasonic, int16_t aoa, int16_t aoy) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AIRSPEEDS_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int16_t(buf, 4, airspeed_imu); - _mav_put_int16_t(buf, 6, airspeed_pitot); - _mav_put_int16_t(buf, 8, airspeed_hot_wire); - _mav_put_int16_t(buf, 10, airspeed_ultrasonic); - _mav_put_int16_t(buf, 12, aoa); - _mav_put_int16_t(buf, 14, aoy); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEEDS_LEN); -#else - mavlink_airspeeds_t packet; - packet.time_boot_ms = time_boot_ms; - packet.airspeed_imu = airspeed_imu; - packet.airspeed_pitot = airspeed_pitot; - packet.airspeed_hot_wire = airspeed_hot_wire; - packet.airspeed_ultrasonic = airspeed_ultrasonic; - packet.aoa = aoa; - packet.aoy = aoy; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEEDS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AIRSPEEDS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIRSPEEDS_LEN, MAVLINK_MSG_ID_AIRSPEEDS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIRSPEEDS_LEN); -#endif -} - -/** - * @brief Pack a airspeeds message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param airspeed_imu Airspeed estimate from IMU, cm/s - * @param airspeed_pitot Pitot measured forward airpseed, cm/s - * @param airspeed_hot_wire Hot wire anenometer measured airspeed, cm/s - * @param airspeed_ultrasonic Ultrasonic measured airspeed, cm/s - * @param aoa Angle of attack sensor, degrees * 10 - * @param aoy Yaw angle sensor, degrees * 10 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_airspeeds_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,int16_t airspeed_imu,int16_t airspeed_pitot,int16_t airspeed_hot_wire,int16_t airspeed_ultrasonic,int16_t aoa,int16_t aoy) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AIRSPEEDS_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int16_t(buf, 4, airspeed_imu); - _mav_put_int16_t(buf, 6, airspeed_pitot); - _mav_put_int16_t(buf, 8, airspeed_hot_wire); - _mav_put_int16_t(buf, 10, airspeed_ultrasonic); - _mav_put_int16_t(buf, 12, aoa); - _mav_put_int16_t(buf, 14, aoy); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEEDS_LEN); -#else - mavlink_airspeeds_t packet; - packet.time_boot_ms = time_boot_ms; - packet.airspeed_imu = airspeed_imu; - packet.airspeed_pitot = airspeed_pitot; - packet.airspeed_hot_wire = airspeed_hot_wire; - packet.airspeed_ultrasonic = airspeed_ultrasonic; - packet.aoa = aoa; - packet.aoy = aoy; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEEDS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AIRSPEEDS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIRSPEEDS_LEN, MAVLINK_MSG_ID_AIRSPEEDS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIRSPEEDS_LEN); -#endif -} - -/** - * @brief Encode a airspeeds struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param airspeeds C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_airspeeds_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_airspeeds_t* airspeeds) -{ - return mavlink_msg_airspeeds_pack(system_id, component_id, msg, airspeeds->time_boot_ms, airspeeds->airspeed_imu, airspeeds->airspeed_pitot, airspeeds->airspeed_hot_wire, airspeeds->airspeed_ultrasonic, airspeeds->aoa, airspeeds->aoy); -} - -/** - * @brief Encode a airspeeds struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param airspeeds C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_airspeeds_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_airspeeds_t* airspeeds) -{ - return mavlink_msg_airspeeds_pack_chan(system_id, component_id, chan, msg, airspeeds->time_boot_ms, airspeeds->airspeed_imu, airspeeds->airspeed_pitot, airspeeds->airspeed_hot_wire, airspeeds->airspeed_ultrasonic, airspeeds->aoa, airspeeds->aoy); -} - -/** - * @brief Send a airspeeds message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param airspeed_imu Airspeed estimate from IMU, cm/s - * @param airspeed_pitot Pitot measured forward airpseed, cm/s - * @param airspeed_hot_wire Hot wire anenometer measured airspeed, cm/s - * @param airspeed_ultrasonic Ultrasonic measured airspeed, cm/s - * @param aoa Angle of attack sensor, degrees * 10 - * @param aoy Yaw angle sensor, degrees * 10 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_airspeeds_send(mavlink_channel_t chan, uint32_t time_boot_ms, int16_t airspeed_imu, int16_t airspeed_pitot, int16_t airspeed_hot_wire, int16_t airspeed_ultrasonic, int16_t aoa, int16_t aoy) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AIRSPEEDS_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int16_t(buf, 4, airspeed_imu); - _mav_put_int16_t(buf, 6, airspeed_pitot); - _mav_put_int16_t(buf, 8, airspeed_hot_wire); - _mav_put_int16_t(buf, 10, airspeed_ultrasonic); - _mav_put_int16_t(buf, 12, aoa); - _mav_put_int16_t(buf, 14, aoy); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, buf, MAVLINK_MSG_ID_AIRSPEEDS_LEN, MAVLINK_MSG_ID_AIRSPEEDS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, buf, MAVLINK_MSG_ID_AIRSPEEDS_LEN); -#endif -#else - mavlink_airspeeds_t packet; - packet.time_boot_ms = time_boot_ms; - packet.airspeed_imu = airspeed_imu; - packet.airspeed_pitot = airspeed_pitot; - packet.airspeed_hot_wire = airspeed_hot_wire; - packet.airspeed_ultrasonic = airspeed_ultrasonic; - packet.aoa = aoa; - packet.aoy = aoy; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, (const char *)&packet, MAVLINK_MSG_ID_AIRSPEEDS_LEN, MAVLINK_MSG_ID_AIRSPEEDS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, (const char *)&packet, MAVLINK_MSG_ID_AIRSPEEDS_LEN); -#endif -#endif -} - -#endif - -// MESSAGE AIRSPEEDS UNPACKING - - -/** - * @brief Get field time_boot_ms from airspeeds message - * - * @return Timestamp (milliseconds since system boot) - */ -static inline uint32_t mavlink_msg_airspeeds_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field airspeed_imu from airspeeds message - * - * @return Airspeed estimate from IMU, cm/s - */ -static inline int16_t mavlink_msg_airspeeds_get_airspeed_imu(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 4); -} - -/** - * @brief Get field airspeed_pitot from airspeeds message - * - * @return Pitot measured forward airpseed, cm/s - */ -static inline int16_t mavlink_msg_airspeeds_get_airspeed_pitot(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 6); -} - -/** - * @brief Get field airspeed_hot_wire from airspeeds message - * - * @return Hot wire anenometer measured airspeed, cm/s - */ -static inline int16_t mavlink_msg_airspeeds_get_airspeed_hot_wire(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 8); -} - -/** - * @brief Get field airspeed_ultrasonic from airspeeds message - * - * @return Ultrasonic measured airspeed, cm/s - */ -static inline int16_t mavlink_msg_airspeeds_get_airspeed_ultrasonic(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 10); -} - -/** - * @brief Get field aoa from airspeeds message - * - * @return Angle of attack sensor, degrees * 10 - */ -static inline int16_t mavlink_msg_airspeeds_get_aoa(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 12); -} - -/** - * @brief Get field aoy from airspeeds message - * - * @return Yaw angle sensor, degrees * 10 - */ -static inline int16_t mavlink_msg_airspeeds_get_aoy(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 14); -} - -/** - * @brief Decode a airspeeds message into a struct - * - * @param msg The message to decode - * @param airspeeds C-struct to decode the message contents into - */ -static inline void mavlink_msg_airspeeds_decode(const mavlink_message_t* msg, mavlink_airspeeds_t* airspeeds) -{ -#if MAVLINK_NEED_BYTE_SWAP - airspeeds->time_boot_ms = mavlink_msg_airspeeds_get_time_boot_ms(msg); - airspeeds->airspeed_imu = mavlink_msg_airspeeds_get_airspeed_imu(msg); - airspeeds->airspeed_pitot = mavlink_msg_airspeeds_get_airspeed_pitot(msg); - airspeeds->airspeed_hot_wire = mavlink_msg_airspeeds_get_airspeed_hot_wire(msg); - airspeeds->airspeed_ultrasonic = mavlink_msg_airspeeds_get_airspeed_ultrasonic(msg); - airspeeds->aoa = mavlink_msg_airspeeds_get_aoa(msg); - airspeeds->aoy = mavlink_msg_airspeeds_get_aoy(msg); -#else - memcpy(airspeeds, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AIRSPEEDS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h deleted file mode 100644 index e64787c..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h +++ /dev/null @@ -1,309 +0,0 @@ -// MESSAGE ALTITUDES PACKING - -#define MAVLINK_MSG_ID_ALTITUDES 181 - -typedef struct __mavlink_altitudes_t -{ - uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - int32_t alt_gps; ///< GPS altitude in meters, expressed as * 1000 (millimeters), above MSL - int32_t alt_imu; ///< IMU altitude above ground in meters, expressed as * 1000 (millimeters) - int32_t alt_barometric; ///< barometeric altitude above ground in meters, expressed as * 1000 (millimeters) - int32_t alt_optical_flow; ///< Optical flow altitude above ground in meters, expressed as * 1000 (millimeters) - int32_t alt_range_finder; ///< Rangefinder Altitude above ground in meters, expressed as * 1000 (millimeters) - int32_t alt_extra; ///< Extra altitude above ground in meters, expressed as * 1000 (millimeters) -} mavlink_altitudes_t; - -#define MAVLINK_MSG_ID_ALTITUDES_LEN 28 -#define MAVLINK_MSG_ID_181_LEN 28 - -#define MAVLINK_MSG_ID_ALTITUDES_CRC 55 -#define MAVLINK_MSG_ID_181_CRC 55 - - - -#define MAVLINK_MESSAGE_INFO_ALTITUDES { \ - "ALTITUDES", \ - 7, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_altitudes_t, time_boot_ms) }, \ - { "alt_gps", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_altitudes_t, alt_gps) }, \ - { "alt_imu", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_altitudes_t, alt_imu) }, \ - { "alt_barometric", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_altitudes_t, alt_barometric) }, \ - { "alt_optical_flow", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_altitudes_t, alt_optical_flow) }, \ - { "alt_range_finder", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_altitudes_t, alt_range_finder) }, \ - { "alt_extra", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_altitudes_t, alt_extra) }, \ - } \ -} - - -/** - * @brief Pack a altitudes message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param alt_gps GPS altitude in meters, expressed as * 1000 (millimeters), above MSL - * @param alt_imu IMU altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_barometric barometeric altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_optical_flow Optical flow altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_range_finder Rangefinder Altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_extra Extra altitude above ground in meters, expressed as * 1000 (millimeters) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_altitudes_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, int32_t alt_gps, int32_t alt_imu, int32_t alt_barometric, int32_t alt_optical_flow, int32_t alt_range_finder, int32_t alt_extra) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ALTITUDES_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int32_t(buf, 4, alt_gps); - _mav_put_int32_t(buf, 8, alt_imu); - _mav_put_int32_t(buf, 12, alt_barometric); - _mav_put_int32_t(buf, 16, alt_optical_flow); - _mav_put_int32_t(buf, 20, alt_range_finder); - _mav_put_int32_t(buf, 24, alt_extra); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ALTITUDES_LEN); -#else - mavlink_altitudes_t packet; - packet.time_boot_ms = time_boot_ms; - packet.alt_gps = alt_gps; - packet.alt_imu = alt_imu; - packet.alt_barometric = alt_barometric; - packet.alt_optical_flow = alt_optical_flow; - packet.alt_range_finder = alt_range_finder; - packet.alt_extra = alt_extra; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ALTITUDES_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ALTITUDES; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ALTITUDES_LEN, MAVLINK_MSG_ID_ALTITUDES_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ALTITUDES_LEN); -#endif -} - -/** - * @brief Pack a altitudes message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param alt_gps GPS altitude in meters, expressed as * 1000 (millimeters), above MSL - * @param alt_imu IMU altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_barometric barometeric altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_optical_flow Optical flow altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_range_finder Rangefinder Altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_extra Extra altitude above ground in meters, expressed as * 1000 (millimeters) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_altitudes_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,int32_t alt_gps,int32_t alt_imu,int32_t alt_barometric,int32_t alt_optical_flow,int32_t alt_range_finder,int32_t alt_extra) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ALTITUDES_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int32_t(buf, 4, alt_gps); - _mav_put_int32_t(buf, 8, alt_imu); - _mav_put_int32_t(buf, 12, alt_barometric); - _mav_put_int32_t(buf, 16, alt_optical_flow); - _mav_put_int32_t(buf, 20, alt_range_finder); - _mav_put_int32_t(buf, 24, alt_extra); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ALTITUDES_LEN); -#else - mavlink_altitudes_t packet; - packet.time_boot_ms = time_boot_ms; - packet.alt_gps = alt_gps; - packet.alt_imu = alt_imu; - packet.alt_barometric = alt_barometric; - packet.alt_optical_flow = alt_optical_flow; - packet.alt_range_finder = alt_range_finder; - packet.alt_extra = alt_extra; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ALTITUDES_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ALTITUDES; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ALTITUDES_LEN, MAVLINK_MSG_ID_ALTITUDES_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ALTITUDES_LEN); -#endif -} - -/** - * @brief Encode a altitudes struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param altitudes C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_altitudes_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_altitudes_t* altitudes) -{ - return mavlink_msg_altitudes_pack(system_id, component_id, msg, altitudes->time_boot_ms, altitudes->alt_gps, altitudes->alt_imu, altitudes->alt_barometric, altitudes->alt_optical_flow, altitudes->alt_range_finder, altitudes->alt_extra); -} - -/** - * @brief Encode a altitudes struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param altitudes C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_altitudes_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_altitudes_t* altitudes) -{ - return mavlink_msg_altitudes_pack_chan(system_id, component_id, chan, msg, altitudes->time_boot_ms, altitudes->alt_gps, altitudes->alt_imu, altitudes->alt_barometric, altitudes->alt_optical_flow, altitudes->alt_range_finder, altitudes->alt_extra); -} - -/** - * @brief Send a altitudes message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param alt_gps GPS altitude in meters, expressed as * 1000 (millimeters), above MSL - * @param alt_imu IMU altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_barometric barometeric altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_optical_flow Optical flow altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_range_finder Rangefinder Altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_extra Extra altitude above ground in meters, expressed as * 1000 (millimeters) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_altitudes_send(mavlink_channel_t chan, uint32_t time_boot_ms, int32_t alt_gps, int32_t alt_imu, int32_t alt_barometric, int32_t alt_optical_flow, int32_t alt_range_finder, int32_t alt_extra) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ALTITUDES_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int32_t(buf, 4, alt_gps); - _mav_put_int32_t(buf, 8, alt_imu); - _mav_put_int32_t(buf, 12, alt_barometric); - _mav_put_int32_t(buf, 16, alt_optical_flow); - _mav_put_int32_t(buf, 20, alt_range_finder); - _mav_put_int32_t(buf, 24, alt_extra); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, buf, MAVLINK_MSG_ID_ALTITUDES_LEN, MAVLINK_MSG_ID_ALTITUDES_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, buf, MAVLINK_MSG_ID_ALTITUDES_LEN); -#endif -#else - mavlink_altitudes_t packet; - packet.time_boot_ms = time_boot_ms; - packet.alt_gps = alt_gps; - packet.alt_imu = alt_imu; - packet.alt_barometric = alt_barometric; - packet.alt_optical_flow = alt_optical_flow; - packet.alt_range_finder = alt_range_finder; - packet.alt_extra = alt_extra; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, (const char *)&packet, MAVLINK_MSG_ID_ALTITUDES_LEN, MAVLINK_MSG_ID_ALTITUDES_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, (const char *)&packet, MAVLINK_MSG_ID_ALTITUDES_LEN); -#endif -#endif -} - -#endif - -// MESSAGE ALTITUDES UNPACKING - - -/** - * @brief Get field time_boot_ms from altitudes message - * - * @return Timestamp (milliseconds since system boot) - */ -static inline uint32_t mavlink_msg_altitudes_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field alt_gps from altitudes message - * - * @return GPS altitude in meters, expressed as * 1000 (millimeters), above MSL - */ -static inline int32_t mavlink_msg_altitudes_get_alt_gps(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field alt_imu from altitudes message - * - * @return IMU altitude above ground in meters, expressed as * 1000 (millimeters) - */ -static inline int32_t mavlink_msg_altitudes_get_alt_imu(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Get field alt_barometric from altitudes message - * - * @return barometeric altitude above ground in meters, expressed as * 1000 (millimeters) - */ -static inline int32_t mavlink_msg_altitudes_get_alt_barometric(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 12); -} - -/** - * @brief Get field alt_optical_flow from altitudes message - * - * @return Optical flow altitude above ground in meters, expressed as * 1000 (millimeters) - */ -static inline int32_t mavlink_msg_altitudes_get_alt_optical_flow(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 16); -} - -/** - * @brief Get field alt_range_finder from altitudes message - * - * @return Rangefinder Altitude above ground in meters, expressed as * 1000 (millimeters) - */ -static inline int32_t mavlink_msg_altitudes_get_alt_range_finder(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 20); -} - -/** - * @brief Get field alt_extra from altitudes message - * - * @return Extra altitude above ground in meters, expressed as * 1000 (millimeters) - */ -static inline int32_t mavlink_msg_altitudes_get_alt_extra(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 24); -} - -/** - * @brief Decode a altitudes message into a struct - * - * @param msg The message to decode - * @param altitudes C-struct to decode the message contents into - */ -static inline void mavlink_msg_altitudes_decode(const mavlink_message_t* msg, mavlink_altitudes_t* altitudes) -{ -#if MAVLINK_NEED_BYTE_SWAP - altitudes->time_boot_ms = mavlink_msg_altitudes_get_time_boot_ms(msg); - altitudes->alt_gps = mavlink_msg_altitudes_get_alt_gps(msg); - altitudes->alt_imu = mavlink_msg_altitudes_get_alt_imu(msg); - altitudes->alt_barometric = mavlink_msg_altitudes_get_alt_barometric(msg); - altitudes->alt_optical_flow = mavlink_msg_altitudes_get_alt_optical_flow(msg); - altitudes->alt_range_finder = mavlink_msg_altitudes_get_alt_range_finder(msg); - altitudes->alt_extra = mavlink_msg_altitudes_get_alt_extra(msg); -#else - memcpy(altitudes, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ALTITUDES_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h deleted file mode 100644 index 581bd35..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h +++ /dev/null @@ -1,303 +0,0 @@ -// MESSAGE FLEXIFUNCTION_BUFFER_FUNCTION PACKING - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION 152 - -typedef struct __mavlink_flexifunction_buffer_function_t -{ - uint16_t func_index; ///< Function index - uint16_t func_count; ///< Total count of functions - uint16_t data_address; ///< Address in the flexifunction data, Set to 0xFFFF to use address in target memory - uint16_t data_size; ///< Size of the - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - int8_t data[48]; ///< Settings data -} mavlink_flexifunction_buffer_function_t; - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN 58 -#define MAVLINK_MSG_ID_152_LEN 58 - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_CRC 101 -#define MAVLINK_MSG_ID_152_CRC 101 - -#define MAVLINK_MSG_FLEXIFUNCTION_BUFFER_FUNCTION_FIELD_DATA_LEN 48 - -#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_BUFFER_FUNCTION { \ - "FLEXIFUNCTION_BUFFER_FUNCTION", \ - 7, \ - { { "func_index", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_flexifunction_buffer_function_t, func_index) }, \ - { "func_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_flexifunction_buffer_function_t, func_count) }, \ - { "data_address", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_flexifunction_buffer_function_t, data_address) }, \ - { "data_size", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_flexifunction_buffer_function_t, data_size) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_flexifunction_buffer_function_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_flexifunction_buffer_function_t, target_component) }, \ - { "data", NULL, MAVLINK_TYPE_INT8_T, 48, 10, offsetof(mavlink_flexifunction_buffer_function_t, data) }, \ - } \ -} - - -/** - * @brief Pack a flexifunction_buffer_function message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param func_index Function index - * @param func_count Total count of functions - * @param data_address Address in the flexifunction data, Set to 0xFFFF to use address in target memory - * @param data_size Size of the - * @param data Settings data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t func_index, uint16_t func_count, uint16_t data_address, uint16_t data_size, const int8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN]; - _mav_put_uint16_t(buf, 0, func_index); - _mav_put_uint16_t(buf, 2, func_count); - _mav_put_uint16_t(buf, 4, data_address); - _mav_put_uint16_t(buf, 6, data_size); - _mav_put_uint8_t(buf, 8, target_system); - _mav_put_uint8_t(buf, 9, target_component); - _mav_put_int8_t_array(buf, 10, data, 48); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); -#else - mavlink_flexifunction_buffer_function_t packet; - packet.func_index = func_index; - packet.func_count = func_count; - packet.data_address = data_address; - packet.data_size = data_size; - packet.target_system = target_system; - packet.target_component = target_component; - mav_array_memcpy(packet.data, data, sizeof(int8_t)*48); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); -#endif -} - -/** - * @brief Pack a flexifunction_buffer_function message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param func_index Function index - * @param func_count Total count of functions - * @param data_address Address in the flexifunction data, Set to 0xFFFF to use address in target memory - * @param data_size Size of the - * @param data Settings data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t func_index,uint16_t func_count,uint16_t data_address,uint16_t data_size,const int8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN]; - _mav_put_uint16_t(buf, 0, func_index); - _mav_put_uint16_t(buf, 2, func_count); - _mav_put_uint16_t(buf, 4, data_address); - _mav_put_uint16_t(buf, 6, data_size); - _mav_put_uint8_t(buf, 8, target_system); - _mav_put_uint8_t(buf, 9, target_component); - _mav_put_int8_t_array(buf, 10, data, 48); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); -#else - mavlink_flexifunction_buffer_function_t packet; - packet.func_index = func_index; - packet.func_count = func_count; - packet.data_address = data_address; - packet.data_size = data_size; - packet.target_system = target_system; - packet.target_component = target_component; - mav_array_memcpy(packet.data, data, sizeof(int8_t)*48); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); -#endif -} - -/** - * @brief Encode a flexifunction_buffer_function struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param flexifunction_buffer_function C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_buffer_function_t* flexifunction_buffer_function) -{ - return mavlink_msg_flexifunction_buffer_function_pack(system_id, component_id, msg, flexifunction_buffer_function->target_system, flexifunction_buffer_function->target_component, flexifunction_buffer_function->func_index, flexifunction_buffer_function->func_count, flexifunction_buffer_function->data_address, flexifunction_buffer_function->data_size, flexifunction_buffer_function->data); -} - -/** - * @brief Encode a flexifunction_buffer_function struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param flexifunction_buffer_function C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_buffer_function_t* flexifunction_buffer_function) -{ - return mavlink_msg_flexifunction_buffer_function_pack_chan(system_id, component_id, chan, msg, flexifunction_buffer_function->target_system, flexifunction_buffer_function->target_component, flexifunction_buffer_function->func_index, flexifunction_buffer_function->func_count, flexifunction_buffer_function->data_address, flexifunction_buffer_function->data_size, flexifunction_buffer_function->data); -} - -/** - * @brief Send a flexifunction_buffer_function message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param func_index Function index - * @param func_count Total count of functions - * @param data_address Address in the flexifunction data, Set to 0xFFFF to use address in target memory - * @param data_size Size of the - * @param data Settings data - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_flexifunction_buffer_function_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t func_index, uint16_t func_count, uint16_t data_address, uint16_t data_size, const int8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN]; - _mav_put_uint16_t(buf, 0, func_index); - _mav_put_uint16_t(buf, 2, func_count); - _mav_put_uint16_t(buf, 4, data_address); - _mav_put_uint16_t(buf, 6, data_size); - _mav_put_uint8_t(buf, 8, target_system); - _mav_put_uint8_t(buf, 9, target_component); - _mav_put_int8_t_array(buf, 10, data, 48); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); -#endif -#else - mavlink_flexifunction_buffer_function_t packet; - packet.func_index = func_index; - packet.func_count = func_count; - packet.data_address = data_address; - packet.data_size = data_size; - packet.target_system = target_system; - packet.target_component = target_component; - mav_array_memcpy(packet.data, data, sizeof(int8_t)*48); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); -#endif -#endif -} - -#endif - -// MESSAGE FLEXIFUNCTION_BUFFER_FUNCTION UNPACKING - - -/** - * @brief Get field target_system from flexifunction_buffer_function message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_flexifunction_buffer_function_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 8); -} - -/** - * @brief Get field target_component from flexifunction_buffer_function message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_flexifunction_buffer_function_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 9); -} - -/** - * @brief Get field func_index from flexifunction_buffer_function message - * - * @return Function index - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_get_func_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field func_count from flexifunction_buffer_function message - * - * @return Total count of functions - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_get_func_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Get field data_address from flexifunction_buffer_function message - * - * @return Address in the flexifunction data, Set to 0xFFFF to use address in target memory - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_get_data_address(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field data_size from flexifunction_buffer_function message - * - * @return Size of the - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_get_data_size(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Get field data from flexifunction_buffer_function message - * - * @return Settings data - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_get_data(const mavlink_message_t* msg, int8_t *data) -{ - return _MAV_RETURN_int8_t_array(msg, data, 48, 10); -} - -/** - * @brief Decode a flexifunction_buffer_function message into a struct - * - * @param msg The message to decode - * @param flexifunction_buffer_function C-struct to decode the message contents into - */ -static inline void mavlink_msg_flexifunction_buffer_function_decode(const mavlink_message_t* msg, mavlink_flexifunction_buffer_function_t* flexifunction_buffer_function) -{ -#if MAVLINK_NEED_BYTE_SWAP - flexifunction_buffer_function->func_index = mavlink_msg_flexifunction_buffer_function_get_func_index(msg); - flexifunction_buffer_function->func_count = mavlink_msg_flexifunction_buffer_function_get_func_count(msg); - flexifunction_buffer_function->data_address = mavlink_msg_flexifunction_buffer_function_get_data_address(msg); - flexifunction_buffer_function->data_size = mavlink_msg_flexifunction_buffer_function_get_data_size(msg); - flexifunction_buffer_function->target_system = mavlink_msg_flexifunction_buffer_function_get_target_system(msg); - flexifunction_buffer_function->target_component = mavlink_msg_flexifunction_buffer_function_get_target_component(msg); - mavlink_msg_flexifunction_buffer_function_get_data(msg, flexifunction_buffer_function->data); -#else - memcpy(flexifunction_buffer_function, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h deleted file mode 100644 index 790afd5..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h +++ /dev/null @@ -1,243 +0,0 @@ -// MESSAGE FLEXIFUNCTION_BUFFER_FUNCTION_ACK PACKING - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK 153 - -typedef struct __mavlink_flexifunction_buffer_function_ack_t -{ - uint16_t func_index; ///< Function index - uint16_t result; ///< result of acknowledge, 0=fail, 1=good - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_flexifunction_buffer_function_ack_t; - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN 6 -#define MAVLINK_MSG_ID_153_LEN 6 - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_CRC 109 -#define MAVLINK_MSG_ID_153_CRC 109 - - - -#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_BUFFER_FUNCTION_ACK { \ - "FLEXIFUNCTION_BUFFER_FUNCTION_ACK", \ - 4, \ - { { "func_index", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_flexifunction_buffer_function_ack_t, func_index) }, \ - { "result", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_flexifunction_buffer_function_ack_t, result) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_flexifunction_buffer_function_ack_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_flexifunction_buffer_function_ack_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a flexifunction_buffer_function_ack message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param func_index Function index - * @param result result of acknowledge, 0=fail, 1=good - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t func_index, uint16_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN]; - _mav_put_uint16_t(buf, 0, func_index); - _mav_put_uint16_t(buf, 2, result); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); -#else - mavlink_flexifunction_buffer_function_ack_t packet; - packet.func_index = func_index; - packet.result = result; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); -#endif -} - -/** - * @brief Pack a flexifunction_buffer_function_ack message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param func_index Function index - * @param result result of acknowledge, 0=fail, 1=good - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t func_index,uint16_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN]; - _mav_put_uint16_t(buf, 0, func_index); - _mav_put_uint16_t(buf, 2, result); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); -#else - mavlink_flexifunction_buffer_function_ack_t packet; - packet.func_index = func_index; - packet.result = result; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); -#endif -} - -/** - * @brief Encode a flexifunction_buffer_function_ack struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param flexifunction_buffer_function_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_buffer_function_ack_t* flexifunction_buffer_function_ack) -{ - return mavlink_msg_flexifunction_buffer_function_ack_pack(system_id, component_id, msg, flexifunction_buffer_function_ack->target_system, flexifunction_buffer_function_ack->target_component, flexifunction_buffer_function_ack->func_index, flexifunction_buffer_function_ack->result); -} - -/** - * @brief Encode a flexifunction_buffer_function_ack struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param flexifunction_buffer_function_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_buffer_function_ack_t* flexifunction_buffer_function_ack) -{ - return mavlink_msg_flexifunction_buffer_function_ack_pack_chan(system_id, component_id, chan, msg, flexifunction_buffer_function_ack->target_system, flexifunction_buffer_function_ack->target_component, flexifunction_buffer_function_ack->func_index, flexifunction_buffer_function_ack->result); -} - -/** - * @brief Send a flexifunction_buffer_function_ack message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param func_index Function index - * @param result result of acknowledge, 0=fail, 1=good - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_flexifunction_buffer_function_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t func_index, uint16_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN]; - _mav_put_uint16_t(buf, 0, func_index); - _mav_put_uint16_t(buf, 2, result); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); -#endif -#else - mavlink_flexifunction_buffer_function_ack_t packet; - packet.func_index = func_index; - packet.result = result; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); -#endif -#endif -} - -#endif - -// MESSAGE FLEXIFUNCTION_BUFFER_FUNCTION_ACK UNPACKING - - -/** - * @brief Get field target_system from flexifunction_buffer_function_ack message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_flexifunction_buffer_function_ack_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field target_component from flexifunction_buffer_function_ack message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_flexifunction_buffer_function_ack_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field func_index from flexifunction_buffer_function_ack message - * - * @return Function index - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_get_func_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field result from flexifunction_buffer_function_ack message - * - * @return result of acknowledge, 0=fail, 1=good - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_get_result(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Decode a flexifunction_buffer_function_ack message into a struct - * - * @param msg The message to decode - * @param flexifunction_buffer_function_ack C-struct to decode the message contents into - */ -static inline void mavlink_msg_flexifunction_buffer_function_ack_decode(const mavlink_message_t* msg, mavlink_flexifunction_buffer_function_ack_t* flexifunction_buffer_function_ack) -{ -#if MAVLINK_NEED_BYTE_SWAP - flexifunction_buffer_function_ack->func_index = mavlink_msg_flexifunction_buffer_function_ack_get_func_index(msg); - flexifunction_buffer_function_ack->result = mavlink_msg_flexifunction_buffer_function_ack_get_result(msg); - flexifunction_buffer_function_ack->target_system = mavlink_msg_flexifunction_buffer_function_ack_get_target_system(msg); - flexifunction_buffer_function_ack->target_component = mavlink_msg_flexifunction_buffer_function_ack_get_target_component(msg); -#else - memcpy(flexifunction_buffer_function_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h deleted file mode 100644 index ce722c8..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h +++ /dev/null @@ -1,221 +0,0 @@ -// MESSAGE FLEXIFUNCTION_COMMAND PACKING - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND 157 - -typedef struct __mavlink_flexifunction_command_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t command_type; ///< Flexifunction command type -} mavlink_flexifunction_command_t; - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN 3 -#define MAVLINK_MSG_ID_157_LEN 3 - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_CRC 133 -#define MAVLINK_MSG_ID_157_CRC 133 - - - -#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_COMMAND { \ - "FLEXIFUNCTION_COMMAND", \ - 3, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_flexifunction_command_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_flexifunction_command_t, target_component) }, \ - { "command_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_flexifunction_command_t, command_type) }, \ - } \ -} - - -/** - * @brief Pack a flexifunction_command message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param command_type Flexifunction command type - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_command_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t command_type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, command_type); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); -#else - mavlink_flexifunction_command_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.command_type = command_type; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); -#endif -} - -/** - * @brief Pack a flexifunction_command message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param command_type Flexifunction command type - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_command_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t command_type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, command_type); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); -#else - mavlink_flexifunction_command_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.command_type = command_type; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); -#endif -} - -/** - * @brief Encode a flexifunction_command struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param flexifunction_command C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_command_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_command_t* flexifunction_command) -{ - return mavlink_msg_flexifunction_command_pack(system_id, component_id, msg, flexifunction_command->target_system, flexifunction_command->target_component, flexifunction_command->command_type); -} - -/** - * @brief Encode a flexifunction_command struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param flexifunction_command C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_command_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_command_t* flexifunction_command) -{ - return mavlink_msg_flexifunction_command_pack_chan(system_id, component_id, chan, msg, flexifunction_command->target_system, flexifunction_command->target_component, flexifunction_command->command_type); -} - -/** - * @brief Send a flexifunction_command message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param command_type Flexifunction command type - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_flexifunction_command_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t command_type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, command_type); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); -#endif -#else - mavlink_flexifunction_command_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.command_type = command_type; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); -#endif -#endif -} - -#endif - -// MESSAGE FLEXIFUNCTION_COMMAND UNPACKING - - -/** - * @brief Get field target_system from flexifunction_command message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_flexifunction_command_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from flexifunction_command message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_flexifunction_command_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field command_type from flexifunction_command message - * - * @return Flexifunction command type - */ -static inline uint8_t mavlink_msg_flexifunction_command_get_command_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Decode a flexifunction_command message into a struct - * - * @param msg The message to decode - * @param flexifunction_command C-struct to decode the message contents into - */ -static inline void mavlink_msg_flexifunction_command_decode(const mavlink_message_t* msg, mavlink_flexifunction_command_t* flexifunction_command) -{ -#if MAVLINK_NEED_BYTE_SWAP - flexifunction_command->target_system = mavlink_msg_flexifunction_command_get_target_system(msg); - flexifunction_command->target_component = mavlink_msg_flexifunction_command_get_target_component(msg); - flexifunction_command->command_type = mavlink_msg_flexifunction_command_get_command_type(msg); -#else - memcpy(flexifunction_command, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h deleted file mode 100644 index 070dc4b..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h +++ /dev/null @@ -1,199 +0,0 @@ -// MESSAGE FLEXIFUNCTION_COMMAND_ACK PACKING - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK 158 - -typedef struct __mavlink_flexifunction_command_ack_t -{ - uint16_t command_type; ///< Command acknowledged - uint16_t result; ///< result of acknowledge -} mavlink_flexifunction_command_ack_t; - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN 4 -#define MAVLINK_MSG_ID_158_LEN 4 - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_CRC 208 -#define MAVLINK_MSG_ID_158_CRC 208 - - - -#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_COMMAND_ACK { \ - "FLEXIFUNCTION_COMMAND_ACK", \ - 2, \ - { { "command_type", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_flexifunction_command_ack_t, command_type) }, \ - { "result", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_flexifunction_command_ack_t, result) }, \ - } \ -} - - -/** - * @brief Pack a flexifunction_command_ack message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param command_type Command acknowledged - * @param result result of acknowledge - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_command_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t command_type, uint16_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN]; - _mav_put_uint16_t(buf, 0, command_type); - _mav_put_uint16_t(buf, 2, result); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); -#else - mavlink_flexifunction_command_ack_t packet; - packet.command_type = command_type; - packet.result = result; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); -#endif -} - -/** - * @brief Pack a flexifunction_command_ack message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param command_type Command acknowledged - * @param result result of acknowledge - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_command_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t command_type,uint16_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN]; - _mav_put_uint16_t(buf, 0, command_type); - _mav_put_uint16_t(buf, 2, result); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); -#else - mavlink_flexifunction_command_ack_t packet; - packet.command_type = command_type; - packet.result = result; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); -#endif -} - -/** - * @brief Encode a flexifunction_command_ack struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param flexifunction_command_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_command_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_command_ack_t* flexifunction_command_ack) -{ - return mavlink_msg_flexifunction_command_ack_pack(system_id, component_id, msg, flexifunction_command_ack->command_type, flexifunction_command_ack->result); -} - -/** - * @brief Encode a flexifunction_command_ack struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param flexifunction_command_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_command_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_command_ack_t* flexifunction_command_ack) -{ - return mavlink_msg_flexifunction_command_ack_pack_chan(system_id, component_id, chan, msg, flexifunction_command_ack->command_type, flexifunction_command_ack->result); -} - -/** - * @brief Send a flexifunction_command_ack message - * @param chan MAVLink channel to send the message - * - * @param command_type Command acknowledged - * @param result result of acknowledge - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_flexifunction_command_ack_send(mavlink_channel_t chan, uint16_t command_type, uint16_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN]; - _mav_put_uint16_t(buf, 0, command_type); - _mav_put_uint16_t(buf, 2, result); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); -#endif -#else - mavlink_flexifunction_command_ack_t packet; - packet.command_type = command_type; - packet.result = result; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); -#endif -#endif -} - -#endif - -// MESSAGE FLEXIFUNCTION_COMMAND_ACK UNPACKING - - -/** - * @brief Get field command_type from flexifunction_command_ack message - * - * @return Command acknowledged - */ -static inline uint16_t mavlink_msg_flexifunction_command_ack_get_command_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field result from flexifunction_command_ack message - * - * @return result of acknowledge - */ -static inline uint16_t mavlink_msg_flexifunction_command_ack_get_result(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Decode a flexifunction_command_ack message into a struct - * - * @param msg The message to decode - * @param flexifunction_command_ack C-struct to decode the message contents into - */ -static inline void mavlink_msg_flexifunction_command_ack_decode(const mavlink_message_t* msg, mavlink_flexifunction_command_ack_t* flexifunction_command_ack) -{ -#if MAVLINK_NEED_BYTE_SWAP - flexifunction_command_ack->command_type = mavlink_msg_flexifunction_command_ack_get_command_type(msg); - flexifunction_command_ack->result = mavlink_msg_flexifunction_command_ack_get_result(msg); -#else - memcpy(flexifunction_command_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h deleted file mode 100644 index ef262a6..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h +++ /dev/null @@ -1,281 +0,0 @@ -// MESSAGE FLEXIFUNCTION_DIRECTORY PACKING - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY 155 - -typedef struct __mavlink_flexifunction_directory_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t directory_type; ///< 0=inputs, 1=outputs - uint8_t start_index; ///< index of first directory entry to write - uint8_t count; ///< count of directory entries to write - int8_t directory_data[48]; ///< Settings data -} mavlink_flexifunction_directory_t; - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN 53 -#define MAVLINK_MSG_ID_155_LEN 53 - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_CRC 12 -#define MAVLINK_MSG_ID_155_CRC 12 - -#define MAVLINK_MSG_FLEXIFUNCTION_DIRECTORY_FIELD_DIRECTORY_DATA_LEN 48 - -#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_DIRECTORY { \ - "FLEXIFUNCTION_DIRECTORY", \ - 6, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_flexifunction_directory_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_flexifunction_directory_t, target_component) }, \ - { "directory_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_flexifunction_directory_t, directory_type) }, \ - { "start_index", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_flexifunction_directory_t, start_index) }, \ - { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_flexifunction_directory_t, count) }, \ - { "directory_data", NULL, MAVLINK_TYPE_INT8_T, 48, 5, offsetof(mavlink_flexifunction_directory_t, directory_data) }, \ - } \ -} - - -/** - * @brief Pack a flexifunction_directory message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param directory_type 0=inputs, 1=outputs - * @param start_index index of first directory entry to write - * @param count count of directory entries to write - * @param directory_data Settings data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_directory_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t directory_type, uint8_t start_index, uint8_t count, const int8_t *directory_data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, directory_type); - _mav_put_uint8_t(buf, 3, start_index); - _mav_put_uint8_t(buf, 4, count); - _mav_put_int8_t_array(buf, 5, directory_data, 48); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); -#else - mavlink_flexifunction_directory_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.directory_type = directory_type; - packet.start_index = start_index; - packet.count = count; - mav_array_memcpy(packet.directory_data, directory_data, sizeof(int8_t)*48); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); -#endif -} - -/** - * @brief Pack a flexifunction_directory message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param directory_type 0=inputs, 1=outputs - * @param start_index index of first directory entry to write - * @param count count of directory entries to write - * @param directory_data Settings data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_directory_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t directory_type,uint8_t start_index,uint8_t count,const int8_t *directory_data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, directory_type); - _mav_put_uint8_t(buf, 3, start_index); - _mav_put_uint8_t(buf, 4, count); - _mav_put_int8_t_array(buf, 5, directory_data, 48); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); -#else - mavlink_flexifunction_directory_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.directory_type = directory_type; - packet.start_index = start_index; - packet.count = count; - mav_array_memcpy(packet.directory_data, directory_data, sizeof(int8_t)*48); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); -#endif -} - -/** - * @brief Encode a flexifunction_directory struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param flexifunction_directory C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_directory_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_directory_t* flexifunction_directory) -{ - return mavlink_msg_flexifunction_directory_pack(system_id, component_id, msg, flexifunction_directory->target_system, flexifunction_directory->target_component, flexifunction_directory->directory_type, flexifunction_directory->start_index, flexifunction_directory->count, flexifunction_directory->directory_data); -} - -/** - * @brief Encode a flexifunction_directory struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param flexifunction_directory C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_directory_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_directory_t* flexifunction_directory) -{ - return mavlink_msg_flexifunction_directory_pack_chan(system_id, component_id, chan, msg, flexifunction_directory->target_system, flexifunction_directory->target_component, flexifunction_directory->directory_type, flexifunction_directory->start_index, flexifunction_directory->count, flexifunction_directory->directory_data); -} - -/** - * @brief Send a flexifunction_directory message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param directory_type 0=inputs, 1=outputs - * @param start_index index of first directory entry to write - * @param count count of directory entries to write - * @param directory_data Settings data - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_flexifunction_directory_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t directory_type, uint8_t start_index, uint8_t count, const int8_t *directory_data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, directory_type); - _mav_put_uint8_t(buf, 3, start_index); - _mav_put_uint8_t(buf, 4, count); - _mav_put_int8_t_array(buf, 5, directory_data, 48); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); -#endif -#else - mavlink_flexifunction_directory_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.directory_type = directory_type; - packet.start_index = start_index; - packet.count = count; - mav_array_memcpy(packet.directory_data, directory_data, sizeof(int8_t)*48); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); -#endif -#endif -} - -#endif - -// MESSAGE FLEXIFUNCTION_DIRECTORY UNPACKING - - -/** - * @brief Get field target_system from flexifunction_directory message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_flexifunction_directory_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from flexifunction_directory message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_flexifunction_directory_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field directory_type from flexifunction_directory message - * - * @return 0=inputs, 1=outputs - */ -static inline uint8_t mavlink_msg_flexifunction_directory_get_directory_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field start_index from flexifunction_directory message - * - * @return index of first directory entry to write - */ -static inline uint8_t mavlink_msg_flexifunction_directory_get_start_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field count from flexifunction_directory message - * - * @return count of directory entries to write - */ -static inline uint8_t mavlink_msg_flexifunction_directory_get_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field directory_data from flexifunction_directory message - * - * @return Settings data - */ -static inline uint16_t mavlink_msg_flexifunction_directory_get_directory_data(const mavlink_message_t* msg, int8_t *directory_data) -{ - return _MAV_RETURN_int8_t_array(msg, directory_data, 48, 5); -} - -/** - * @brief Decode a flexifunction_directory message into a struct - * - * @param msg The message to decode - * @param flexifunction_directory C-struct to decode the message contents into - */ -static inline void mavlink_msg_flexifunction_directory_decode(const mavlink_message_t* msg, mavlink_flexifunction_directory_t* flexifunction_directory) -{ -#if MAVLINK_NEED_BYTE_SWAP - flexifunction_directory->target_system = mavlink_msg_flexifunction_directory_get_target_system(msg); - flexifunction_directory->target_component = mavlink_msg_flexifunction_directory_get_target_component(msg); - flexifunction_directory->directory_type = mavlink_msg_flexifunction_directory_get_directory_type(msg); - flexifunction_directory->start_index = mavlink_msg_flexifunction_directory_get_start_index(msg); - flexifunction_directory->count = mavlink_msg_flexifunction_directory_get_count(msg); - mavlink_msg_flexifunction_directory_get_directory_data(msg, flexifunction_directory->directory_data); -#else - memcpy(flexifunction_directory, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h deleted file mode 100644 index d3a386c..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h +++ /dev/null @@ -1,287 +0,0 @@ -// MESSAGE FLEXIFUNCTION_DIRECTORY_ACK PACKING - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK 156 - -typedef struct __mavlink_flexifunction_directory_ack_t -{ - uint16_t result; ///< result of acknowledge, 0=fail, 1=good - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t directory_type; ///< 0=inputs, 1=outputs - uint8_t start_index; ///< index of first directory entry to write - uint8_t count; ///< count of directory entries to write -} mavlink_flexifunction_directory_ack_t; - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN 7 -#define MAVLINK_MSG_ID_156_LEN 7 - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_CRC 218 -#define MAVLINK_MSG_ID_156_CRC 218 - - - -#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_DIRECTORY_ACK { \ - "FLEXIFUNCTION_DIRECTORY_ACK", \ - 6, \ - { { "result", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_flexifunction_directory_ack_t, result) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_flexifunction_directory_ack_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_flexifunction_directory_ack_t, target_component) }, \ - { "directory_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_flexifunction_directory_ack_t, directory_type) }, \ - { "start_index", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_flexifunction_directory_ack_t, start_index) }, \ - { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_flexifunction_directory_ack_t, count) }, \ - } \ -} - - -/** - * @brief Pack a flexifunction_directory_ack message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param directory_type 0=inputs, 1=outputs - * @param start_index index of first directory entry to write - * @param count count of directory entries to write - * @param result result of acknowledge, 0=fail, 1=good - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_directory_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t directory_type, uint8_t start_index, uint8_t count, uint16_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN]; - _mav_put_uint16_t(buf, 0, result); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - _mav_put_uint8_t(buf, 4, directory_type); - _mav_put_uint8_t(buf, 5, start_index); - _mav_put_uint8_t(buf, 6, count); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); -#else - mavlink_flexifunction_directory_ack_t packet; - packet.result = result; - packet.target_system = target_system; - packet.target_component = target_component; - packet.directory_type = directory_type; - packet.start_index = start_index; - packet.count = count; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); -#endif -} - -/** - * @brief Pack a flexifunction_directory_ack message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param directory_type 0=inputs, 1=outputs - * @param start_index index of first directory entry to write - * @param count count of directory entries to write - * @param result result of acknowledge, 0=fail, 1=good - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_directory_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t directory_type,uint8_t start_index,uint8_t count,uint16_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN]; - _mav_put_uint16_t(buf, 0, result); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - _mav_put_uint8_t(buf, 4, directory_type); - _mav_put_uint8_t(buf, 5, start_index); - _mav_put_uint8_t(buf, 6, count); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); -#else - mavlink_flexifunction_directory_ack_t packet; - packet.result = result; - packet.target_system = target_system; - packet.target_component = target_component; - packet.directory_type = directory_type; - packet.start_index = start_index; - packet.count = count; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); -#endif -} - -/** - * @brief Encode a flexifunction_directory_ack struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param flexifunction_directory_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_directory_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_directory_ack_t* flexifunction_directory_ack) -{ - return mavlink_msg_flexifunction_directory_ack_pack(system_id, component_id, msg, flexifunction_directory_ack->target_system, flexifunction_directory_ack->target_component, flexifunction_directory_ack->directory_type, flexifunction_directory_ack->start_index, flexifunction_directory_ack->count, flexifunction_directory_ack->result); -} - -/** - * @brief Encode a flexifunction_directory_ack struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param flexifunction_directory_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_directory_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_directory_ack_t* flexifunction_directory_ack) -{ - return mavlink_msg_flexifunction_directory_ack_pack_chan(system_id, component_id, chan, msg, flexifunction_directory_ack->target_system, flexifunction_directory_ack->target_component, flexifunction_directory_ack->directory_type, flexifunction_directory_ack->start_index, flexifunction_directory_ack->count, flexifunction_directory_ack->result); -} - -/** - * @brief Send a flexifunction_directory_ack message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param directory_type 0=inputs, 1=outputs - * @param start_index index of first directory entry to write - * @param count count of directory entries to write - * @param result result of acknowledge, 0=fail, 1=good - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_flexifunction_directory_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t directory_type, uint8_t start_index, uint8_t count, uint16_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN]; - _mav_put_uint16_t(buf, 0, result); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - _mav_put_uint8_t(buf, 4, directory_type); - _mav_put_uint8_t(buf, 5, start_index); - _mav_put_uint8_t(buf, 6, count); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); -#endif -#else - mavlink_flexifunction_directory_ack_t packet; - packet.result = result; - packet.target_system = target_system; - packet.target_component = target_component; - packet.directory_type = directory_type; - packet.start_index = start_index; - packet.count = count; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); -#endif -#endif -} - -#endif - -// MESSAGE FLEXIFUNCTION_DIRECTORY_ACK UNPACKING - - -/** - * @brief Get field target_system from flexifunction_directory_ack message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_flexifunction_directory_ack_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field target_component from flexifunction_directory_ack message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_flexifunction_directory_ack_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field directory_type from flexifunction_directory_ack message - * - * @return 0=inputs, 1=outputs - */ -static inline uint8_t mavlink_msg_flexifunction_directory_ack_get_directory_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field start_index from flexifunction_directory_ack message - * - * @return index of first directory entry to write - */ -static inline uint8_t mavlink_msg_flexifunction_directory_ack_get_start_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field count from flexifunction_directory_ack message - * - * @return count of directory entries to write - */ -static inline uint8_t mavlink_msg_flexifunction_directory_ack_get_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @brief Get field result from flexifunction_directory_ack message - * - * @return result of acknowledge, 0=fail, 1=good - */ -static inline uint16_t mavlink_msg_flexifunction_directory_ack_get_result(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Decode a flexifunction_directory_ack message into a struct - * - * @param msg The message to decode - * @param flexifunction_directory_ack C-struct to decode the message contents into - */ -static inline void mavlink_msg_flexifunction_directory_ack_decode(const mavlink_message_t* msg, mavlink_flexifunction_directory_ack_t* flexifunction_directory_ack) -{ -#if MAVLINK_NEED_BYTE_SWAP - flexifunction_directory_ack->result = mavlink_msg_flexifunction_directory_ack_get_result(msg); - flexifunction_directory_ack->target_system = mavlink_msg_flexifunction_directory_ack_get_target_system(msg); - flexifunction_directory_ack->target_component = mavlink_msg_flexifunction_directory_ack_get_target_component(msg); - flexifunction_directory_ack->directory_type = mavlink_msg_flexifunction_directory_ack_get_directory_type(msg); - flexifunction_directory_ack->start_index = mavlink_msg_flexifunction_directory_ack_get_start_index(msg); - flexifunction_directory_ack->count = mavlink_msg_flexifunction_directory_ack_get_count(msg); -#else - memcpy(flexifunction_directory_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h deleted file mode 100644 index e50f77b..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h +++ /dev/null @@ -1,243 +0,0 @@ -// MESSAGE FLEXIFUNCTION_READ_REQ PACKING - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ 151 - -typedef struct __mavlink_flexifunction_read_req_t -{ - int16_t read_req_type; ///< Type of flexifunction data requested - int16_t data_index; ///< index into data where needed - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_flexifunction_read_req_t; - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN 6 -#define MAVLINK_MSG_ID_151_LEN 6 - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_CRC 26 -#define MAVLINK_MSG_ID_151_CRC 26 - - - -#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_READ_REQ { \ - "FLEXIFUNCTION_READ_REQ", \ - 4, \ - { { "read_req_type", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_flexifunction_read_req_t, read_req_type) }, \ - { "data_index", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_flexifunction_read_req_t, data_index) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_flexifunction_read_req_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_flexifunction_read_req_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a flexifunction_read_req message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param read_req_type Type of flexifunction data requested - * @param data_index index into data where needed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_read_req_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, int16_t read_req_type, int16_t data_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN]; - _mav_put_int16_t(buf, 0, read_req_type); - _mav_put_int16_t(buf, 2, data_index); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); -#else - mavlink_flexifunction_read_req_t packet; - packet.read_req_type = read_req_type; - packet.data_index = data_index; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); -#endif -} - -/** - * @brief Pack a flexifunction_read_req message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param read_req_type Type of flexifunction data requested - * @param data_index index into data where needed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_read_req_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,int16_t read_req_type,int16_t data_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN]; - _mav_put_int16_t(buf, 0, read_req_type); - _mav_put_int16_t(buf, 2, data_index); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); -#else - mavlink_flexifunction_read_req_t packet; - packet.read_req_type = read_req_type; - packet.data_index = data_index; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); -#endif -} - -/** - * @brief Encode a flexifunction_read_req struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param flexifunction_read_req C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_read_req_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_read_req_t* flexifunction_read_req) -{ - return mavlink_msg_flexifunction_read_req_pack(system_id, component_id, msg, flexifunction_read_req->target_system, flexifunction_read_req->target_component, flexifunction_read_req->read_req_type, flexifunction_read_req->data_index); -} - -/** - * @brief Encode a flexifunction_read_req struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param flexifunction_read_req C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_read_req_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_read_req_t* flexifunction_read_req) -{ - return mavlink_msg_flexifunction_read_req_pack_chan(system_id, component_id, chan, msg, flexifunction_read_req->target_system, flexifunction_read_req->target_component, flexifunction_read_req->read_req_type, flexifunction_read_req->data_index); -} - -/** - * @brief Send a flexifunction_read_req message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param read_req_type Type of flexifunction data requested - * @param data_index index into data where needed - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_flexifunction_read_req_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t read_req_type, int16_t data_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN]; - _mav_put_int16_t(buf, 0, read_req_type); - _mav_put_int16_t(buf, 2, data_index); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); -#endif -#else - mavlink_flexifunction_read_req_t packet; - packet.read_req_type = read_req_type; - packet.data_index = data_index; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); -#endif -#endif -} - -#endif - -// MESSAGE FLEXIFUNCTION_READ_REQ UNPACKING - - -/** - * @brief Get field target_system from flexifunction_read_req message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_flexifunction_read_req_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field target_component from flexifunction_read_req message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_flexifunction_read_req_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field read_req_type from flexifunction_read_req message - * - * @return Type of flexifunction data requested - */ -static inline int16_t mavlink_msg_flexifunction_read_req_get_read_req_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 0); -} - -/** - * @brief Get field data_index from flexifunction_read_req message - * - * @return index into data where needed - */ -static inline int16_t mavlink_msg_flexifunction_read_req_get_data_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 2); -} - -/** - * @brief Decode a flexifunction_read_req message into a struct - * - * @param msg The message to decode - * @param flexifunction_read_req C-struct to decode the message contents into - */ -static inline void mavlink_msg_flexifunction_read_req_decode(const mavlink_message_t* msg, mavlink_flexifunction_read_req_t* flexifunction_read_req) -{ -#if MAVLINK_NEED_BYTE_SWAP - flexifunction_read_req->read_req_type = mavlink_msg_flexifunction_read_req_get_read_req_type(msg); - flexifunction_read_req->data_index = mavlink_msg_flexifunction_read_req_get_data_index(msg); - flexifunction_read_req->target_system = mavlink_msg_flexifunction_read_req_get_target_system(msg); - flexifunction_read_req->target_component = mavlink_msg_flexifunction_read_req_get_target_component(msg); -#else - memcpy(flexifunction_read_req, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h deleted file mode 100644 index 41afb38..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h +++ /dev/null @@ -1,199 +0,0 @@ -// MESSAGE FLEXIFUNCTION_SET PACKING - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_SET 150 - -typedef struct __mavlink_flexifunction_set_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_flexifunction_set_t; - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN 2 -#define MAVLINK_MSG_ID_150_LEN 2 - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_SET_CRC 181 -#define MAVLINK_MSG_ID_150_CRC 181 - - - -#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_SET { \ - "FLEXIFUNCTION_SET", \ - 2, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_flexifunction_set_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_flexifunction_set_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a flexifunction_set message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_SET_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_FLEXIFUNCTION_SET_LEN); -#else - mavlink_flexifunction_set_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_SET; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); -#endif -} - -/** - * @brief Pack a flexifunction_set message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_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) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_SET_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_FLEXIFUNCTION_SET_LEN); -#else - mavlink_flexifunction_set_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_SET; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); -#endif -} - -/** - * @brief Encode a flexifunction_set struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param flexifunction_set C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_set_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_set_t* flexifunction_set) -{ - return mavlink_msg_flexifunction_set_pack(system_id, component_id, msg, flexifunction_set->target_system, flexifunction_set->target_component); -} - -/** - * @brief Encode a flexifunction_set struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param flexifunction_set C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_set_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_set_t* flexifunction_set) -{ - return mavlink_msg_flexifunction_set_pack_chan(system_id, component_id, chan, msg, flexifunction_set->target_system, flexifunction_set->target_component); -} - -/** - * @brief Send a flexifunction_set message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_flexifunction_set_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_FLEXIFUNCTION_SET_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_FLEXIFUNCTION_SET, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); -#endif -#else - mavlink_flexifunction_set_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_FLEXIFUNCTION_SET, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); -#endif -#endif -} - -#endif - -// MESSAGE FLEXIFUNCTION_SET UNPACKING - - -/** - * @brief Get field target_system from flexifunction_set message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_flexifunction_set_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from flexifunction_set message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_flexifunction_set_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Decode a flexifunction_set message into a struct - * - * @param msg The message to decode - * @param flexifunction_set C-struct to decode the message contents into - */ -static inline void mavlink_msg_flexifunction_set_decode(const mavlink_message_t* msg, mavlink_flexifunction_set_t* flexifunction_set) -{ -#if MAVLINK_NEED_BYTE_SWAP - flexifunction_set->target_system = mavlink_msg_flexifunction_set_get_target_system(msg); - flexifunction_set->target_component = mavlink_msg_flexifunction_set_get_target_component(msg); -#else - memcpy(flexifunction_set, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h deleted file mode 100644 index d21ab48..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h +++ /dev/null @@ -1,243 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F13 PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13 177 - -typedef struct __mavlink_serial_udb_extra_f13_t -{ - int32_t sue_lat_origin; ///< Serial UDB Extra MP Origin Latitude - int32_t sue_lon_origin; ///< Serial UDB Extra MP Origin Longitude - int32_t sue_alt_origin; ///< Serial UDB Extra MP Origin Altitude Above Sea Level - int16_t sue_week_no; ///< Serial UDB Extra GPS Week Number -} mavlink_serial_udb_extra_f13_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN 14 -#define MAVLINK_MSG_ID_177_LEN 14 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_CRC 249 -#define MAVLINK_MSG_ID_177_CRC 249 - - - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F13 { \ - "SERIAL_UDB_EXTRA_F13", \ - 4, \ - { { "sue_lat_origin", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_serial_udb_extra_f13_t, sue_lat_origin) }, \ - { "sue_lon_origin", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_serial_udb_extra_f13_t, sue_lon_origin) }, \ - { "sue_alt_origin", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_serial_udb_extra_f13_t, sue_alt_origin) }, \ - { "sue_week_no", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_serial_udb_extra_f13_t, sue_week_no) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f13 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_week_no Serial UDB Extra GPS Week Number - * @param sue_lat_origin Serial UDB Extra MP Origin Latitude - * @param sue_lon_origin Serial UDB Extra MP Origin Longitude - * @param sue_alt_origin Serial UDB Extra MP Origin Altitude Above Sea Level - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f13_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - int16_t sue_week_no, int32_t sue_lat_origin, int32_t sue_lon_origin, int32_t sue_alt_origin) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN]; - _mav_put_int32_t(buf, 0, sue_lat_origin); - _mav_put_int32_t(buf, 4, sue_lon_origin); - _mav_put_int32_t(buf, 8, sue_alt_origin); - _mav_put_int16_t(buf, 12, sue_week_no); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); -#else - mavlink_serial_udb_extra_f13_t packet; - packet.sue_lat_origin = sue_lat_origin; - packet.sue_lon_origin = sue_lon_origin; - packet.sue_alt_origin = sue_alt_origin; - packet.sue_week_no = sue_week_no; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f13 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_week_no Serial UDB Extra GPS Week Number - * @param sue_lat_origin Serial UDB Extra MP Origin Latitude - * @param sue_lon_origin Serial UDB Extra MP Origin Longitude - * @param sue_alt_origin Serial UDB Extra MP Origin Altitude Above Sea Level - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f13_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - int16_t sue_week_no,int32_t sue_lat_origin,int32_t sue_lon_origin,int32_t sue_alt_origin) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN]; - _mav_put_int32_t(buf, 0, sue_lat_origin); - _mav_put_int32_t(buf, 4, sue_lon_origin); - _mav_put_int32_t(buf, 8, sue_alt_origin); - _mav_put_int16_t(buf, 12, sue_week_no); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); -#else - mavlink_serial_udb_extra_f13_t packet; - packet.sue_lat_origin = sue_lat_origin; - packet.sue_lon_origin = sue_lon_origin; - packet.sue_alt_origin = sue_alt_origin; - packet.sue_week_no = sue_week_no; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f13 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f13 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f13_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f13_t* serial_udb_extra_f13) -{ - return mavlink_msg_serial_udb_extra_f13_pack(system_id, component_id, msg, serial_udb_extra_f13->sue_week_no, serial_udb_extra_f13->sue_lat_origin, serial_udb_extra_f13->sue_lon_origin, serial_udb_extra_f13->sue_alt_origin); -} - -/** - * @brief Encode a serial_udb_extra_f13 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f13 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f13_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f13_t* serial_udb_extra_f13) -{ - return mavlink_msg_serial_udb_extra_f13_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f13->sue_week_no, serial_udb_extra_f13->sue_lat_origin, serial_udb_extra_f13->sue_lon_origin, serial_udb_extra_f13->sue_alt_origin); -} - -/** - * @brief Send a serial_udb_extra_f13 message - * @param chan MAVLink channel to send the message - * - * @param sue_week_no Serial UDB Extra GPS Week Number - * @param sue_lat_origin Serial UDB Extra MP Origin Latitude - * @param sue_lon_origin Serial UDB Extra MP Origin Longitude - * @param sue_alt_origin Serial UDB Extra MP Origin Altitude Above Sea Level - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f13_send(mavlink_channel_t chan, int16_t sue_week_no, int32_t sue_lat_origin, int32_t sue_lon_origin, int32_t sue_alt_origin) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN]; - _mav_put_int32_t(buf, 0, sue_lat_origin); - _mav_put_int32_t(buf, 4, sue_lon_origin); - _mav_put_int32_t(buf, 8, sue_alt_origin); - _mav_put_int16_t(buf, 12, sue_week_no); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); -#endif -#else - mavlink_serial_udb_extra_f13_t packet; - packet.sue_lat_origin = sue_lat_origin; - packet.sue_lon_origin = sue_lon_origin; - packet.sue_alt_origin = sue_alt_origin; - packet.sue_week_no = sue_week_no; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F13 UNPACKING - - -/** - * @brief Get field sue_week_no from serial_udb_extra_f13 message - * - * @return Serial UDB Extra GPS Week Number - */ -static inline int16_t mavlink_msg_serial_udb_extra_f13_get_sue_week_no(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 12); -} - -/** - * @brief Get field sue_lat_origin from serial_udb_extra_f13 message - * - * @return Serial UDB Extra MP Origin Latitude - */ -static inline int32_t mavlink_msg_serial_udb_extra_f13_get_sue_lat_origin(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field sue_lon_origin from serial_udb_extra_f13 message - * - * @return Serial UDB Extra MP Origin Longitude - */ -static inline int32_t mavlink_msg_serial_udb_extra_f13_get_sue_lon_origin(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field sue_alt_origin from serial_udb_extra_f13 message - * - * @return Serial UDB Extra MP Origin Altitude Above Sea Level - */ -static inline int32_t mavlink_msg_serial_udb_extra_f13_get_sue_alt_origin(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Decode a serial_udb_extra_f13 message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f13 C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f13_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f13_t* serial_udb_extra_f13) -{ -#if MAVLINK_NEED_BYTE_SWAP - serial_udb_extra_f13->sue_lat_origin = mavlink_msg_serial_udb_extra_f13_get_sue_lat_origin(msg); - serial_udb_extra_f13->sue_lon_origin = mavlink_msg_serial_udb_extra_f13_get_sue_lon_origin(msg); - serial_udb_extra_f13->sue_alt_origin = mavlink_msg_serial_udb_extra_f13_get_sue_alt_origin(msg); - serial_udb_extra_f13->sue_week_no = mavlink_msg_serial_udb_extra_f13_get_sue_week_no(msg); -#else - memcpy(serial_udb_extra_f13, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h deleted file mode 100644 index 6ac12f2..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h +++ /dev/null @@ -1,397 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F14 PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14 178 - -typedef struct __mavlink_serial_udb_extra_f14_t -{ - uint32_t sue_TRAP_SOURCE; ///< Serial UDB Extra Type Program Address of Last Trap - int16_t sue_RCON; ///< Serial UDB Extra Reboot Regitster of DSPIC - int16_t sue_TRAP_FLAGS; ///< Serial UDB Extra Last dspic Trap Flags - int16_t sue_osc_fail_count; ///< Serial UDB Extra Number of Ocillator Failures - uint8_t sue_WIND_ESTIMATION; ///< Serial UDB Extra Wind Estimation Enabled - uint8_t sue_GPS_TYPE; ///< Serial UDB Extra Type of GPS Unit - uint8_t sue_DR; ///< Serial UDB Extra Dead Reckoning Enabled - uint8_t sue_BOARD_TYPE; ///< Serial UDB Extra Type of UDB Hardware - uint8_t sue_AIRFRAME; ///< Serial UDB Extra Type of Airframe - uint8_t sue_CLOCK_CONFIG; ///< Serial UDB Extra UDB Internal Clock Configuration - uint8_t sue_FLIGHT_PLAN_TYPE; ///< Serial UDB Extra Type of Flight Plan -} mavlink_serial_udb_extra_f14_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN 17 -#define MAVLINK_MSG_ID_178_LEN 17 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_CRC 123 -#define MAVLINK_MSG_ID_178_CRC 123 - - - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F14 { \ - "SERIAL_UDB_EXTRA_F14", \ - 11, \ - { { "sue_TRAP_SOURCE", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_serial_udb_extra_f14_t, sue_TRAP_SOURCE) }, \ - { "sue_RCON", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_serial_udb_extra_f14_t, sue_RCON) }, \ - { "sue_TRAP_FLAGS", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_serial_udb_extra_f14_t, sue_TRAP_FLAGS) }, \ - { "sue_osc_fail_count", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_serial_udb_extra_f14_t, sue_osc_fail_count) }, \ - { "sue_WIND_ESTIMATION", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_serial_udb_extra_f14_t, sue_WIND_ESTIMATION) }, \ - { "sue_GPS_TYPE", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_serial_udb_extra_f14_t, sue_GPS_TYPE) }, \ - { "sue_DR", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_serial_udb_extra_f14_t, sue_DR) }, \ - { "sue_BOARD_TYPE", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_serial_udb_extra_f14_t, sue_BOARD_TYPE) }, \ - { "sue_AIRFRAME", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_serial_udb_extra_f14_t, sue_AIRFRAME) }, \ - { "sue_CLOCK_CONFIG", NULL, MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_serial_udb_extra_f14_t, sue_CLOCK_CONFIG) }, \ - { "sue_FLIGHT_PLAN_TYPE", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_serial_udb_extra_f14_t, sue_FLIGHT_PLAN_TYPE) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f14 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_WIND_ESTIMATION Serial UDB Extra Wind Estimation Enabled - * @param sue_GPS_TYPE Serial UDB Extra Type of GPS Unit - * @param sue_DR Serial UDB Extra Dead Reckoning Enabled - * @param sue_BOARD_TYPE Serial UDB Extra Type of UDB Hardware - * @param sue_AIRFRAME Serial UDB Extra Type of Airframe - * @param sue_RCON Serial UDB Extra Reboot Regitster of DSPIC - * @param sue_TRAP_FLAGS Serial UDB Extra Last dspic Trap Flags - * @param sue_TRAP_SOURCE Serial UDB Extra Type Program Address of Last Trap - * @param sue_osc_fail_count Serial UDB Extra Number of Ocillator Failures - * @param sue_CLOCK_CONFIG Serial UDB Extra UDB Internal Clock Configuration - * @param sue_FLIGHT_PLAN_TYPE Serial UDB Extra Type of Flight Plan - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f14_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t sue_WIND_ESTIMATION, uint8_t sue_GPS_TYPE, uint8_t sue_DR, uint8_t sue_BOARD_TYPE, uint8_t sue_AIRFRAME, int16_t sue_RCON, int16_t sue_TRAP_FLAGS, uint32_t sue_TRAP_SOURCE, int16_t sue_osc_fail_count, uint8_t sue_CLOCK_CONFIG, uint8_t sue_FLIGHT_PLAN_TYPE) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN]; - _mav_put_uint32_t(buf, 0, sue_TRAP_SOURCE); - _mav_put_int16_t(buf, 4, sue_RCON); - _mav_put_int16_t(buf, 6, sue_TRAP_FLAGS); - _mav_put_int16_t(buf, 8, sue_osc_fail_count); - _mav_put_uint8_t(buf, 10, sue_WIND_ESTIMATION); - _mav_put_uint8_t(buf, 11, sue_GPS_TYPE); - _mav_put_uint8_t(buf, 12, sue_DR); - _mav_put_uint8_t(buf, 13, sue_BOARD_TYPE); - _mav_put_uint8_t(buf, 14, sue_AIRFRAME); - _mav_put_uint8_t(buf, 15, sue_CLOCK_CONFIG); - _mav_put_uint8_t(buf, 16, sue_FLIGHT_PLAN_TYPE); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); -#else - mavlink_serial_udb_extra_f14_t packet; - packet.sue_TRAP_SOURCE = sue_TRAP_SOURCE; - packet.sue_RCON = sue_RCON; - packet.sue_TRAP_FLAGS = sue_TRAP_FLAGS; - packet.sue_osc_fail_count = sue_osc_fail_count; - packet.sue_WIND_ESTIMATION = sue_WIND_ESTIMATION; - packet.sue_GPS_TYPE = sue_GPS_TYPE; - packet.sue_DR = sue_DR; - packet.sue_BOARD_TYPE = sue_BOARD_TYPE; - packet.sue_AIRFRAME = sue_AIRFRAME; - packet.sue_CLOCK_CONFIG = sue_CLOCK_CONFIG; - packet.sue_FLIGHT_PLAN_TYPE = sue_FLIGHT_PLAN_TYPE; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f14 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_WIND_ESTIMATION Serial UDB Extra Wind Estimation Enabled - * @param sue_GPS_TYPE Serial UDB Extra Type of GPS Unit - * @param sue_DR Serial UDB Extra Dead Reckoning Enabled - * @param sue_BOARD_TYPE Serial UDB Extra Type of UDB Hardware - * @param sue_AIRFRAME Serial UDB Extra Type of Airframe - * @param sue_RCON Serial UDB Extra Reboot Regitster of DSPIC - * @param sue_TRAP_FLAGS Serial UDB Extra Last dspic Trap Flags - * @param sue_TRAP_SOURCE Serial UDB Extra Type Program Address of Last Trap - * @param sue_osc_fail_count Serial UDB Extra Number of Ocillator Failures - * @param sue_CLOCK_CONFIG Serial UDB Extra UDB Internal Clock Configuration - * @param sue_FLIGHT_PLAN_TYPE Serial UDB Extra Type of Flight Plan - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f14_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t sue_WIND_ESTIMATION,uint8_t sue_GPS_TYPE,uint8_t sue_DR,uint8_t sue_BOARD_TYPE,uint8_t sue_AIRFRAME,int16_t sue_RCON,int16_t sue_TRAP_FLAGS,uint32_t sue_TRAP_SOURCE,int16_t sue_osc_fail_count,uint8_t sue_CLOCK_CONFIG,uint8_t sue_FLIGHT_PLAN_TYPE) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN]; - _mav_put_uint32_t(buf, 0, sue_TRAP_SOURCE); - _mav_put_int16_t(buf, 4, sue_RCON); - _mav_put_int16_t(buf, 6, sue_TRAP_FLAGS); - _mav_put_int16_t(buf, 8, sue_osc_fail_count); - _mav_put_uint8_t(buf, 10, sue_WIND_ESTIMATION); - _mav_put_uint8_t(buf, 11, sue_GPS_TYPE); - _mav_put_uint8_t(buf, 12, sue_DR); - _mav_put_uint8_t(buf, 13, sue_BOARD_TYPE); - _mav_put_uint8_t(buf, 14, sue_AIRFRAME); - _mav_put_uint8_t(buf, 15, sue_CLOCK_CONFIG); - _mav_put_uint8_t(buf, 16, sue_FLIGHT_PLAN_TYPE); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); -#else - mavlink_serial_udb_extra_f14_t packet; - packet.sue_TRAP_SOURCE = sue_TRAP_SOURCE; - packet.sue_RCON = sue_RCON; - packet.sue_TRAP_FLAGS = sue_TRAP_FLAGS; - packet.sue_osc_fail_count = sue_osc_fail_count; - packet.sue_WIND_ESTIMATION = sue_WIND_ESTIMATION; - packet.sue_GPS_TYPE = sue_GPS_TYPE; - packet.sue_DR = sue_DR; - packet.sue_BOARD_TYPE = sue_BOARD_TYPE; - packet.sue_AIRFRAME = sue_AIRFRAME; - packet.sue_CLOCK_CONFIG = sue_CLOCK_CONFIG; - packet.sue_FLIGHT_PLAN_TYPE = sue_FLIGHT_PLAN_TYPE; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f14 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f14 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f14_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f14_t* serial_udb_extra_f14) -{ - return mavlink_msg_serial_udb_extra_f14_pack(system_id, component_id, msg, serial_udb_extra_f14->sue_WIND_ESTIMATION, serial_udb_extra_f14->sue_GPS_TYPE, serial_udb_extra_f14->sue_DR, serial_udb_extra_f14->sue_BOARD_TYPE, serial_udb_extra_f14->sue_AIRFRAME, serial_udb_extra_f14->sue_RCON, serial_udb_extra_f14->sue_TRAP_FLAGS, serial_udb_extra_f14->sue_TRAP_SOURCE, serial_udb_extra_f14->sue_osc_fail_count, serial_udb_extra_f14->sue_CLOCK_CONFIG, serial_udb_extra_f14->sue_FLIGHT_PLAN_TYPE); -} - -/** - * @brief Encode a serial_udb_extra_f14 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f14 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f14_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f14_t* serial_udb_extra_f14) -{ - return mavlink_msg_serial_udb_extra_f14_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f14->sue_WIND_ESTIMATION, serial_udb_extra_f14->sue_GPS_TYPE, serial_udb_extra_f14->sue_DR, serial_udb_extra_f14->sue_BOARD_TYPE, serial_udb_extra_f14->sue_AIRFRAME, serial_udb_extra_f14->sue_RCON, serial_udb_extra_f14->sue_TRAP_FLAGS, serial_udb_extra_f14->sue_TRAP_SOURCE, serial_udb_extra_f14->sue_osc_fail_count, serial_udb_extra_f14->sue_CLOCK_CONFIG, serial_udb_extra_f14->sue_FLIGHT_PLAN_TYPE); -} - -/** - * @brief Send a serial_udb_extra_f14 message - * @param chan MAVLink channel to send the message - * - * @param sue_WIND_ESTIMATION Serial UDB Extra Wind Estimation Enabled - * @param sue_GPS_TYPE Serial UDB Extra Type of GPS Unit - * @param sue_DR Serial UDB Extra Dead Reckoning Enabled - * @param sue_BOARD_TYPE Serial UDB Extra Type of UDB Hardware - * @param sue_AIRFRAME Serial UDB Extra Type of Airframe - * @param sue_RCON Serial UDB Extra Reboot Regitster of DSPIC - * @param sue_TRAP_FLAGS Serial UDB Extra Last dspic Trap Flags - * @param sue_TRAP_SOURCE Serial UDB Extra Type Program Address of Last Trap - * @param sue_osc_fail_count Serial UDB Extra Number of Ocillator Failures - * @param sue_CLOCK_CONFIG Serial UDB Extra UDB Internal Clock Configuration - * @param sue_FLIGHT_PLAN_TYPE Serial UDB Extra Type of Flight Plan - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f14_send(mavlink_channel_t chan, uint8_t sue_WIND_ESTIMATION, uint8_t sue_GPS_TYPE, uint8_t sue_DR, uint8_t sue_BOARD_TYPE, uint8_t sue_AIRFRAME, int16_t sue_RCON, int16_t sue_TRAP_FLAGS, uint32_t sue_TRAP_SOURCE, int16_t sue_osc_fail_count, uint8_t sue_CLOCK_CONFIG, uint8_t sue_FLIGHT_PLAN_TYPE) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN]; - _mav_put_uint32_t(buf, 0, sue_TRAP_SOURCE); - _mav_put_int16_t(buf, 4, sue_RCON); - _mav_put_int16_t(buf, 6, sue_TRAP_FLAGS); - _mav_put_int16_t(buf, 8, sue_osc_fail_count); - _mav_put_uint8_t(buf, 10, sue_WIND_ESTIMATION); - _mav_put_uint8_t(buf, 11, sue_GPS_TYPE); - _mav_put_uint8_t(buf, 12, sue_DR); - _mav_put_uint8_t(buf, 13, sue_BOARD_TYPE); - _mav_put_uint8_t(buf, 14, sue_AIRFRAME); - _mav_put_uint8_t(buf, 15, sue_CLOCK_CONFIG); - _mav_put_uint8_t(buf, 16, sue_FLIGHT_PLAN_TYPE); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); -#endif -#else - mavlink_serial_udb_extra_f14_t packet; - packet.sue_TRAP_SOURCE = sue_TRAP_SOURCE; - packet.sue_RCON = sue_RCON; - packet.sue_TRAP_FLAGS = sue_TRAP_FLAGS; - packet.sue_osc_fail_count = sue_osc_fail_count; - packet.sue_WIND_ESTIMATION = sue_WIND_ESTIMATION; - packet.sue_GPS_TYPE = sue_GPS_TYPE; - packet.sue_DR = sue_DR; - packet.sue_BOARD_TYPE = sue_BOARD_TYPE; - packet.sue_AIRFRAME = sue_AIRFRAME; - packet.sue_CLOCK_CONFIG = sue_CLOCK_CONFIG; - packet.sue_FLIGHT_PLAN_TYPE = sue_FLIGHT_PLAN_TYPE; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F14 UNPACKING - - -/** - * @brief Get field sue_WIND_ESTIMATION from serial_udb_extra_f14 message - * - * @return Serial UDB Extra Wind Estimation Enabled - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f14_get_sue_WIND_ESTIMATION(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 10); -} - -/** - * @brief Get field sue_GPS_TYPE from serial_udb_extra_f14 message - * - * @return Serial UDB Extra Type of GPS Unit - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f14_get_sue_GPS_TYPE(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 11); -} - -/** - * @brief Get field sue_DR from serial_udb_extra_f14 message - * - * @return Serial UDB Extra Dead Reckoning Enabled - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f14_get_sue_DR(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 12); -} - -/** - * @brief Get field sue_BOARD_TYPE from serial_udb_extra_f14 message - * - * @return Serial UDB Extra Type of UDB Hardware - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f14_get_sue_BOARD_TYPE(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 13); -} - -/** - * @brief Get field sue_AIRFRAME from serial_udb_extra_f14 message - * - * @return Serial UDB Extra Type of Airframe - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f14_get_sue_AIRFRAME(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 14); -} - -/** - * @brief Get field sue_RCON from serial_udb_extra_f14 message - * - * @return Serial UDB Extra Reboot Regitster of DSPIC - */ -static inline int16_t mavlink_msg_serial_udb_extra_f14_get_sue_RCON(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 4); -} - -/** - * @brief Get field sue_TRAP_FLAGS from serial_udb_extra_f14 message - * - * @return Serial UDB Extra Last dspic Trap Flags - */ -static inline int16_t mavlink_msg_serial_udb_extra_f14_get_sue_TRAP_FLAGS(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 6); -} - -/** - * @brief Get field sue_TRAP_SOURCE from serial_udb_extra_f14 message - * - * @return Serial UDB Extra Type Program Address of Last Trap - */ -static inline uint32_t mavlink_msg_serial_udb_extra_f14_get_sue_TRAP_SOURCE(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field sue_osc_fail_count from serial_udb_extra_f14 message - * - * @return Serial UDB Extra Number of Ocillator Failures - */ -static inline int16_t mavlink_msg_serial_udb_extra_f14_get_sue_osc_fail_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 8); -} - -/** - * @brief Get field sue_CLOCK_CONFIG from serial_udb_extra_f14 message - * - * @return Serial UDB Extra UDB Internal Clock Configuration - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f14_get_sue_CLOCK_CONFIG(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 15); -} - -/** - * @brief Get field sue_FLIGHT_PLAN_TYPE from serial_udb_extra_f14 message - * - * @return Serial UDB Extra Type of Flight Plan - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f14_get_sue_FLIGHT_PLAN_TYPE(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Decode a serial_udb_extra_f14 message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f14 C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f14_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f14_t* serial_udb_extra_f14) -{ -#if MAVLINK_NEED_BYTE_SWAP - serial_udb_extra_f14->sue_TRAP_SOURCE = mavlink_msg_serial_udb_extra_f14_get_sue_TRAP_SOURCE(msg); - serial_udb_extra_f14->sue_RCON = mavlink_msg_serial_udb_extra_f14_get_sue_RCON(msg); - serial_udb_extra_f14->sue_TRAP_FLAGS = mavlink_msg_serial_udb_extra_f14_get_sue_TRAP_FLAGS(msg); - serial_udb_extra_f14->sue_osc_fail_count = mavlink_msg_serial_udb_extra_f14_get_sue_osc_fail_count(msg); - serial_udb_extra_f14->sue_WIND_ESTIMATION = mavlink_msg_serial_udb_extra_f14_get_sue_WIND_ESTIMATION(msg); - serial_udb_extra_f14->sue_GPS_TYPE = mavlink_msg_serial_udb_extra_f14_get_sue_GPS_TYPE(msg); - serial_udb_extra_f14->sue_DR = mavlink_msg_serial_udb_extra_f14_get_sue_DR(msg); - serial_udb_extra_f14->sue_BOARD_TYPE = mavlink_msg_serial_udb_extra_f14_get_sue_BOARD_TYPE(msg); - serial_udb_extra_f14->sue_AIRFRAME = mavlink_msg_serial_udb_extra_f14_get_sue_AIRFRAME(msg); - serial_udb_extra_f14->sue_CLOCK_CONFIG = mavlink_msg_serial_udb_extra_f14_get_sue_CLOCK_CONFIG(msg); - serial_udb_extra_f14->sue_FLIGHT_PLAN_TYPE = mavlink_msg_serial_udb_extra_f14_get_sue_FLIGHT_PLAN_TYPE(msg); -#else - memcpy(serial_udb_extra_f14, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h deleted file mode 100644 index 10c3f4c..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h +++ /dev/null @@ -1,200 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F15 PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15 179 - -typedef struct __mavlink_serial_udb_extra_f15_t -{ - uint8_t sue_ID_VEHICLE_MODEL_NAME[40]; ///< Serial UDB Extra Model Name Of Vehicle - uint8_t sue_ID_VEHICLE_REGISTRATION[20]; ///< Serial UDB Extra Registraton Number of Vehicle -} mavlink_serial_udb_extra_f15_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN 60 -#define MAVLINK_MSG_ID_179_LEN 60 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_CRC 7 -#define MAVLINK_MSG_ID_179_CRC 7 - -#define MAVLINK_MSG_SERIAL_UDB_EXTRA_F15_FIELD_SUE_ID_VEHICLE_MODEL_NAME_LEN 40 -#define MAVLINK_MSG_SERIAL_UDB_EXTRA_F15_FIELD_SUE_ID_VEHICLE_REGISTRATION_LEN 20 - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F15 { \ - "SERIAL_UDB_EXTRA_F15", \ - 2, \ - { { "sue_ID_VEHICLE_MODEL_NAME", NULL, MAVLINK_TYPE_UINT8_T, 40, 0, offsetof(mavlink_serial_udb_extra_f15_t, sue_ID_VEHICLE_MODEL_NAME) }, \ - { "sue_ID_VEHICLE_REGISTRATION", NULL, MAVLINK_TYPE_UINT8_T, 20, 40, offsetof(mavlink_serial_udb_extra_f15_t, sue_ID_VEHICLE_REGISTRATION) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f15 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_ID_VEHICLE_MODEL_NAME Serial UDB Extra Model Name Of Vehicle - * @param sue_ID_VEHICLE_REGISTRATION Serial UDB Extra Registraton Number of Vehicle - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f15_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const uint8_t *sue_ID_VEHICLE_MODEL_NAME, const uint8_t *sue_ID_VEHICLE_REGISTRATION) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN]; - - _mav_put_uint8_t_array(buf, 0, sue_ID_VEHICLE_MODEL_NAME, 40); - _mav_put_uint8_t_array(buf, 40, sue_ID_VEHICLE_REGISTRATION, 20); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); -#else - mavlink_serial_udb_extra_f15_t packet; - - mav_array_memcpy(packet.sue_ID_VEHICLE_MODEL_NAME, sue_ID_VEHICLE_MODEL_NAME, sizeof(uint8_t)*40); - mav_array_memcpy(packet.sue_ID_VEHICLE_REGISTRATION, sue_ID_VEHICLE_REGISTRATION, sizeof(uint8_t)*20); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f15 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_ID_VEHICLE_MODEL_NAME Serial UDB Extra Model Name Of Vehicle - * @param sue_ID_VEHICLE_REGISTRATION Serial UDB Extra Registraton Number of Vehicle - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f15_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const uint8_t *sue_ID_VEHICLE_MODEL_NAME,const uint8_t *sue_ID_VEHICLE_REGISTRATION) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN]; - - _mav_put_uint8_t_array(buf, 0, sue_ID_VEHICLE_MODEL_NAME, 40); - _mav_put_uint8_t_array(buf, 40, sue_ID_VEHICLE_REGISTRATION, 20); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); -#else - mavlink_serial_udb_extra_f15_t packet; - - mav_array_memcpy(packet.sue_ID_VEHICLE_MODEL_NAME, sue_ID_VEHICLE_MODEL_NAME, sizeof(uint8_t)*40); - mav_array_memcpy(packet.sue_ID_VEHICLE_REGISTRATION, sue_ID_VEHICLE_REGISTRATION, sizeof(uint8_t)*20); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f15 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f15 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f15_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f15_t* serial_udb_extra_f15) -{ - return mavlink_msg_serial_udb_extra_f15_pack(system_id, component_id, msg, serial_udb_extra_f15->sue_ID_VEHICLE_MODEL_NAME, serial_udb_extra_f15->sue_ID_VEHICLE_REGISTRATION); -} - -/** - * @brief Encode a serial_udb_extra_f15 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f15 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f15_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f15_t* serial_udb_extra_f15) -{ - return mavlink_msg_serial_udb_extra_f15_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f15->sue_ID_VEHICLE_MODEL_NAME, serial_udb_extra_f15->sue_ID_VEHICLE_REGISTRATION); -} - -/** - * @brief Send a serial_udb_extra_f15 message - * @param chan MAVLink channel to send the message - * - * @param sue_ID_VEHICLE_MODEL_NAME Serial UDB Extra Model Name Of Vehicle - * @param sue_ID_VEHICLE_REGISTRATION Serial UDB Extra Registraton Number of Vehicle - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f15_send(mavlink_channel_t chan, const uint8_t *sue_ID_VEHICLE_MODEL_NAME, const uint8_t *sue_ID_VEHICLE_REGISTRATION) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN]; - - _mav_put_uint8_t_array(buf, 0, sue_ID_VEHICLE_MODEL_NAME, 40); - _mav_put_uint8_t_array(buf, 40, sue_ID_VEHICLE_REGISTRATION, 20); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); -#endif -#else - mavlink_serial_udb_extra_f15_t packet; - - mav_array_memcpy(packet.sue_ID_VEHICLE_MODEL_NAME, sue_ID_VEHICLE_MODEL_NAME, sizeof(uint8_t)*40); - mav_array_memcpy(packet.sue_ID_VEHICLE_REGISTRATION, sue_ID_VEHICLE_REGISTRATION, sizeof(uint8_t)*20); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F15 UNPACKING - - -/** - * @brief Get field sue_ID_VEHICLE_MODEL_NAME from serial_udb_extra_f15 message - * - * @return Serial UDB Extra Model Name Of Vehicle - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f15_get_sue_ID_VEHICLE_MODEL_NAME(const mavlink_message_t* msg, uint8_t *sue_ID_VEHICLE_MODEL_NAME) -{ - return _MAV_RETURN_uint8_t_array(msg, sue_ID_VEHICLE_MODEL_NAME, 40, 0); -} - -/** - * @brief Get field sue_ID_VEHICLE_REGISTRATION from serial_udb_extra_f15 message - * - * @return Serial UDB Extra Registraton Number of Vehicle - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f15_get_sue_ID_VEHICLE_REGISTRATION(const mavlink_message_t* msg, uint8_t *sue_ID_VEHICLE_REGISTRATION) -{ - return _MAV_RETURN_uint8_t_array(msg, sue_ID_VEHICLE_REGISTRATION, 20, 40); -} - -/** - * @brief Decode a serial_udb_extra_f15 message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f15 C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f15_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f15_t* serial_udb_extra_f15) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_serial_udb_extra_f15_get_sue_ID_VEHICLE_MODEL_NAME(msg, serial_udb_extra_f15->sue_ID_VEHICLE_MODEL_NAME); - mavlink_msg_serial_udb_extra_f15_get_sue_ID_VEHICLE_REGISTRATION(msg, serial_udb_extra_f15->sue_ID_VEHICLE_REGISTRATION); -#else - memcpy(serial_udb_extra_f15, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h deleted file mode 100644 index 659e6b7..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h +++ /dev/null @@ -1,200 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F16 PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16 180 - -typedef struct __mavlink_serial_udb_extra_f16_t -{ - uint8_t sue_ID_LEAD_PILOT[40]; ///< Serial UDB Extra Name of Expected Lead Pilot - uint8_t sue_ID_DIY_DRONES_URL[70]; ///< Serial UDB Extra URL of Lead Pilot or Team -} mavlink_serial_udb_extra_f16_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN 110 -#define MAVLINK_MSG_ID_180_LEN 110 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_CRC 222 -#define MAVLINK_MSG_ID_180_CRC 222 - -#define MAVLINK_MSG_SERIAL_UDB_EXTRA_F16_FIELD_SUE_ID_LEAD_PILOT_LEN 40 -#define MAVLINK_MSG_SERIAL_UDB_EXTRA_F16_FIELD_SUE_ID_DIY_DRONES_URL_LEN 70 - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F16 { \ - "SERIAL_UDB_EXTRA_F16", \ - 2, \ - { { "sue_ID_LEAD_PILOT", NULL, MAVLINK_TYPE_UINT8_T, 40, 0, offsetof(mavlink_serial_udb_extra_f16_t, sue_ID_LEAD_PILOT) }, \ - { "sue_ID_DIY_DRONES_URL", NULL, MAVLINK_TYPE_UINT8_T, 70, 40, offsetof(mavlink_serial_udb_extra_f16_t, sue_ID_DIY_DRONES_URL) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f16 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_ID_LEAD_PILOT Serial UDB Extra Name of Expected Lead Pilot - * @param sue_ID_DIY_DRONES_URL Serial UDB Extra URL of Lead Pilot or Team - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f16_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const uint8_t *sue_ID_LEAD_PILOT, const uint8_t *sue_ID_DIY_DRONES_URL) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN]; - - _mav_put_uint8_t_array(buf, 0, sue_ID_LEAD_PILOT, 40); - _mav_put_uint8_t_array(buf, 40, sue_ID_DIY_DRONES_URL, 70); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); -#else - mavlink_serial_udb_extra_f16_t packet; - - mav_array_memcpy(packet.sue_ID_LEAD_PILOT, sue_ID_LEAD_PILOT, sizeof(uint8_t)*40); - mav_array_memcpy(packet.sue_ID_DIY_DRONES_URL, sue_ID_DIY_DRONES_URL, sizeof(uint8_t)*70); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f16 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_ID_LEAD_PILOT Serial UDB Extra Name of Expected Lead Pilot - * @param sue_ID_DIY_DRONES_URL Serial UDB Extra URL of Lead Pilot or Team - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f16_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const uint8_t *sue_ID_LEAD_PILOT,const uint8_t *sue_ID_DIY_DRONES_URL) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN]; - - _mav_put_uint8_t_array(buf, 0, sue_ID_LEAD_PILOT, 40); - _mav_put_uint8_t_array(buf, 40, sue_ID_DIY_DRONES_URL, 70); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); -#else - mavlink_serial_udb_extra_f16_t packet; - - mav_array_memcpy(packet.sue_ID_LEAD_PILOT, sue_ID_LEAD_PILOT, sizeof(uint8_t)*40); - mav_array_memcpy(packet.sue_ID_DIY_DRONES_URL, sue_ID_DIY_DRONES_URL, sizeof(uint8_t)*70); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f16 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f16 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f16_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f16_t* serial_udb_extra_f16) -{ - return mavlink_msg_serial_udb_extra_f16_pack(system_id, component_id, msg, serial_udb_extra_f16->sue_ID_LEAD_PILOT, serial_udb_extra_f16->sue_ID_DIY_DRONES_URL); -} - -/** - * @brief Encode a serial_udb_extra_f16 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f16 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f16_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f16_t* serial_udb_extra_f16) -{ - return mavlink_msg_serial_udb_extra_f16_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f16->sue_ID_LEAD_PILOT, serial_udb_extra_f16->sue_ID_DIY_DRONES_URL); -} - -/** - * @brief Send a serial_udb_extra_f16 message - * @param chan MAVLink channel to send the message - * - * @param sue_ID_LEAD_PILOT Serial UDB Extra Name of Expected Lead Pilot - * @param sue_ID_DIY_DRONES_URL Serial UDB Extra URL of Lead Pilot or Team - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f16_send(mavlink_channel_t chan, const uint8_t *sue_ID_LEAD_PILOT, const uint8_t *sue_ID_DIY_DRONES_URL) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN]; - - _mav_put_uint8_t_array(buf, 0, sue_ID_LEAD_PILOT, 40); - _mav_put_uint8_t_array(buf, 40, sue_ID_DIY_DRONES_URL, 70); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); -#endif -#else - mavlink_serial_udb_extra_f16_t packet; - - mav_array_memcpy(packet.sue_ID_LEAD_PILOT, sue_ID_LEAD_PILOT, sizeof(uint8_t)*40); - mav_array_memcpy(packet.sue_ID_DIY_DRONES_URL, sue_ID_DIY_DRONES_URL, sizeof(uint8_t)*70); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F16 UNPACKING - - -/** - * @brief Get field sue_ID_LEAD_PILOT from serial_udb_extra_f16 message - * - * @return Serial UDB Extra Name of Expected Lead Pilot - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f16_get_sue_ID_LEAD_PILOT(const mavlink_message_t* msg, uint8_t *sue_ID_LEAD_PILOT) -{ - return _MAV_RETURN_uint8_t_array(msg, sue_ID_LEAD_PILOT, 40, 0); -} - -/** - * @brief Get field sue_ID_DIY_DRONES_URL from serial_udb_extra_f16 message - * - * @return Serial UDB Extra URL of Lead Pilot or Team - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f16_get_sue_ID_DIY_DRONES_URL(const mavlink_message_t* msg, uint8_t *sue_ID_DIY_DRONES_URL) -{ - return _MAV_RETURN_uint8_t_array(msg, sue_ID_DIY_DRONES_URL, 70, 40); -} - -/** - * @brief Decode a serial_udb_extra_f16 message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f16 C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f16_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f16_t* serial_udb_extra_f16) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_serial_udb_extra_f16_get_sue_ID_LEAD_PILOT(msg, serial_udb_extra_f16->sue_ID_LEAD_PILOT); - mavlink_msg_serial_udb_extra_f16_get_sue_ID_DIY_DRONES_URL(msg, serial_udb_extra_f16->sue_ID_DIY_DRONES_URL); -#else - memcpy(serial_udb_extra_f16, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h deleted file mode 100644 index 15ba68a..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h +++ /dev/null @@ -1,771 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F2_A PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A 170 - -typedef struct __mavlink_serial_udb_extra_f2_a_t -{ - uint32_t sue_time; ///< Serial UDB Extra Time - int32_t sue_latitude; ///< Serial UDB Extra Latitude - int32_t sue_longitude; ///< Serial UDB Extra Longitude - int32_t sue_altitude; ///< Serial UDB Extra Altitude - uint16_t sue_waypoint_index; ///< Serial UDB Extra Waypoint Index - int16_t sue_rmat0; ///< Serial UDB Extra Rmat 0 - int16_t sue_rmat1; ///< Serial UDB Extra Rmat 1 - int16_t sue_rmat2; ///< Serial UDB Extra Rmat 2 - int16_t sue_rmat3; ///< Serial UDB Extra Rmat 3 - int16_t sue_rmat4; ///< Serial UDB Extra Rmat 4 - int16_t sue_rmat5; ///< Serial UDB Extra Rmat 5 - int16_t sue_rmat6; ///< Serial UDB Extra Rmat 6 - int16_t sue_rmat7; ///< Serial UDB Extra Rmat 7 - int16_t sue_rmat8; ///< Serial UDB Extra Rmat 8 - uint16_t sue_cog; ///< Serial UDB Extra GPS Course Over Ground - int16_t sue_sog; ///< Serial UDB Extra Speed Over Ground - uint16_t sue_cpu_load; ///< Serial UDB Extra CPU Load - int16_t sue_voltage_milis; ///< Serial UDB Extra Voltage in MilliVolts - uint16_t sue_air_speed_3DIMU; ///< Serial UDB Extra 3D IMU Air Speed - int16_t sue_estimated_wind_0; ///< Serial UDB Extra Estimated Wind 0 - int16_t sue_estimated_wind_1; ///< Serial UDB Extra Estimated Wind 1 - int16_t sue_estimated_wind_2; ///< Serial UDB Extra Estimated Wind 2 - int16_t sue_magFieldEarth0; ///< Serial UDB Extra Magnetic Field Earth 0 - int16_t sue_magFieldEarth1; ///< Serial UDB Extra Magnetic Field Earth 1 - int16_t sue_magFieldEarth2; ///< Serial UDB Extra Magnetic Field Earth 2 - int16_t sue_svs; ///< Serial UDB Extra Number of Sattelites in View - int16_t sue_hdop; ///< Serial UDB Extra GPS Horizontal Dilution of Precision - uint8_t sue_status; ///< Serial UDB Extra Status -} mavlink_serial_udb_extra_f2_a_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN 63 -#define MAVLINK_MSG_ID_170_LEN 63 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_CRC 150 -#define MAVLINK_MSG_ID_170_CRC 150 - - - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F2_A { \ - "SERIAL_UDB_EXTRA_F2_A", \ - 28, \ - { { "sue_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_time) }, \ - { "sue_latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_latitude) }, \ - { "sue_longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_longitude) }, \ - { "sue_altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_altitude) }, \ - { "sue_waypoint_index", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_waypoint_index) }, \ - { "sue_rmat0", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat0) }, \ - { "sue_rmat1", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat1) }, \ - { "sue_rmat2", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat2) }, \ - { "sue_rmat3", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat3) }, \ - { "sue_rmat4", NULL, MAVLINK_TYPE_INT16_T, 0, 26, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat4) }, \ - { "sue_rmat5", NULL, MAVLINK_TYPE_INT16_T, 0, 28, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat5) }, \ - { "sue_rmat6", NULL, MAVLINK_TYPE_INT16_T, 0, 30, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat6) }, \ - { "sue_rmat7", NULL, MAVLINK_TYPE_INT16_T, 0, 32, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat7) }, \ - { "sue_rmat8", NULL, MAVLINK_TYPE_INT16_T, 0, 34, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat8) }, \ - { "sue_cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_cog) }, \ - { "sue_sog", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_sog) }, \ - { "sue_cpu_load", NULL, MAVLINK_TYPE_UINT16_T, 0, 40, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_cpu_load) }, \ - { "sue_voltage_milis", NULL, MAVLINK_TYPE_INT16_T, 0, 42, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_voltage_milis) }, \ - { "sue_air_speed_3DIMU", NULL, MAVLINK_TYPE_UINT16_T, 0, 44, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_air_speed_3DIMU) }, \ - { "sue_estimated_wind_0", NULL, MAVLINK_TYPE_INT16_T, 0, 46, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_estimated_wind_0) }, \ - { "sue_estimated_wind_1", NULL, MAVLINK_TYPE_INT16_T, 0, 48, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_estimated_wind_1) }, \ - { "sue_estimated_wind_2", NULL, MAVLINK_TYPE_INT16_T, 0, 50, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_estimated_wind_2) }, \ - { "sue_magFieldEarth0", NULL, MAVLINK_TYPE_INT16_T, 0, 52, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_magFieldEarth0) }, \ - { "sue_magFieldEarth1", NULL, MAVLINK_TYPE_INT16_T, 0, 54, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_magFieldEarth1) }, \ - { "sue_magFieldEarth2", NULL, MAVLINK_TYPE_INT16_T, 0, 56, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_magFieldEarth2) }, \ - { "sue_svs", NULL, MAVLINK_TYPE_INT16_T, 0, 58, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_svs) }, \ - { "sue_hdop", NULL, MAVLINK_TYPE_INT16_T, 0, 60, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_hdop) }, \ - { "sue_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 62, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_status) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f2_a message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_time Serial UDB Extra Time - * @param sue_status Serial UDB Extra Status - * @param sue_latitude Serial UDB Extra Latitude - * @param sue_longitude Serial UDB Extra Longitude - * @param sue_altitude Serial UDB Extra Altitude - * @param sue_waypoint_index Serial UDB Extra Waypoint Index - * @param sue_rmat0 Serial UDB Extra Rmat 0 - * @param sue_rmat1 Serial UDB Extra Rmat 1 - * @param sue_rmat2 Serial UDB Extra Rmat 2 - * @param sue_rmat3 Serial UDB Extra Rmat 3 - * @param sue_rmat4 Serial UDB Extra Rmat 4 - * @param sue_rmat5 Serial UDB Extra Rmat 5 - * @param sue_rmat6 Serial UDB Extra Rmat 6 - * @param sue_rmat7 Serial UDB Extra Rmat 7 - * @param sue_rmat8 Serial UDB Extra Rmat 8 - * @param sue_cog Serial UDB Extra GPS Course Over Ground - * @param sue_sog Serial UDB Extra Speed Over Ground - * @param sue_cpu_load Serial UDB Extra CPU Load - * @param sue_voltage_milis Serial UDB Extra Voltage in MilliVolts - * @param sue_air_speed_3DIMU Serial UDB Extra 3D IMU Air Speed - * @param sue_estimated_wind_0 Serial UDB Extra Estimated Wind 0 - * @param sue_estimated_wind_1 Serial UDB Extra Estimated Wind 1 - * @param sue_estimated_wind_2 Serial UDB Extra Estimated Wind 2 - * @param sue_magFieldEarth0 Serial UDB Extra Magnetic Field Earth 0 - * @param sue_magFieldEarth1 Serial UDB Extra Magnetic Field Earth 1 - * @param sue_magFieldEarth2 Serial UDB Extra Magnetic Field Earth 2 - * @param sue_svs Serial UDB Extra Number of Sattelites in View - * @param sue_hdop Serial UDB Extra GPS Horizontal Dilution of Precision - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t sue_time, uint8_t sue_status, int32_t sue_latitude, int32_t sue_longitude, int32_t sue_altitude, uint16_t sue_waypoint_index, int16_t sue_rmat0, int16_t sue_rmat1, int16_t sue_rmat2, int16_t sue_rmat3, int16_t sue_rmat4, int16_t sue_rmat5, int16_t sue_rmat6, int16_t sue_rmat7, int16_t sue_rmat8, uint16_t sue_cog, int16_t sue_sog, uint16_t sue_cpu_load, int16_t sue_voltage_milis, uint16_t sue_air_speed_3DIMU, int16_t sue_estimated_wind_0, int16_t sue_estimated_wind_1, int16_t sue_estimated_wind_2, int16_t sue_magFieldEarth0, int16_t sue_magFieldEarth1, int16_t sue_magFieldEarth2, int16_t sue_svs, int16_t sue_hdop) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN]; - _mav_put_uint32_t(buf, 0, sue_time); - _mav_put_int32_t(buf, 4, sue_latitude); - _mav_put_int32_t(buf, 8, sue_longitude); - _mav_put_int32_t(buf, 12, sue_altitude); - _mav_put_uint16_t(buf, 16, sue_waypoint_index); - _mav_put_int16_t(buf, 18, sue_rmat0); - _mav_put_int16_t(buf, 20, sue_rmat1); - _mav_put_int16_t(buf, 22, sue_rmat2); - _mav_put_int16_t(buf, 24, sue_rmat3); - _mav_put_int16_t(buf, 26, sue_rmat4); - _mav_put_int16_t(buf, 28, sue_rmat5); - _mav_put_int16_t(buf, 30, sue_rmat6); - _mav_put_int16_t(buf, 32, sue_rmat7); - _mav_put_int16_t(buf, 34, sue_rmat8); - _mav_put_uint16_t(buf, 36, sue_cog); - _mav_put_int16_t(buf, 38, sue_sog); - _mav_put_uint16_t(buf, 40, sue_cpu_load); - _mav_put_int16_t(buf, 42, sue_voltage_milis); - _mav_put_uint16_t(buf, 44, sue_air_speed_3DIMU); - _mav_put_int16_t(buf, 46, sue_estimated_wind_0); - _mav_put_int16_t(buf, 48, sue_estimated_wind_1); - _mav_put_int16_t(buf, 50, sue_estimated_wind_2); - _mav_put_int16_t(buf, 52, sue_magFieldEarth0); - _mav_put_int16_t(buf, 54, sue_magFieldEarth1); - _mav_put_int16_t(buf, 56, sue_magFieldEarth2); - _mav_put_int16_t(buf, 58, sue_svs); - _mav_put_int16_t(buf, 60, sue_hdop); - _mav_put_uint8_t(buf, 62, sue_status); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); -#else - mavlink_serial_udb_extra_f2_a_t packet; - packet.sue_time = sue_time; - packet.sue_latitude = sue_latitude; - packet.sue_longitude = sue_longitude; - packet.sue_altitude = sue_altitude; - packet.sue_waypoint_index = sue_waypoint_index; - packet.sue_rmat0 = sue_rmat0; - packet.sue_rmat1 = sue_rmat1; - packet.sue_rmat2 = sue_rmat2; - packet.sue_rmat3 = sue_rmat3; - packet.sue_rmat4 = sue_rmat4; - packet.sue_rmat5 = sue_rmat5; - packet.sue_rmat6 = sue_rmat6; - packet.sue_rmat7 = sue_rmat7; - packet.sue_rmat8 = sue_rmat8; - packet.sue_cog = sue_cog; - packet.sue_sog = sue_sog; - packet.sue_cpu_load = sue_cpu_load; - packet.sue_voltage_milis = sue_voltage_milis; - packet.sue_air_speed_3DIMU = sue_air_speed_3DIMU; - packet.sue_estimated_wind_0 = sue_estimated_wind_0; - packet.sue_estimated_wind_1 = sue_estimated_wind_1; - packet.sue_estimated_wind_2 = sue_estimated_wind_2; - packet.sue_magFieldEarth0 = sue_magFieldEarth0; - packet.sue_magFieldEarth1 = sue_magFieldEarth1; - packet.sue_magFieldEarth2 = sue_magFieldEarth2; - packet.sue_svs = sue_svs; - packet.sue_hdop = sue_hdop; - packet.sue_status = sue_status; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f2_a message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_time Serial UDB Extra Time - * @param sue_status Serial UDB Extra Status - * @param sue_latitude Serial UDB Extra Latitude - * @param sue_longitude Serial UDB Extra Longitude - * @param sue_altitude Serial UDB Extra Altitude - * @param sue_waypoint_index Serial UDB Extra Waypoint Index - * @param sue_rmat0 Serial UDB Extra Rmat 0 - * @param sue_rmat1 Serial UDB Extra Rmat 1 - * @param sue_rmat2 Serial UDB Extra Rmat 2 - * @param sue_rmat3 Serial UDB Extra Rmat 3 - * @param sue_rmat4 Serial UDB Extra Rmat 4 - * @param sue_rmat5 Serial UDB Extra Rmat 5 - * @param sue_rmat6 Serial UDB Extra Rmat 6 - * @param sue_rmat7 Serial UDB Extra Rmat 7 - * @param sue_rmat8 Serial UDB Extra Rmat 8 - * @param sue_cog Serial UDB Extra GPS Course Over Ground - * @param sue_sog Serial UDB Extra Speed Over Ground - * @param sue_cpu_load Serial UDB Extra CPU Load - * @param sue_voltage_milis Serial UDB Extra Voltage in MilliVolts - * @param sue_air_speed_3DIMU Serial UDB Extra 3D IMU Air Speed - * @param sue_estimated_wind_0 Serial UDB Extra Estimated Wind 0 - * @param sue_estimated_wind_1 Serial UDB Extra Estimated Wind 1 - * @param sue_estimated_wind_2 Serial UDB Extra Estimated Wind 2 - * @param sue_magFieldEarth0 Serial UDB Extra Magnetic Field Earth 0 - * @param sue_magFieldEarth1 Serial UDB Extra Magnetic Field Earth 1 - * @param sue_magFieldEarth2 Serial UDB Extra Magnetic Field Earth 2 - * @param sue_svs Serial UDB Extra Number of Sattelites in View - * @param sue_hdop Serial UDB Extra GPS Horizontal Dilution of Precision - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t sue_time,uint8_t sue_status,int32_t sue_latitude,int32_t sue_longitude,int32_t sue_altitude,uint16_t sue_waypoint_index,int16_t sue_rmat0,int16_t sue_rmat1,int16_t sue_rmat2,int16_t sue_rmat3,int16_t sue_rmat4,int16_t sue_rmat5,int16_t sue_rmat6,int16_t sue_rmat7,int16_t sue_rmat8,uint16_t sue_cog,int16_t sue_sog,uint16_t sue_cpu_load,int16_t sue_voltage_milis,uint16_t sue_air_speed_3DIMU,int16_t sue_estimated_wind_0,int16_t sue_estimated_wind_1,int16_t sue_estimated_wind_2,int16_t sue_magFieldEarth0,int16_t sue_magFieldEarth1,int16_t sue_magFieldEarth2,int16_t sue_svs,int16_t sue_hdop) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN]; - _mav_put_uint32_t(buf, 0, sue_time); - _mav_put_int32_t(buf, 4, sue_latitude); - _mav_put_int32_t(buf, 8, sue_longitude); - _mav_put_int32_t(buf, 12, sue_altitude); - _mav_put_uint16_t(buf, 16, sue_waypoint_index); - _mav_put_int16_t(buf, 18, sue_rmat0); - _mav_put_int16_t(buf, 20, sue_rmat1); - _mav_put_int16_t(buf, 22, sue_rmat2); - _mav_put_int16_t(buf, 24, sue_rmat3); - _mav_put_int16_t(buf, 26, sue_rmat4); - _mav_put_int16_t(buf, 28, sue_rmat5); - _mav_put_int16_t(buf, 30, sue_rmat6); - _mav_put_int16_t(buf, 32, sue_rmat7); - _mav_put_int16_t(buf, 34, sue_rmat8); - _mav_put_uint16_t(buf, 36, sue_cog); - _mav_put_int16_t(buf, 38, sue_sog); - _mav_put_uint16_t(buf, 40, sue_cpu_load); - _mav_put_int16_t(buf, 42, sue_voltage_milis); - _mav_put_uint16_t(buf, 44, sue_air_speed_3DIMU); - _mav_put_int16_t(buf, 46, sue_estimated_wind_0); - _mav_put_int16_t(buf, 48, sue_estimated_wind_1); - _mav_put_int16_t(buf, 50, sue_estimated_wind_2); - _mav_put_int16_t(buf, 52, sue_magFieldEarth0); - _mav_put_int16_t(buf, 54, sue_magFieldEarth1); - _mav_put_int16_t(buf, 56, sue_magFieldEarth2); - _mav_put_int16_t(buf, 58, sue_svs); - _mav_put_int16_t(buf, 60, sue_hdop); - _mav_put_uint8_t(buf, 62, sue_status); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); -#else - mavlink_serial_udb_extra_f2_a_t packet; - packet.sue_time = sue_time; - packet.sue_latitude = sue_latitude; - packet.sue_longitude = sue_longitude; - packet.sue_altitude = sue_altitude; - packet.sue_waypoint_index = sue_waypoint_index; - packet.sue_rmat0 = sue_rmat0; - packet.sue_rmat1 = sue_rmat1; - packet.sue_rmat2 = sue_rmat2; - packet.sue_rmat3 = sue_rmat3; - packet.sue_rmat4 = sue_rmat4; - packet.sue_rmat5 = sue_rmat5; - packet.sue_rmat6 = sue_rmat6; - packet.sue_rmat7 = sue_rmat7; - packet.sue_rmat8 = sue_rmat8; - packet.sue_cog = sue_cog; - packet.sue_sog = sue_sog; - packet.sue_cpu_load = sue_cpu_load; - packet.sue_voltage_milis = sue_voltage_milis; - packet.sue_air_speed_3DIMU = sue_air_speed_3DIMU; - packet.sue_estimated_wind_0 = sue_estimated_wind_0; - packet.sue_estimated_wind_1 = sue_estimated_wind_1; - packet.sue_estimated_wind_2 = sue_estimated_wind_2; - packet.sue_magFieldEarth0 = sue_magFieldEarth0; - packet.sue_magFieldEarth1 = sue_magFieldEarth1; - packet.sue_magFieldEarth2 = sue_magFieldEarth2; - packet.sue_svs = sue_svs; - packet.sue_hdop = sue_hdop; - packet.sue_status = sue_status; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f2_a struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f2_a C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f2_a_t* serial_udb_extra_f2_a) -{ - return mavlink_msg_serial_udb_extra_f2_a_pack(system_id, component_id, msg, serial_udb_extra_f2_a->sue_time, serial_udb_extra_f2_a->sue_status, serial_udb_extra_f2_a->sue_latitude, serial_udb_extra_f2_a->sue_longitude, serial_udb_extra_f2_a->sue_altitude, serial_udb_extra_f2_a->sue_waypoint_index, serial_udb_extra_f2_a->sue_rmat0, serial_udb_extra_f2_a->sue_rmat1, serial_udb_extra_f2_a->sue_rmat2, serial_udb_extra_f2_a->sue_rmat3, serial_udb_extra_f2_a->sue_rmat4, serial_udb_extra_f2_a->sue_rmat5, serial_udb_extra_f2_a->sue_rmat6, serial_udb_extra_f2_a->sue_rmat7, serial_udb_extra_f2_a->sue_rmat8, serial_udb_extra_f2_a->sue_cog, serial_udb_extra_f2_a->sue_sog, serial_udb_extra_f2_a->sue_cpu_load, serial_udb_extra_f2_a->sue_voltage_milis, serial_udb_extra_f2_a->sue_air_speed_3DIMU, serial_udb_extra_f2_a->sue_estimated_wind_0, serial_udb_extra_f2_a->sue_estimated_wind_1, serial_udb_extra_f2_a->sue_estimated_wind_2, serial_udb_extra_f2_a->sue_magFieldEarth0, serial_udb_extra_f2_a->sue_magFieldEarth1, serial_udb_extra_f2_a->sue_magFieldEarth2, serial_udb_extra_f2_a->sue_svs, serial_udb_extra_f2_a->sue_hdop); -} - -/** - * @brief Encode a serial_udb_extra_f2_a struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f2_a C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f2_a_t* serial_udb_extra_f2_a) -{ - return mavlink_msg_serial_udb_extra_f2_a_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f2_a->sue_time, serial_udb_extra_f2_a->sue_status, serial_udb_extra_f2_a->sue_latitude, serial_udb_extra_f2_a->sue_longitude, serial_udb_extra_f2_a->sue_altitude, serial_udb_extra_f2_a->sue_waypoint_index, serial_udb_extra_f2_a->sue_rmat0, serial_udb_extra_f2_a->sue_rmat1, serial_udb_extra_f2_a->sue_rmat2, serial_udb_extra_f2_a->sue_rmat3, serial_udb_extra_f2_a->sue_rmat4, serial_udb_extra_f2_a->sue_rmat5, serial_udb_extra_f2_a->sue_rmat6, serial_udb_extra_f2_a->sue_rmat7, serial_udb_extra_f2_a->sue_rmat8, serial_udb_extra_f2_a->sue_cog, serial_udb_extra_f2_a->sue_sog, serial_udb_extra_f2_a->sue_cpu_load, serial_udb_extra_f2_a->sue_voltage_milis, serial_udb_extra_f2_a->sue_air_speed_3DIMU, serial_udb_extra_f2_a->sue_estimated_wind_0, serial_udb_extra_f2_a->sue_estimated_wind_1, serial_udb_extra_f2_a->sue_estimated_wind_2, serial_udb_extra_f2_a->sue_magFieldEarth0, serial_udb_extra_f2_a->sue_magFieldEarth1, serial_udb_extra_f2_a->sue_magFieldEarth2, serial_udb_extra_f2_a->sue_svs, serial_udb_extra_f2_a->sue_hdop); -} - -/** - * @brief Send a serial_udb_extra_f2_a message - * @param chan MAVLink channel to send the message - * - * @param sue_time Serial UDB Extra Time - * @param sue_status Serial UDB Extra Status - * @param sue_latitude Serial UDB Extra Latitude - * @param sue_longitude Serial UDB Extra Longitude - * @param sue_altitude Serial UDB Extra Altitude - * @param sue_waypoint_index Serial UDB Extra Waypoint Index - * @param sue_rmat0 Serial UDB Extra Rmat 0 - * @param sue_rmat1 Serial UDB Extra Rmat 1 - * @param sue_rmat2 Serial UDB Extra Rmat 2 - * @param sue_rmat3 Serial UDB Extra Rmat 3 - * @param sue_rmat4 Serial UDB Extra Rmat 4 - * @param sue_rmat5 Serial UDB Extra Rmat 5 - * @param sue_rmat6 Serial UDB Extra Rmat 6 - * @param sue_rmat7 Serial UDB Extra Rmat 7 - * @param sue_rmat8 Serial UDB Extra Rmat 8 - * @param sue_cog Serial UDB Extra GPS Course Over Ground - * @param sue_sog Serial UDB Extra Speed Over Ground - * @param sue_cpu_load Serial UDB Extra CPU Load - * @param sue_voltage_milis Serial UDB Extra Voltage in MilliVolts - * @param sue_air_speed_3DIMU Serial UDB Extra 3D IMU Air Speed - * @param sue_estimated_wind_0 Serial UDB Extra Estimated Wind 0 - * @param sue_estimated_wind_1 Serial UDB Extra Estimated Wind 1 - * @param sue_estimated_wind_2 Serial UDB Extra Estimated Wind 2 - * @param sue_magFieldEarth0 Serial UDB Extra Magnetic Field Earth 0 - * @param sue_magFieldEarth1 Serial UDB Extra Magnetic Field Earth 1 - * @param sue_magFieldEarth2 Serial UDB Extra Magnetic Field Earth 2 - * @param sue_svs Serial UDB Extra Number of Sattelites in View - * @param sue_hdop Serial UDB Extra GPS Horizontal Dilution of Precision - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f2_a_send(mavlink_channel_t chan, uint32_t sue_time, uint8_t sue_status, int32_t sue_latitude, int32_t sue_longitude, int32_t sue_altitude, uint16_t sue_waypoint_index, int16_t sue_rmat0, int16_t sue_rmat1, int16_t sue_rmat2, int16_t sue_rmat3, int16_t sue_rmat4, int16_t sue_rmat5, int16_t sue_rmat6, int16_t sue_rmat7, int16_t sue_rmat8, uint16_t sue_cog, int16_t sue_sog, uint16_t sue_cpu_load, int16_t sue_voltage_milis, uint16_t sue_air_speed_3DIMU, int16_t sue_estimated_wind_0, int16_t sue_estimated_wind_1, int16_t sue_estimated_wind_2, int16_t sue_magFieldEarth0, int16_t sue_magFieldEarth1, int16_t sue_magFieldEarth2, int16_t sue_svs, int16_t sue_hdop) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN]; - _mav_put_uint32_t(buf, 0, sue_time); - _mav_put_int32_t(buf, 4, sue_latitude); - _mav_put_int32_t(buf, 8, sue_longitude); - _mav_put_int32_t(buf, 12, sue_altitude); - _mav_put_uint16_t(buf, 16, sue_waypoint_index); - _mav_put_int16_t(buf, 18, sue_rmat0); - _mav_put_int16_t(buf, 20, sue_rmat1); - _mav_put_int16_t(buf, 22, sue_rmat2); - _mav_put_int16_t(buf, 24, sue_rmat3); - _mav_put_int16_t(buf, 26, sue_rmat4); - _mav_put_int16_t(buf, 28, sue_rmat5); - _mav_put_int16_t(buf, 30, sue_rmat6); - _mav_put_int16_t(buf, 32, sue_rmat7); - _mav_put_int16_t(buf, 34, sue_rmat8); - _mav_put_uint16_t(buf, 36, sue_cog); - _mav_put_int16_t(buf, 38, sue_sog); - _mav_put_uint16_t(buf, 40, sue_cpu_load); - _mav_put_int16_t(buf, 42, sue_voltage_milis); - _mav_put_uint16_t(buf, 44, sue_air_speed_3DIMU); - _mav_put_int16_t(buf, 46, sue_estimated_wind_0); - _mav_put_int16_t(buf, 48, sue_estimated_wind_1); - _mav_put_int16_t(buf, 50, sue_estimated_wind_2); - _mav_put_int16_t(buf, 52, sue_magFieldEarth0); - _mav_put_int16_t(buf, 54, sue_magFieldEarth1); - _mav_put_int16_t(buf, 56, sue_magFieldEarth2); - _mav_put_int16_t(buf, 58, sue_svs); - _mav_put_int16_t(buf, 60, sue_hdop); - _mav_put_uint8_t(buf, 62, sue_status); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); -#endif -#else - mavlink_serial_udb_extra_f2_a_t packet; - packet.sue_time = sue_time; - packet.sue_latitude = sue_latitude; - packet.sue_longitude = sue_longitude; - packet.sue_altitude = sue_altitude; - packet.sue_waypoint_index = sue_waypoint_index; - packet.sue_rmat0 = sue_rmat0; - packet.sue_rmat1 = sue_rmat1; - packet.sue_rmat2 = sue_rmat2; - packet.sue_rmat3 = sue_rmat3; - packet.sue_rmat4 = sue_rmat4; - packet.sue_rmat5 = sue_rmat5; - packet.sue_rmat6 = sue_rmat6; - packet.sue_rmat7 = sue_rmat7; - packet.sue_rmat8 = sue_rmat8; - packet.sue_cog = sue_cog; - packet.sue_sog = sue_sog; - packet.sue_cpu_load = sue_cpu_load; - packet.sue_voltage_milis = sue_voltage_milis; - packet.sue_air_speed_3DIMU = sue_air_speed_3DIMU; - packet.sue_estimated_wind_0 = sue_estimated_wind_0; - packet.sue_estimated_wind_1 = sue_estimated_wind_1; - packet.sue_estimated_wind_2 = sue_estimated_wind_2; - packet.sue_magFieldEarth0 = sue_magFieldEarth0; - packet.sue_magFieldEarth1 = sue_magFieldEarth1; - packet.sue_magFieldEarth2 = sue_magFieldEarth2; - packet.sue_svs = sue_svs; - packet.sue_hdop = sue_hdop; - packet.sue_status = sue_status; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F2_A UNPACKING - - -/** - * @brief Get field sue_time from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Time - */ -static inline uint32_t mavlink_msg_serial_udb_extra_f2_a_get_sue_time(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field sue_status from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Status - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f2_a_get_sue_status(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 62); -} - -/** - * @brief Get field sue_latitude from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Latitude - */ -static inline int32_t mavlink_msg_serial_udb_extra_f2_a_get_sue_latitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field sue_longitude from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Longitude - */ -static inline int32_t mavlink_msg_serial_udb_extra_f2_a_get_sue_longitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Get field sue_altitude from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Altitude - */ -static inline int32_t mavlink_msg_serial_udb_extra_f2_a_get_sue_altitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 12); -} - -/** - * @brief Get field sue_waypoint_index from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Waypoint Index - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_waypoint_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 16); -} - -/** - * @brief Get field sue_rmat0 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Rmat 0 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat0(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 18); -} - -/** - * @brief Get field sue_rmat1 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Rmat 1 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 20); -} - -/** - * @brief Get field sue_rmat2 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Rmat 2 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 22); -} - -/** - * @brief Get field sue_rmat3 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Rmat 3 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 24); -} - -/** - * @brief Get field sue_rmat4 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Rmat 4 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 26); -} - -/** - * @brief Get field sue_rmat5 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Rmat 5 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 28); -} - -/** - * @brief Get field sue_rmat6 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Rmat 6 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 30); -} - -/** - * @brief Get field sue_rmat7 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Rmat 7 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat7(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 32); -} - -/** - * @brief Get field sue_rmat8 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Rmat 8 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat8(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 34); -} - -/** - * @brief Get field sue_cog from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra GPS Course Over Ground - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_cog(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 36); -} - -/** - * @brief Get field sue_sog from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Speed Over Ground - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_sog(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 38); -} - -/** - * @brief Get field sue_cpu_load from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra CPU Load - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_cpu_load(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 40); -} - -/** - * @brief Get field sue_voltage_milis from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Voltage in MilliVolts - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_voltage_milis(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 42); -} - -/** - * @brief Get field sue_air_speed_3DIMU from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra 3D IMU Air Speed - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_air_speed_3DIMU(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 44); -} - -/** - * @brief Get field sue_estimated_wind_0 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Estimated Wind 0 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_estimated_wind_0(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 46); -} - -/** - * @brief Get field sue_estimated_wind_1 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Estimated Wind 1 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_estimated_wind_1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 48); -} - -/** - * @brief Get field sue_estimated_wind_2 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Estimated Wind 2 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_estimated_wind_2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 50); -} - -/** - * @brief Get field sue_magFieldEarth0 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Magnetic Field Earth 0 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_magFieldEarth0(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 52); -} - -/** - * @brief Get field sue_magFieldEarth1 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Magnetic Field Earth 1 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_magFieldEarth1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 54); -} - -/** - * @brief Get field sue_magFieldEarth2 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Magnetic Field Earth 2 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_magFieldEarth2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 56); -} - -/** - * @brief Get field sue_svs from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Number of Sattelites in View - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_svs(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 58); -} - -/** - * @brief Get field sue_hdop from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra GPS Horizontal Dilution of Precision - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_hdop(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 60); -} - -/** - * @brief Decode a serial_udb_extra_f2_a message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f2_a C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f2_a_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f2_a_t* serial_udb_extra_f2_a) -{ -#if MAVLINK_NEED_BYTE_SWAP - serial_udb_extra_f2_a->sue_time = mavlink_msg_serial_udb_extra_f2_a_get_sue_time(msg); - serial_udb_extra_f2_a->sue_latitude = mavlink_msg_serial_udb_extra_f2_a_get_sue_latitude(msg); - serial_udb_extra_f2_a->sue_longitude = mavlink_msg_serial_udb_extra_f2_a_get_sue_longitude(msg); - serial_udb_extra_f2_a->sue_altitude = mavlink_msg_serial_udb_extra_f2_a_get_sue_altitude(msg); - serial_udb_extra_f2_a->sue_waypoint_index = mavlink_msg_serial_udb_extra_f2_a_get_sue_waypoint_index(msg); - serial_udb_extra_f2_a->sue_rmat0 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat0(msg); - serial_udb_extra_f2_a->sue_rmat1 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat1(msg); - serial_udb_extra_f2_a->sue_rmat2 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat2(msg); - serial_udb_extra_f2_a->sue_rmat3 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat3(msg); - serial_udb_extra_f2_a->sue_rmat4 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat4(msg); - serial_udb_extra_f2_a->sue_rmat5 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat5(msg); - serial_udb_extra_f2_a->sue_rmat6 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat6(msg); - serial_udb_extra_f2_a->sue_rmat7 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat7(msg); - serial_udb_extra_f2_a->sue_rmat8 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat8(msg); - serial_udb_extra_f2_a->sue_cog = mavlink_msg_serial_udb_extra_f2_a_get_sue_cog(msg); - serial_udb_extra_f2_a->sue_sog = mavlink_msg_serial_udb_extra_f2_a_get_sue_sog(msg); - serial_udb_extra_f2_a->sue_cpu_load = mavlink_msg_serial_udb_extra_f2_a_get_sue_cpu_load(msg); - serial_udb_extra_f2_a->sue_voltage_milis = mavlink_msg_serial_udb_extra_f2_a_get_sue_voltage_milis(msg); - serial_udb_extra_f2_a->sue_air_speed_3DIMU = mavlink_msg_serial_udb_extra_f2_a_get_sue_air_speed_3DIMU(msg); - serial_udb_extra_f2_a->sue_estimated_wind_0 = mavlink_msg_serial_udb_extra_f2_a_get_sue_estimated_wind_0(msg); - serial_udb_extra_f2_a->sue_estimated_wind_1 = mavlink_msg_serial_udb_extra_f2_a_get_sue_estimated_wind_1(msg); - serial_udb_extra_f2_a->sue_estimated_wind_2 = mavlink_msg_serial_udb_extra_f2_a_get_sue_estimated_wind_2(msg); - serial_udb_extra_f2_a->sue_magFieldEarth0 = mavlink_msg_serial_udb_extra_f2_a_get_sue_magFieldEarth0(msg); - serial_udb_extra_f2_a->sue_magFieldEarth1 = mavlink_msg_serial_udb_extra_f2_a_get_sue_magFieldEarth1(msg); - serial_udb_extra_f2_a->sue_magFieldEarth2 = mavlink_msg_serial_udb_extra_f2_a_get_sue_magFieldEarth2(msg); - serial_udb_extra_f2_a->sue_svs = mavlink_msg_serial_udb_extra_f2_a_get_sue_svs(msg); - serial_udb_extra_f2_a->sue_hdop = mavlink_msg_serial_udb_extra_f2_a_get_sue_hdop(msg); - serial_udb_extra_f2_a->sue_status = mavlink_msg_serial_udb_extra_f2_a_get_sue_status(msg); -#else - memcpy(serial_udb_extra_f2_a, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h deleted file mode 100644 index 7cb8c87..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h +++ /dev/null @@ -1,881 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F2_B PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B 171 - -typedef struct __mavlink_serial_udb_extra_f2_b_t -{ - uint32_t sue_time; ///< Serial UDB Extra Time - uint32_t sue_flags; ///< Serial UDB Extra Status Flags - int16_t sue_pwm_input_1; ///< Serial UDB Extra PWM Input Channel 1 - int16_t sue_pwm_input_2; ///< Serial UDB Extra PWM Input Channel 2 - int16_t sue_pwm_input_3; ///< Serial UDB Extra PWM Input Channel 3 - int16_t sue_pwm_input_4; ///< Serial UDB Extra PWM Input Channel 4 - int16_t sue_pwm_input_5; ///< Serial UDB Extra PWM Input Channel 5 - int16_t sue_pwm_input_6; ///< Serial UDB Extra PWM Input Channel 6 - int16_t sue_pwm_input_7; ///< Serial UDB Extra PWM Input Channel 7 - int16_t sue_pwm_input_8; ///< Serial UDB Extra PWM Input Channel 8 - int16_t sue_pwm_input_9; ///< Serial UDB Extra PWM Input Channel 9 - int16_t sue_pwm_input_10; ///< Serial UDB Extra PWM Input Channel 10 - int16_t sue_pwm_output_1; ///< Serial UDB Extra PWM Output Channel 1 - int16_t sue_pwm_output_2; ///< Serial UDB Extra PWM Output Channel 2 - int16_t sue_pwm_output_3; ///< Serial UDB Extra PWM Output Channel 3 - int16_t sue_pwm_output_4; ///< Serial UDB Extra PWM Output Channel 4 - int16_t sue_pwm_output_5; ///< Serial UDB Extra PWM Output Channel 5 - int16_t sue_pwm_output_6; ///< Serial UDB Extra PWM Output Channel 6 - int16_t sue_pwm_output_7; ///< Serial UDB Extra PWM Output Channel 7 - int16_t sue_pwm_output_8; ///< Serial UDB Extra PWM Output Channel 8 - int16_t sue_pwm_output_9; ///< Serial UDB Extra PWM Output Channel 9 - int16_t sue_pwm_output_10; ///< Serial UDB Extra PWM Output Channel 10 - int16_t sue_imu_location_x; ///< Serial UDB Extra IMU Location X - int16_t sue_imu_location_y; ///< Serial UDB Extra IMU Location Y - int16_t sue_imu_location_z; ///< Serial UDB Extra IMU Location Z - int16_t sue_osc_fails; ///< Serial UDB Extra Oscillator Failure Count - int16_t sue_imu_velocity_x; ///< Serial UDB Extra IMU Velocity X - int16_t sue_imu_velocity_y; ///< Serial UDB Extra IMU Velocity Y - int16_t sue_imu_velocity_z; ///< Serial UDB Extra IMU Velocity Z - int16_t sue_waypoint_goal_x; ///< Serial UDB Extra Current Waypoint Goal X - int16_t sue_waypoint_goal_y; ///< Serial UDB Extra Current Waypoint Goal Y - int16_t sue_waypoint_goal_z; ///< Serial UDB Extra Current Waypoint Goal Z - int16_t sue_memory_stack_free; ///< Serial UDB Extra Stack Memory Free -} mavlink_serial_udb_extra_f2_b_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN 70 -#define MAVLINK_MSG_ID_171_LEN 70 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_CRC 169 -#define MAVLINK_MSG_ID_171_CRC 169 - - - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F2_B { \ - "SERIAL_UDB_EXTRA_F2_B", \ - 33, \ - { { "sue_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_time) }, \ - { "sue_flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_flags) }, \ - { "sue_pwm_input_1", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_1) }, \ - { "sue_pwm_input_2", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_2) }, \ - { "sue_pwm_input_3", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_3) }, \ - { "sue_pwm_input_4", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_4) }, \ - { "sue_pwm_input_5", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_5) }, \ - { "sue_pwm_input_6", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_6) }, \ - { "sue_pwm_input_7", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_7) }, \ - { "sue_pwm_input_8", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_8) }, \ - { "sue_pwm_input_9", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_9) }, \ - { "sue_pwm_input_10", NULL, MAVLINK_TYPE_INT16_T, 0, 26, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_10) }, \ - { "sue_pwm_output_1", NULL, MAVLINK_TYPE_INT16_T, 0, 28, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_1) }, \ - { "sue_pwm_output_2", NULL, MAVLINK_TYPE_INT16_T, 0, 30, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_2) }, \ - { "sue_pwm_output_3", NULL, MAVLINK_TYPE_INT16_T, 0, 32, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_3) }, \ - { "sue_pwm_output_4", NULL, MAVLINK_TYPE_INT16_T, 0, 34, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_4) }, \ - { "sue_pwm_output_5", NULL, MAVLINK_TYPE_INT16_T, 0, 36, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_5) }, \ - { "sue_pwm_output_6", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_6) }, \ - { "sue_pwm_output_7", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_7) }, \ - { "sue_pwm_output_8", NULL, MAVLINK_TYPE_INT16_T, 0, 42, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_8) }, \ - { "sue_pwm_output_9", NULL, MAVLINK_TYPE_INT16_T, 0, 44, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_9) }, \ - { "sue_pwm_output_10", NULL, MAVLINK_TYPE_INT16_T, 0, 46, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_10) }, \ - { "sue_imu_location_x", NULL, MAVLINK_TYPE_INT16_T, 0, 48, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_imu_location_x) }, \ - { "sue_imu_location_y", NULL, MAVLINK_TYPE_INT16_T, 0, 50, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_imu_location_y) }, \ - { "sue_imu_location_z", NULL, MAVLINK_TYPE_INT16_T, 0, 52, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_imu_location_z) }, \ - { "sue_osc_fails", NULL, MAVLINK_TYPE_INT16_T, 0, 54, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_osc_fails) }, \ - { "sue_imu_velocity_x", NULL, MAVLINK_TYPE_INT16_T, 0, 56, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_imu_velocity_x) }, \ - { "sue_imu_velocity_y", NULL, MAVLINK_TYPE_INT16_T, 0, 58, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_imu_velocity_y) }, \ - { "sue_imu_velocity_z", NULL, MAVLINK_TYPE_INT16_T, 0, 60, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_imu_velocity_z) }, \ - { "sue_waypoint_goal_x", NULL, MAVLINK_TYPE_INT16_T, 0, 62, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_waypoint_goal_x) }, \ - { "sue_waypoint_goal_y", NULL, MAVLINK_TYPE_INT16_T, 0, 64, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_waypoint_goal_y) }, \ - { "sue_waypoint_goal_z", NULL, MAVLINK_TYPE_INT16_T, 0, 66, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_waypoint_goal_z) }, \ - { "sue_memory_stack_free", NULL, MAVLINK_TYPE_INT16_T, 0, 68, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_memory_stack_free) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f2_b message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_time Serial UDB Extra Time - * @param sue_pwm_input_1 Serial UDB Extra PWM Input Channel 1 - * @param sue_pwm_input_2 Serial UDB Extra PWM Input Channel 2 - * @param sue_pwm_input_3 Serial UDB Extra PWM Input Channel 3 - * @param sue_pwm_input_4 Serial UDB Extra PWM Input Channel 4 - * @param sue_pwm_input_5 Serial UDB Extra PWM Input Channel 5 - * @param sue_pwm_input_6 Serial UDB Extra PWM Input Channel 6 - * @param sue_pwm_input_7 Serial UDB Extra PWM Input Channel 7 - * @param sue_pwm_input_8 Serial UDB Extra PWM Input Channel 8 - * @param sue_pwm_input_9 Serial UDB Extra PWM Input Channel 9 - * @param sue_pwm_input_10 Serial UDB Extra PWM Input Channel 10 - * @param sue_pwm_output_1 Serial UDB Extra PWM Output Channel 1 - * @param sue_pwm_output_2 Serial UDB Extra PWM Output Channel 2 - * @param sue_pwm_output_3 Serial UDB Extra PWM Output Channel 3 - * @param sue_pwm_output_4 Serial UDB Extra PWM Output Channel 4 - * @param sue_pwm_output_5 Serial UDB Extra PWM Output Channel 5 - * @param sue_pwm_output_6 Serial UDB Extra PWM Output Channel 6 - * @param sue_pwm_output_7 Serial UDB Extra PWM Output Channel 7 - * @param sue_pwm_output_8 Serial UDB Extra PWM Output Channel 8 - * @param sue_pwm_output_9 Serial UDB Extra PWM Output Channel 9 - * @param sue_pwm_output_10 Serial UDB Extra PWM Output Channel 10 - * @param sue_imu_location_x Serial UDB Extra IMU Location X - * @param sue_imu_location_y Serial UDB Extra IMU Location Y - * @param sue_imu_location_z Serial UDB Extra IMU Location Z - * @param sue_flags Serial UDB Extra Status Flags - * @param sue_osc_fails Serial UDB Extra Oscillator Failure Count - * @param sue_imu_velocity_x Serial UDB Extra IMU Velocity X - * @param sue_imu_velocity_y Serial UDB Extra IMU Velocity Y - * @param sue_imu_velocity_z Serial UDB Extra IMU Velocity Z - * @param sue_waypoint_goal_x Serial UDB Extra Current Waypoint Goal X - * @param sue_waypoint_goal_y Serial UDB Extra Current Waypoint Goal Y - * @param sue_waypoint_goal_z Serial UDB Extra Current Waypoint Goal Z - * @param sue_memory_stack_free Serial UDB Extra Stack Memory Free - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t sue_time, int16_t sue_pwm_input_1, int16_t sue_pwm_input_2, int16_t sue_pwm_input_3, int16_t sue_pwm_input_4, int16_t sue_pwm_input_5, int16_t sue_pwm_input_6, int16_t sue_pwm_input_7, int16_t sue_pwm_input_8, int16_t sue_pwm_input_9, int16_t sue_pwm_input_10, int16_t sue_pwm_output_1, int16_t sue_pwm_output_2, int16_t sue_pwm_output_3, int16_t sue_pwm_output_4, int16_t sue_pwm_output_5, int16_t sue_pwm_output_6, int16_t sue_pwm_output_7, int16_t sue_pwm_output_8, int16_t sue_pwm_output_9, int16_t sue_pwm_output_10, int16_t sue_imu_location_x, int16_t sue_imu_location_y, int16_t sue_imu_location_z, uint32_t sue_flags, int16_t sue_osc_fails, int16_t sue_imu_velocity_x, int16_t sue_imu_velocity_y, int16_t sue_imu_velocity_z, int16_t sue_waypoint_goal_x, int16_t sue_waypoint_goal_y, int16_t sue_waypoint_goal_z, int16_t sue_memory_stack_free) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN]; - _mav_put_uint32_t(buf, 0, sue_time); - _mav_put_uint32_t(buf, 4, sue_flags); - _mav_put_int16_t(buf, 8, sue_pwm_input_1); - _mav_put_int16_t(buf, 10, sue_pwm_input_2); - _mav_put_int16_t(buf, 12, sue_pwm_input_3); - _mav_put_int16_t(buf, 14, sue_pwm_input_4); - _mav_put_int16_t(buf, 16, sue_pwm_input_5); - _mav_put_int16_t(buf, 18, sue_pwm_input_6); - _mav_put_int16_t(buf, 20, sue_pwm_input_7); - _mav_put_int16_t(buf, 22, sue_pwm_input_8); - _mav_put_int16_t(buf, 24, sue_pwm_input_9); - _mav_put_int16_t(buf, 26, sue_pwm_input_10); - _mav_put_int16_t(buf, 28, sue_pwm_output_1); - _mav_put_int16_t(buf, 30, sue_pwm_output_2); - _mav_put_int16_t(buf, 32, sue_pwm_output_3); - _mav_put_int16_t(buf, 34, sue_pwm_output_4); - _mav_put_int16_t(buf, 36, sue_pwm_output_5); - _mav_put_int16_t(buf, 38, sue_pwm_output_6); - _mav_put_int16_t(buf, 40, sue_pwm_output_7); - _mav_put_int16_t(buf, 42, sue_pwm_output_8); - _mav_put_int16_t(buf, 44, sue_pwm_output_9); - _mav_put_int16_t(buf, 46, sue_pwm_output_10); - _mav_put_int16_t(buf, 48, sue_imu_location_x); - _mav_put_int16_t(buf, 50, sue_imu_location_y); - _mav_put_int16_t(buf, 52, sue_imu_location_z); - _mav_put_int16_t(buf, 54, sue_osc_fails); - _mav_put_int16_t(buf, 56, sue_imu_velocity_x); - _mav_put_int16_t(buf, 58, sue_imu_velocity_y); - _mav_put_int16_t(buf, 60, sue_imu_velocity_z); - _mav_put_int16_t(buf, 62, sue_waypoint_goal_x); - _mav_put_int16_t(buf, 64, sue_waypoint_goal_y); - _mav_put_int16_t(buf, 66, sue_waypoint_goal_z); - _mav_put_int16_t(buf, 68, sue_memory_stack_free); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); -#else - mavlink_serial_udb_extra_f2_b_t packet; - packet.sue_time = sue_time; - packet.sue_flags = sue_flags; - packet.sue_pwm_input_1 = sue_pwm_input_1; - packet.sue_pwm_input_2 = sue_pwm_input_2; - packet.sue_pwm_input_3 = sue_pwm_input_3; - packet.sue_pwm_input_4 = sue_pwm_input_4; - packet.sue_pwm_input_5 = sue_pwm_input_5; - packet.sue_pwm_input_6 = sue_pwm_input_6; - packet.sue_pwm_input_7 = sue_pwm_input_7; - packet.sue_pwm_input_8 = sue_pwm_input_8; - packet.sue_pwm_input_9 = sue_pwm_input_9; - packet.sue_pwm_input_10 = sue_pwm_input_10; - packet.sue_pwm_output_1 = sue_pwm_output_1; - packet.sue_pwm_output_2 = sue_pwm_output_2; - packet.sue_pwm_output_3 = sue_pwm_output_3; - packet.sue_pwm_output_4 = sue_pwm_output_4; - packet.sue_pwm_output_5 = sue_pwm_output_5; - packet.sue_pwm_output_6 = sue_pwm_output_6; - packet.sue_pwm_output_7 = sue_pwm_output_7; - packet.sue_pwm_output_8 = sue_pwm_output_8; - packet.sue_pwm_output_9 = sue_pwm_output_9; - packet.sue_pwm_output_10 = sue_pwm_output_10; - packet.sue_imu_location_x = sue_imu_location_x; - packet.sue_imu_location_y = sue_imu_location_y; - packet.sue_imu_location_z = sue_imu_location_z; - packet.sue_osc_fails = sue_osc_fails; - packet.sue_imu_velocity_x = sue_imu_velocity_x; - packet.sue_imu_velocity_y = sue_imu_velocity_y; - packet.sue_imu_velocity_z = sue_imu_velocity_z; - packet.sue_waypoint_goal_x = sue_waypoint_goal_x; - packet.sue_waypoint_goal_y = sue_waypoint_goal_y; - packet.sue_waypoint_goal_z = sue_waypoint_goal_z; - packet.sue_memory_stack_free = sue_memory_stack_free; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f2_b message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_time Serial UDB Extra Time - * @param sue_pwm_input_1 Serial UDB Extra PWM Input Channel 1 - * @param sue_pwm_input_2 Serial UDB Extra PWM Input Channel 2 - * @param sue_pwm_input_3 Serial UDB Extra PWM Input Channel 3 - * @param sue_pwm_input_4 Serial UDB Extra PWM Input Channel 4 - * @param sue_pwm_input_5 Serial UDB Extra PWM Input Channel 5 - * @param sue_pwm_input_6 Serial UDB Extra PWM Input Channel 6 - * @param sue_pwm_input_7 Serial UDB Extra PWM Input Channel 7 - * @param sue_pwm_input_8 Serial UDB Extra PWM Input Channel 8 - * @param sue_pwm_input_9 Serial UDB Extra PWM Input Channel 9 - * @param sue_pwm_input_10 Serial UDB Extra PWM Input Channel 10 - * @param sue_pwm_output_1 Serial UDB Extra PWM Output Channel 1 - * @param sue_pwm_output_2 Serial UDB Extra PWM Output Channel 2 - * @param sue_pwm_output_3 Serial UDB Extra PWM Output Channel 3 - * @param sue_pwm_output_4 Serial UDB Extra PWM Output Channel 4 - * @param sue_pwm_output_5 Serial UDB Extra PWM Output Channel 5 - * @param sue_pwm_output_6 Serial UDB Extra PWM Output Channel 6 - * @param sue_pwm_output_7 Serial UDB Extra PWM Output Channel 7 - * @param sue_pwm_output_8 Serial UDB Extra PWM Output Channel 8 - * @param sue_pwm_output_9 Serial UDB Extra PWM Output Channel 9 - * @param sue_pwm_output_10 Serial UDB Extra PWM Output Channel 10 - * @param sue_imu_location_x Serial UDB Extra IMU Location X - * @param sue_imu_location_y Serial UDB Extra IMU Location Y - * @param sue_imu_location_z Serial UDB Extra IMU Location Z - * @param sue_flags Serial UDB Extra Status Flags - * @param sue_osc_fails Serial UDB Extra Oscillator Failure Count - * @param sue_imu_velocity_x Serial UDB Extra IMU Velocity X - * @param sue_imu_velocity_y Serial UDB Extra IMU Velocity Y - * @param sue_imu_velocity_z Serial UDB Extra IMU Velocity Z - * @param sue_waypoint_goal_x Serial UDB Extra Current Waypoint Goal X - * @param sue_waypoint_goal_y Serial UDB Extra Current Waypoint Goal Y - * @param sue_waypoint_goal_z Serial UDB Extra Current Waypoint Goal Z - * @param sue_memory_stack_free Serial UDB Extra Stack Memory Free - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t sue_time,int16_t sue_pwm_input_1,int16_t sue_pwm_input_2,int16_t sue_pwm_input_3,int16_t sue_pwm_input_4,int16_t sue_pwm_input_5,int16_t sue_pwm_input_6,int16_t sue_pwm_input_7,int16_t sue_pwm_input_8,int16_t sue_pwm_input_9,int16_t sue_pwm_input_10,int16_t sue_pwm_output_1,int16_t sue_pwm_output_2,int16_t sue_pwm_output_3,int16_t sue_pwm_output_4,int16_t sue_pwm_output_5,int16_t sue_pwm_output_6,int16_t sue_pwm_output_7,int16_t sue_pwm_output_8,int16_t sue_pwm_output_9,int16_t sue_pwm_output_10,int16_t sue_imu_location_x,int16_t sue_imu_location_y,int16_t sue_imu_location_z,uint32_t sue_flags,int16_t sue_osc_fails,int16_t sue_imu_velocity_x,int16_t sue_imu_velocity_y,int16_t sue_imu_velocity_z,int16_t sue_waypoint_goal_x,int16_t sue_waypoint_goal_y,int16_t sue_waypoint_goal_z,int16_t sue_memory_stack_free) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN]; - _mav_put_uint32_t(buf, 0, sue_time); - _mav_put_uint32_t(buf, 4, sue_flags); - _mav_put_int16_t(buf, 8, sue_pwm_input_1); - _mav_put_int16_t(buf, 10, sue_pwm_input_2); - _mav_put_int16_t(buf, 12, sue_pwm_input_3); - _mav_put_int16_t(buf, 14, sue_pwm_input_4); - _mav_put_int16_t(buf, 16, sue_pwm_input_5); - _mav_put_int16_t(buf, 18, sue_pwm_input_6); - _mav_put_int16_t(buf, 20, sue_pwm_input_7); - _mav_put_int16_t(buf, 22, sue_pwm_input_8); - _mav_put_int16_t(buf, 24, sue_pwm_input_9); - _mav_put_int16_t(buf, 26, sue_pwm_input_10); - _mav_put_int16_t(buf, 28, sue_pwm_output_1); - _mav_put_int16_t(buf, 30, sue_pwm_output_2); - _mav_put_int16_t(buf, 32, sue_pwm_output_3); - _mav_put_int16_t(buf, 34, sue_pwm_output_4); - _mav_put_int16_t(buf, 36, sue_pwm_output_5); - _mav_put_int16_t(buf, 38, sue_pwm_output_6); - _mav_put_int16_t(buf, 40, sue_pwm_output_7); - _mav_put_int16_t(buf, 42, sue_pwm_output_8); - _mav_put_int16_t(buf, 44, sue_pwm_output_9); - _mav_put_int16_t(buf, 46, sue_pwm_output_10); - _mav_put_int16_t(buf, 48, sue_imu_location_x); - _mav_put_int16_t(buf, 50, sue_imu_location_y); - _mav_put_int16_t(buf, 52, sue_imu_location_z); - _mav_put_int16_t(buf, 54, sue_osc_fails); - _mav_put_int16_t(buf, 56, sue_imu_velocity_x); - _mav_put_int16_t(buf, 58, sue_imu_velocity_y); - _mav_put_int16_t(buf, 60, sue_imu_velocity_z); - _mav_put_int16_t(buf, 62, sue_waypoint_goal_x); - _mav_put_int16_t(buf, 64, sue_waypoint_goal_y); - _mav_put_int16_t(buf, 66, sue_waypoint_goal_z); - _mav_put_int16_t(buf, 68, sue_memory_stack_free); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); -#else - mavlink_serial_udb_extra_f2_b_t packet; - packet.sue_time = sue_time; - packet.sue_flags = sue_flags; - packet.sue_pwm_input_1 = sue_pwm_input_1; - packet.sue_pwm_input_2 = sue_pwm_input_2; - packet.sue_pwm_input_3 = sue_pwm_input_3; - packet.sue_pwm_input_4 = sue_pwm_input_4; - packet.sue_pwm_input_5 = sue_pwm_input_5; - packet.sue_pwm_input_6 = sue_pwm_input_6; - packet.sue_pwm_input_7 = sue_pwm_input_7; - packet.sue_pwm_input_8 = sue_pwm_input_8; - packet.sue_pwm_input_9 = sue_pwm_input_9; - packet.sue_pwm_input_10 = sue_pwm_input_10; - packet.sue_pwm_output_1 = sue_pwm_output_1; - packet.sue_pwm_output_2 = sue_pwm_output_2; - packet.sue_pwm_output_3 = sue_pwm_output_3; - packet.sue_pwm_output_4 = sue_pwm_output_4; - packet.sue_pwm_output_5 = sue_pwm_output_5; - packet.sue_pwm_output_6 = sue_pwm_output_6; - packet.sue_pwm_output_7 = sue_pwm_output_7; - packet.sue_pwm_output_8 = sue_pwm_output_8; - packet.sue_pwm_output_9 = sue_pwm_output_9; - packet.sue_pwm_output_10 = sue_pwm_output_10; - packet.sue_imu_location_x = sue_imu_location_x; - packet.sue_imu_location_y = sue_imu_location_y; - packet.sue_imu_location_z = sue_imu_location_z; - packet.sue_osc_fails = sue_osc_fails; - packet.sue_imu_velocity_x = sue_imu_velocity_x; - packet.sue_imu_velocity_y = sue_imu_velocity_y; - packet.sue_imu_velocity_z = sue_imu_velocity_z; - packet.sue_waypoint_goal_x = sue_waypoint_goal_x; - packet.sue_waypoint_goal_y = sue_waypoint_goal_y; - packet.sue_waypoint_goal_z = sue_waypoint_goal_z; - packet.sue_memory_stack_free = sue_memory_stack_free; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f2_b struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f2_b C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f2_b_t* serial_udb_extra_f2_b) -{ - return mavlink_msg_serial_udb_extra_f2_b_pack(system_id, component_id, msg, serial_udb_extra_f2_b->sue_time, serial_udb_extra_f2_b->sue_pwm_input_1, serial_udb_extra_f2_b->sue_pwm_input_2, serial_udb_extra_f2_b->sue_pwm_input_3, serial_udb_extra_f2_b->sue_pwm_input_4, serial_udb_extra_f2_b->sue_pwm_input_5, serial_udb_extra_f2_b->sue_pwm_input_6, serial_udb_extra_f2_b->sue_pwm_input_7, serial_udb_extra_f2_b->sue_pwm_input_8, serial_udb_extra_f2_b->sue_pwm_input_9, serial_udb_extra_f2_b->sue_pwm_input_10, serial_udb_extra_f2_b->sue_pwm_output_1, serial_udb_extra_f2_b->sue_pwm_output_2, serial_udb_extra_f2_b->sue_pwm_output_3, serial_udb_extra_f2_b->sue_pwm_output_4, serial_udb_extra_f2_b->sue_pwm_output_5, serial_udb_extra_f2_b->sue_pwm_output_6, serial_udb_extra_f2_b->sue_pwm_output_7, serial_udb_extra_f2_b->sue_pwm_output_8, serial_udb_extra_f2_b->sue_pwm_output_9, serial_udb_extra_f2_b->sue_pwm_output_10, serial_udb_extra_f2_b->sue_imu_location_x, serial_udb_extra_f2_b->sue_imu_location_y, serial_udb_extra_f2_b->sue_imu_location_z, serial_udb_extra_f2_b->sue_flags, serial_udb_extra_f2_b->sue_osc_fails, serial_udb_extra_f2_b->sue_imu_velocity_x, serial_udb_extra_f2_b->sue_imu_velocity_y, serial_udb_extra_f2_b->sue_imu_velocity_z, serial_udb_extra_f2_b->sue_waypoint_goal_x, serial_udb_extra_f2_b->sue_waypoint_goal_y, serial_udb_extra_f2_b->sue_waypoint_goal_z, serial_udb_extra_f2_b->sue_memory_stack_free); -} - -/** - * @brief Encode a serial_udb_extra_f2_b struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f2_b C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f2_b_t* serial_udb_extra_f2_b) -{ - return mavlink_msg_serial_udb_extra_f2_b_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f2_b->sue_time, serial_udb_extra_f2_b->sue_pwm_input_1, serial_udb_extra_f2_b->sue_pwm_input_2, serial_udb_extra_f2_b->sue_pwm_input_3, serial_udb_extra_f2_b->sue_pwm_input_4, serial_udb_extra_f2_b->sue_pwm_input_5, serial_udb_extra_f2_b->sue_pwm_input_6, serial_udb_extra_f2_b->sue_pwm_input_7, serial_udb_extra_f2_b->sue_pwm_input_8, serial_udb_extra_f2_b->sue_pwm_input_9, serial_udb_extra_f2_b->sue_pwm_input_10, serial_udb_extra_f2_b->sue_pwm_output_1, serial_udb_extra_f2_b->sue_pwm_output_2, serial_udb_extra_f2_b->sue_pwm_output_3, serial_udb_extra_f2_b->sue_pwm_output_4, serial_udb_extra_f2_b->sue_pwm_output_5, serial_udb_extra_f2_b->sue_pwm_output_6, serial_udb_extra_f2_b->sue_pwm_output_7, serial_udb_extra_f2_b->sue_pwm_output_8, serial_udb_extra_f2_b->sue_pwm_output_9, serial_udb_extra_f2_b->sue_pwm_output_10, serial_udb_extra_f2_b->sue_imu_location_x, serial_udb_extra_f2_b->sue_imu_location_y, serial_udb_extra_f2_b->sue_imu_location_z, serial_udb_extra_f2_b->sue_flags, serial_udb_extra_f2_b->sue_osc_fails, serial_udb_extra_f2_b->sue_imu_velocity_x, serial_udb_extra_f2_b->sue_imu_velocity_y, serial_udb_extra_f2_b->sue_imu_velocity_z, serial_udb_extra_f2_b->sue_waypoint_goal_x, serial_udb_extra_f2_b->sue_waypoint_goal_y, serial_udb_extra_f2_b->sue_waypoint_goal_z, serial_udb_extra_f2_b->sue_memory_stack_free); -} - -/** - * @brief Send a serial_udb_extra_f2_b message - * @param chan MAVLink channel to send the message - * - * @param sue_time Serial UDB Extra Time - * @param sue_pwm_input_1 Serial UDB Extra PWM Input Channel 1 - * @param sue_pwm_input_2 Serial UDB Extra PWM Input Channel 2 - * @param sue_pwm_input_3 Serial UDB Extra PWM Input Channel 3 - * @param sue_pwm_input_4 Serial UDB Extra PWM Input Channel 4 - * @param sue_pwm_input_5 Serial UDB Extra PWM Input Channel 5 - * @param sue_pwm_input_6 Serial UDB Extra PWM Input Channel 6 - * @param sue_pwm_input_7 Serial UDB Extra PWM Input Channel 7 - * @param sue_pwm_input_8 Serial UDB Extra PWM Input Channel 8 - * @param sue_pwm_input_9 Serial UDB Extra PWM Input Channel 9 - * @param sue_pwm_input_10 Serial UDB Extra PWM Input Channel 10 - * @param sue_pwm_output_1 Serial UDB Extra PWM Output Channel 1 - * @param sue_pwm_output_2 Serial UDB Extra PWM Output Channel 2 - * @param sue_pwm_output_3 Serial UDB Extra PWM Output Channel 3 - * @param sue_pwm_output_4 Serial UDB Extra PWM Output Channel 4 - * @param sue_pwm_output_5 Serial UDB Extra PWM Output Channel 5 - * @param sue_pwm_output_6 Serial UDB Extra PWM Output Channel 6 - * @param sue_pwm_output_7 Serial UDB Extra PWM Output Channel 7 - * @param sue_pwm_output_8 Serial UDB Extra PWM Output Channel 8 - * @param sue_pwm_output_9 Serial UDB Extra PWM Output Channel 9 - * @param sue_pwm_output_10 Serial UDB Extra PWM Output Channel 10 - * @param sue_imu_location_x Serial UDB Extra IMU Location X - * @param sue_imu_location_y Serial UDB Extra IMU Location Y - * @param sue_imu_location_z Serial UDB Extra IMU Location Z - * @param sue_flags Serial UDB Extra Status Flags - * @param sue_osc_fails Serial UDB Extra Oscillator Failure Count - * @param sue_imu_velocity_x Serial UDB Extra IMU Velocity X - * @param sue_imu_velocity_y Serial UDB Extra IMU Velocity Y - * @param sue_imu_velocity_z Serial UDB Extra IMU Velocity Z - * @param sue_waypoint_goal_x Serial UDB Extra Current Waypoint Goal X - * @param sue_waypoint_goal_y Serial UDB Extra Current Waypoint Goal Y - * @param sue_waypoint_goal_z Serial UDB Extra Current Waypoint Goal Z - * @param sue_memory_stack_free Serial UDB Extra Stack Memory Free - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f2_b_send(mavlink_channel_t chan, uint32_t sue_time, int16_t sue_pwm_input_1, int16_t sue_pwm_input_2, int16_t sue_pwm_input_3, int16_t sue_pwm_input_4, int16_t sue_pwm_input_5, int16_t sue_pwm_input_6, int16_t sue_pwm_input_7, int16_t sue_pwm_input_8, int16_t sue_pwm_input_9, int16_t sue_pwm_input_10, int16_t sue_pwm_output_1, int16_t sue_pwm_output_2, int16_t sue_pwm_output_3, int16_t sue_pwm_output_4, int16_t sue_pwm_output_5, int16_t sue_pwm_output_6, int16_t sue_pwm_output_7, int16_t sue_pwm_output_8, int16_t sue_pwm_output_9, int16_t sue_pwm_output_10, int16_t sue_imu_location_x, int16_t sue_imu_location_y, int16_t sue_imu_location_z, uint32_t sue_flags, int16_t sue_osc_fails, int16_t sue_imu_velocity_x, int16_t sue_imu_velocity_y, int16_t sue_imu_velocity_z, int16_t sue_waypoint_goal_x, int16_t sue_waypoint_goal_y, int16_t sue_waypoint_goal_z, int16_t sue_memory_stack_free) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN]; - _mav_put_uint32_t(buf, 0, sue_time); - _mav_put_uint32_t(buf, 4, sue_flags); - _mav_put_int16_t(buf, 8, sue_pwm_input_1); - _mav_put_int16_t(buf, 10, sue_pwm_input_2); - _mav_put_int16_t(buf, 12, sue_pwm_input_3); - _mav_put_int16_t(buf, 14, sue_pwm_input_4); - _mav_put_int16_t(buf, 16, sue_pwm_input_5); - _mav_put_int16_t(buf, 18, sue_pwm_input_6); - _mav_put_int16_t(buf, 20, sue_pwm_input_7); - _mav_put_int16_t(buf, 22, sue_pwm_input_8); - _mav_put_int16_t(buf, 24, sue_pwm_input_9); - _mav_put_int16_t(buf, 26, sue_pwm_input_10); - _mav_put_int16_t(buf, 28, sue_pwm_output_1); - _mav_put_int16_t(buf, 30, sue_pwm_output_2); - _mav_put_int16_t(buf, 32, sue_pwm_output_3); - _mav_put_int16_t(buf, 34, sue_pwm_output_4); - _mav_put_int16_t(buf, 36, sue_pwm_output_5); - _mav_put_int16_t(buf, 38, sue_pwm_output_6); - _mav_put_int16_t(buf, 40, sue_pwm_output_7); - _mav_put_int16_t(buf, 42, sue_pwm_output_8); - _mav_put_int16_t(buf, 44, sue_pwm_output_9); - _mav_put_int16_t(buf, 46, sue_pwm_output_10); - _mav_put_int16_t(buf, 48, sue_imu_location_x); - _mav_put_int16_t(buf, 50, sue_imu_location_y); - _mav_put_int16_t(buf, 52, sue_imu_location_z); - _mav_put_int16_t(buf, 54, sue_osc_fails); - _mav_put_int16_t(buf, 56, sue_imu_velocity_x); - _mav_put_int16_t(buf, 58, sue_imu_velocity_y); - _mav_put_int16_t(buf, 60, sue_imu_velocity_z); - _mav_put_int16_t(buf, 62, sue_waypoint_goal_x); - _mav_put_int16_t(buf, 64, sue_waypoint_goal_y); - _mav_put_int16_t(buf, 66, sue_waypoint_goal_z); - _mav_put_int16_t(buf, 68, sue_memory_stack_free); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); -#endif -#else - mavlink_serial_udb_extra_f2_b_t packet; - packet.sue_time = sue_time; - packet.sue_flags = sue_flags; - packet.sue_pwm_input_1 = sue_pwm_input_1; - packet.sue_pwm_input_2 = sue_pwm_input_2; - packet.sue_pwm_input_3 = sue_pwm_input_3; - packet.sue_pwm_input_4 = sue_pwm_input_4; - packet.sue_pwm_input_5 = sue_pwm_input_5; - packet.sue_pwm_input_6 = sue_pwm_input_6; - packet.sue_pwm_input_7 = sue_pwm_input_7; - packet.sue_pwm_input_8 = sue_pwm_input_8; - packet.sue_pwm_input_9 = sue_pwm_input_9; - packet.sue_pwm_input_10 = sue_pwm_input_10; - packet.sue_pwm_output_1 = sue_pwm_output_1; - packet.sue_pwm_output_2 = sue_pwm_output_2; - packet.sue_pwm_output_3 = sue_pwm_output_3; - packet.sue_pwm_output_4 = sue_pwm_output_4; - packet.sue_pwm_output_5 = sue_pwm_output_5; - packet.sue_pwm_output_6 = sue_pwm_output_6; - packet.sue_pwm_output_7 = sue_pwm_output_7; - packet.sue_pwm_output_8 = sue_pwm_output_8; - packet.sue_pwm_output_9 = sue_pwm_output_9; - packet.sue_pwm_output_10 = sue_pwm_output_10; - packet.sue_imu_location_x = sue_imu_location_x; - packet.sue_imu_location_y = sue_imu_location_y; - packet.sue_imu_location_z = sue_imu_location_z; - packet.sue_osc_fails = sue_osc_fails; - packet.sue_imu_velocity_x = sue_imu_velocity_x; - packet.sue_imu_velocity_y = sue_imu_velocity_y; - packet.sue_imu_velocity_z = sue_imu_velocity_z; - packet.sue_waypoint_goal_x = sue_waypoint_goal_x; - packet.sue_waypoint_goal_y = sue_waypoint_goal_y; - packet.sue_waypoint_goal_z = sue_waypoint_goal_z; - packet.sue_memory_stack_free = sue_memory_stack_free; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F2_B UNPACKING - - -/** - * @brief Get field sue_time from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra Time - */ -static inline uint32_t mavlink_msg_serial_udb_extra_f2_b_get_sue_time(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field sue_pwm_input_1 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Input Channel 1 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 8); -} - -/** - * @brief Get field sue_pwm_input_2 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Input Channel 2 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 10); -} - -/** - * @brief Get field sue_pwm_input_3 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Input Channel 3 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 12); -} - -/** - * @brief Get field sue_pwm_input_4 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Input Channel 4 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 14); -} - -/** - * @brief Get field sue_pwm_input_5 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Input Channel 5 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 16); -} - -/** - * @brief Get field sue_pwm_input_6 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Input Channel 6 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 18); -} - -/** - * @brief Get field sue_pwm_input_7 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Input Channel 7 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_7(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 20); -} - -/** - * @brief Get field sue_pwm_input_8 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Input Channel 8 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_8(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 22); -} - -/** - * @brief Get field sue_pwm_input_9 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Input Channel 9 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_9(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 24); -} - -/** - * @brief Get field sue_pwm_input_10 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Input Channel 10 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_10(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 26); -} - -/** - * @brief Get field sue_pwm_output_1 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Output Channel 1 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 28); -} - -/** - * @brief Get field sue_pwm_output_2 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Output Channel 2 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 30); -} - -/** - * @brief Get field sue_pwm_output_3 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Output Channel 3 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 32); -} - -/** - * @brief Get field sue_pwm_output_4 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Output Channel 4 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 34); -} - -/** - * @brief Get field sue_pwm_output_5 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Output Channel 5 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 36); -} - -/** - * @brief Get field sue_pwm_output_6 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Output Channel 6 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 38); -} - -/** - * @brief Get field sue_pwm_output_7 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Output Channel 7 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_7(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 40); -} - -/** - * @brief Get field sue_pwm_output_8 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Output Channel 8 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_8(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 42); -} - -/** - * @brief Get field sue_pwm_output_9 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Output Channel 9 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_9(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 44); -} - -/** - * @brief Get field sue_pwm_output_10 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Output Channel 10 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_10(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 46); -} - -/** - * @brief Get field sue_imu_location_x from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra IMU Location X - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_location_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 48); -} - -/** - * @brief Get field sue_imu_location_y from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra IMU Location Y - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_location_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 50); -} - -/** - * @brief Get field sue_imu_location_z from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra IMU Location Z - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_location_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 52); -} - -/** - * @brief Get field sue_flags from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra Status Flags - */ -static inline uint32_t mavlink_msg_serial_udb_extra_f2_b_get_sue_flags(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 4); -} - -/** - * @brief Get field sue_osc_fails from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra Oscillator Failure Count - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_osc_fails(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 54); -} - -/** - * @brief Get field sue_imu_velocity_x from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra IMU Velocity X - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_velocity_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 56); -} - -/** - * @brief Get field sue_imu_velocity_y from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra IMU Velocity Y - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_velocity_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 58); -} - -/** - * @brief Get field sue_imu_velocity_z from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra IMU Velocity Z - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_velocity_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 60); -} - -/** - * @brief Get field sue_waypoint_goal_x from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra Current Waypoint Goal X - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_waypoint_goal_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 62); -} - -/** - * @brief Get field sue_waypoint_goal_y from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra Current Waypoint Goal Y - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_waypoint_goal_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 64); -} - -/** - * @brief Get field sue_waypoint_goal_z from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra Current Waypoint Goal Z - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_waypoint_goal_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 66); -} - -/** - * @brief Get field sue_memory_stack_free from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra Stack Memory Free - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_memory_stack_free(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 68); -} - -/** - * @brief Decode a serial_udb_extra_f2_b message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f2_b C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f2_b_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f2_b_t* serial_udb_extra_f2_b) -{ -#if MAVLINK_NEED_BYTE_SWAP - serial_udb_extra_f2_b->sue_time = mavlink_msg_serial_udb_extra_f2_b_get_sue_time(msg); - serial_udb_extra_f2_b->sue_flags = mavlink_msg_serial_udb_extra_f2_b_get_sue_flags(msg); - serial_udb_extra_f2_b->sue_pwm_input_1 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_1(msg); - serial_udb_extra_f2_b->sue_pwm_input_2 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_2(msg); - serial_udb_extra_f2_b->sue_pwm_input_3 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_3(msg); - serial_udb_extra_f2_b->sue_pwm_input_4 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_4(msg); - serial_udb_extra_f2_b->sue_pwm_input_5 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_5(msg); - serial_udb_extra_f2_b->sue_pwm_input_6 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_6(msg); - serial_udb_extra_f2_b->sue_pwm_input_7 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_7(msg); - serial_udb_extra_f2_b->sue_pwm_input_8 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_8(msg); - serial_udb_extra_f2_b->sue_pwm_input_9 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_9(msg); - serial_udb_extra_f2_b->sue_pwm_input_10 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_10(msg); - serial_udb_extra_f2_b->sue_pwm_output_1 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_1(msg); - serial_udb_extra_f2_b->sue_pwm_output_2 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_2(msg); - serial_udb_extra_f2_b->sue_pwm_output_3 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_3(msg); - serial_udb_extra_f2_b->sue_pwm_output_4 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_4(msg); - serial_udb_extra_f2_b->sue_pwm_output_5 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_5(msg); - serial_udb_extra_f2_b->sue_pwm_output_6 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_6(msg); - serial_udb_extra_f2_b->sue_pwm_output_7 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_7(msg); - serial_udb_extra_f2_b->sue_pwm_output_8 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_8(msg); - serial_udb_extra_f2_b->sue_pwm_output_9 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_9(msg); - serial_udb_extra_f2_b->sue_pwm_output_10 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_10(msg); - serial_udb_extra_f2_b->sue_imu_location_x = mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_location_x(msg); - serial_udb_extra_f2_b->sue_imu_location_y = mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_location_y(msg); - serial_udb_extra_f2_b->sue_imu_location_z = mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_location_z(msg); - serial_udb_extra_f2_b->sue_osc_fails = mavlink_msg_serial_udb_extra_f2_b_get_sue_osc_fails(msg); - serial_udb_extra_f2_b->sue_imu_velocity_x = mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_velocity_x(msg); - serial_udb_extra_f2_b->sue_imu_velocity_y = mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_velocity_y(msg); - serial_udb_extra_f2_b->sue_imu_velocity_z = mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_velocity_z(msg); - serial_udb_extra_f2_b->sue_waypoint_goal_x = mavlink_msg_serial_udb_extra_f2_b_get_sue_waypoint_goal_x(msg); - serial_udb_extra_f2_b->sue_waypoint_goal_y = mavlink_msg_serial_udb_extra_f2_b_get_sue_waypoint_goal_y(msg); - serial_udb_extra_f2_b->sue_waypoint_goal_z = mavlink_msg_serial_udb_extra_f2_b_get_sue_waypoint_goal_z(msg); - serial_udb_extra_f2_b->sue_memory_stack_free = mavlink_msg_serial_udb_extra_f2_b_get_sue_memory_stack_free(msg); -#else - memcpy(serial_udb_extra_f2_b, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h deleted file mode 100644 index 77c616c..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h +++ /dev/null @@ -1,375 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F4 PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4 172 - -typedef struct __mavlink_serial_udb_extra_f4_t -{ - uint8_t sue_ROLL_STABILIZATION_AILERONS; ///< Serial UDB Extra Roll Stabilization with Ailerons Enabled - uint8_t sue_ROLL_STABILIZATION_RUDDER; ///< Serial UDB Extra Roll Stabilization with Rudder Enabled - uint8_t sue_PITCH_STABILIZATION; ///< Serial UDB Extra Pitch Stabilization Enabled - uint8_t sue_YAW_STABILIZATION_RUDDER; ///< Serial UDB Extra Yaw Stabilization using Rudder Enabled - uint8_t sue_YAW_STABILIZATION_AILERON; ///< Serial UDB Extra Yaw Stabilization using Ailerons Enabled - uint8_t sue_AILERON_NAVIGATION; ///< Serial UDB Extra Navigation with Ailerons Enabled - uint8_t sue_RUDDER_NAVIGATION; ///< Serial UDB Extra Navigation with Rudder Enabled - uint8_t sue_ALTITUDEHOLD_STABILIZED; ///< Serial UDB Extra Type of Alitude Hold when in Stabilized Mode - uint8_t sue_ALTITUDEHOLD_WAYPOINT; ///< Serial UDB Extra Type of Alitude Hold when in Waypoint Mode - uint8_t sue_RACING_MODE; ///< Serial UDB Extra Firmware racing mode enabled -} mavlink_serial_udb_extra_f4_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN 10 -#define MAVLINK_MSG_ID_172_LEN 10 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_CRC 191 -#define MAVLINK_MSG_ID_172_CRC 191 - - - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F4 { \ - "SERIAL_UDB_EXTRA_F4", \ - 10, \ - { { "sue_ROLL_STABILIZATION_AILERONS", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_serial_udb_extra_f4_t, sue_ROLL_STABILIZATION_AILERONS) }, \ - { "sue_ROLL_STABILIZATION_RUDDER", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_serial_udb_extra_f4_t, sue_ROLL_STABILIZATION_RUDDER) }, \ - { "sue_PITCH_STABILIZATION", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_serial_udb_extra_f4_t, sue_PITCH_STABILIZATION) }, \ - { "sue_YAW_STABILIZATION_RUDDER", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_serial_udb_extra_f4_t, sue_YAW_STABILIZATION_RUDDER) }, \ - { "sue_YAW_STABILIZATION_AILERON", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_serial_udb_extra_f4_t, sue_YAW_STABILIZATION_AILERON) }, \ - { "sue_AILERON_NAVIGATION", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_serial_udb_extra_f4_t, sue_AILERON_NAVIGATION) }, \ - { "sue_RUDDER_NAVIGATION", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_serial_udb_extra_f4_t, sue_RUDDER_NAVIGATION) }, \ - { "sue_ALTITUDEHOLD_STABILIZED", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_serial_udb_extra_f4_t, sue_ALTITUDEHOLD_STABILIZED) }, \ - { "sue_ALTITUDEHOLD_WAYPOINT", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_serial_udb_extra_f4_t, sue_ALTITUDEHOLD_WAYPOINT) }, \ - { "sue_RACING_MODE", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_serial_udb_extra_f4_t, sue_RACING_MODE) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f4 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_ROLL_STABILIZATION_AILERONS Serial UDB Extra Roll Stabilization with Ailerons Enabled - * @param sue_ROLL_STABILIZATION_RUDDER Serial UDB Extra Roll Stabilization with Rudder Enabled - * @param sue_PITCH_STABILIZATION Serial UDB Extra Pitch Stabilization Enabled - * @param sue_YAW_STABILIZATION_RUDDER Serial UDB Extra Yaw Stabilization using Rudder Enabled - * @param sue_YAW_STABILIZATION_AILERON Serial UDB Extra Yaw Stabilization using Ailerons Enabled - * @param sue_AILERON_NAVIGATION Serial UDB Extra Navigation with Ailerons Enabled - * @param sue_RUDDER_NAVIGATION Serial UDB Extra Navigation with Rudder Enabled - * @param sue_ALTITUDEHOLD_STABILIZED Serial UDB Extra Type of Alitude Hold when in Stabilized Mode - * @param sue_ALTITUDEHOLD_WAYPOINT Serial UDB Extra Type of Alitude Hold when in Waypoint Mode - * @param sue_RACING_MODE Serial UDB Extra Firmware racing mode enabled - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f4_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t sue_ROLL_STABILIZATION_AILERONS, uint8_t sue_ROLL_STABILIZATION_RUDDER, uint8_t sue_PITCH_STABILIZATION, uint8_t sue_YAW_STABILIZATION_RUDDER, uint8_t sue_YAW_STABILIZATION_AILERON, uint8_t sue_AILERON_NAVIGATION, uint8_t sue_RUDDER_NAVIGATION, uint8_t sue_ALTITUDEHOLD_STABILIZED, uint8_t sue_ALTITUDEHOLD_WAYPOINT, uint8_t sue_RACING_MODE) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN]; - _mav_put_uint8_t(buf, 0, sue_ROLL_STABILIZATION_AILERONS); - _mav_put_uint8_t(buf, 1, sue_ROLL_STABILIZATION_RUDDER); - _mav_put_uint8_t(buf, 2, sue_PITCH_STABILIZATION); - _mav_put_uint8_t(buf, 3, sue_YAW_STABILIZATION_RUDDER); - _mav_put_uint8_t(buf, 4, sue_YAW_STABILIZATION_AILERON); - _mav_put_uint8_t(buf, 5, sue_AILERON_NAVIGATION); - _mav_put_uint8_t(buf, 6, sue_RUDDER_NAVIGATION); - _mav_put_uint8_t(buf, 7, sue_ALTITUDEHOLD_STABILIZED); - _mav_put_uint8_t(buf, 8, sue_ALTITUDEHOLD_WAYPOINT); - _mav_put_uint8_t(buf, 9, sue_RACING_MODE); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); -#else - mavlink_serial_udb_extra_f4_t packet; - packet.sue_ROLL_STABILIZATION_AILERONS = sue_ROLL_STABILIZATION_AILERONS; - packet.sue_ROLL_STABILIZATION_RUDDER = sue_ROLL_STABILIZATION_RUDDER; - packet.sue_PITCH_STABILIZATION = sue_PITCH_STABILIZATION; - packet.sue_YAW_STABILIZATION_RUDDER = sue_YAW_STABILIZATION_RUDDER; - packet.sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON; - packet.sue_AILERON_NAVIGATION = sue_AILERON_NAVIGATION; - packet.sue_RUDDER_NAVIGATION = sue_RUDDER_NAVIGATION; - packet.sue_ALTITUDEHOLD_STABILIZED = sue_ALTITUDEHOLD_STABILIZED; - packet.sue_ALTITUDEHOLD_WAYPOINT = sue_ALTITUDEHOLD_WAYPOINT; - packet.sue_RACING_MODE = sue_RACING_MODE; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f4 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_ROLL_STABILIZATION_AILERONS Serial UDB Extra Roll Stabilization with Ailerons Enabled - * @param sue_ROLL_STABILIZATION_RUDDER Serial UDB Extra Roll Stabilization with Rudder Enabled - * @param sue_PITCH_STABILIZATION Serial UDB Extra Pitch Stabilization Enabled - * @param sue_YAW_STABILIZATION_RUDDER Serial UDB Extra Yaw Stabilization using Rudder Enabled - * @param sue_YAW_STABILIZATION_AILERON Serial UDB Extra Yaw Stabilization using Ailerons Enabled - * @param sue_AILERON_NAVIGATION Serial UDB Extra Navigation with Ailerons Enabled - * @param sue_RUDDER_NAVIGATION Serial UDB Extra Navigation with Rudder Enabled - * @param sue_ALTITUDEHOLD_STABILIZED Serial UDB Extra Type of Alitude Hold when in Stabilized Mode - * @param sue_ALTITUDEHOLD_WAYPOINT Serial UDB Extra Type of Alitude Hold when in Waypoint Mode - * @param sue_RACING_MODE Serial UDB Extra Firmware racing mode enabled - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f4_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t sue_ROLL_STABILIZATION_AILERONS,uint8_t sue_ROLL_STABILIZATION_RUDDER,uint8_t sue_PITCH_STABILIZATION,uint8_t sue_YAW_STABILIZATION_RUDDER,uint8_t sue_YAW_STABILIZATION_AILERON,uint8_t sue_AILERON_NAVIGATION,uint8_t sue_RUDDER_NAVIGATION,uint8_t sue_ALTITUDEHOLD_STABILIZED,uint8_t sue_ALTITUDEHOLD_WAYPOINT,uint8_t sue_RACING_MODE) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN]; - _mav_put_uint8_t(buf, 0, sue_ROLL_STABILIZATION_AILERONS); - _mav_put_uint8_t(buf, 1, sue_ROLL_STABILIZATION_RUDDER); - _mav_put_uint8_t(buf, 2, sue_PITCH_STABILIZATION); - _mav_put_uint8_t(buf, 3, sue_YAW_STABILIZATION_RUDDER); - _mav_put_uint8_t(buf, 4, sue_YAW_STABILIZATION_AILERON); - _mav_put_uint8_t(buf, 5, sue_AILERON_NAVIGATION); - _mav_put_uint8_t(buf, 6, sue_RUDDER_NAVIGATION); - _mav_put_uint8_t(buf, 7, sue_ALTITUDEHOLD_STABILIZED); - _mav_put_uint8_t(buf, 8, sue_ALTITUDEHOLD_WAYPOINT); - _mav_put_uint8_t(buf, 9, sue_RACING_MODE); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); -#else - mavlink_serial_udb_extra_f4_t packet; - packet.sue_ROLL_STABILIZATION_AILERONS = sue_ROLL_STABILIZATION_AILERONS; - packet.sue_ROLL_STABILIZATION_RUDDER = sue_ROLL_STABILIZATION_RUDDER; - packet.sue_PITCH_STABILIZATION = sue_PITCH_STABILIZATION; - packet.sue_YAW_STABILIZATION_RUDDER = sue_YAW_STABILIZATION_RUDDER; - packet.sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON; - packet.sue_AILERON_NAVIGATION = sue_AILERON_NAVIGATION; - packet.sue_RUDDER_NAVIGATION = sue_RUDDER_NAVIGATION; - packet.sue_ALTITUDEHOLD_STABILIZED = sue_ALTITUDEHOLD_STABILIZED; - packet.sue_ALTITUDEHOLD_WAYPOINT = sue_ALTITUDEHOLD_WAYPOINT; - packet.sue_RACING_MODE = sue_RACING_MODE; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f4 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f4 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f4_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f4_t* serial_udb_extra_f4) -{ - return mavlink_msg_serial_udb_extra_f4_pack(system_id, component_id, msg, serial_udb_extra_f4->sue_ROLL_STABILIZATION_AILERONS, serial_udb_extra_f4->sue_ROLL_STABILIZATION_RUDDER, serial_udb_extra_f4->sue_PITCH_STABILIZATION, serial_udb_extra_f4->sue_YAW_STABILIZATION_RUDDER, serial_udb_extra_f4->sue_YAW_STABILIZATION_AILERON, serial_udb_extra_f4->sue_AILERON_NAVIGATION, serial_udb_extra_f4->sue_RUDDER_NAVIGATION, serial_udb_extra_f4->sue_ALTITUDEHOLD_STABILIZED, serial_udb_extra_f4->sue_ALTITUDEHOLD_WAYPOINT, serial_udb_extra_f4->sue_RACING_MODE); -} - -/** - * @brief Encode a serial_udb_extra_f4 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f4 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f4_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f4_t* serial_udb_extra_f4) -{ - return mavlink_msg_serial_udb_extra_f4_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f4->sue_ROLL_STABILIZATION_AILERONS, serial_udb_extra_f4->sue_ROLL_STABILIZATION_RUDDER, serial_udb_extra_f4->sue_PITCH_STABILIZATION, serial_udb_extra_f4->sue_YAW_STABILIZATION_RUDDER, serial_udb_extra_f4->sue_YAW_STABILIZATION_AILERON, serial_udb_extra_f4->sue_AILERON_NAVIGATION, serial_udb_extra_f4->sue_RUDDER_NAVIGATION, serial_udb_extra_f4->sue_ALTITUDEHOLD_STABILIZED, serial_udb_extra_f4->sue_ALTITUDEHOLD_WAYPOINT, serial_udb_extra_f4->sue_RACING_MODE); -} - -/** - * @brief Send a serial_udb_extra_f4 message - * @param chan MAVLink channel to send the message - * - * @param sue_ROLL_STABILIZATION_AILERONS Serial UDB Extra Roll Stabilization with Ailerons Enabled - * @param sue_ROLL_STABILIZATION_RUDDER Serial UDB Extra Roll Stabilization with Rudder Enabled - * @param sue_PITCH_STABILIZATION Serial UDB Extra Pitch Stabilization Enabled - * @param sue_YAW_STABILIZATION_RUDDER Serial UDB Extra Yaw Stabilization using Rudder Enabled - * @param sue_YAW_STABILIZATION_AILERON Serial UDB Extra Yaw Stabilization using Ailerons Enabled - * @param sue_AILERON_NAVIGATION Serial UDB Extra Navigation with Ailerons Enabled - * @param sue_RUDDER_NAVIGATION Serial UDB Extra Navigation with Rudder Enabled - * @param sue_ALTITUDEHOLD_STABILIZED Serial UDB Extra Type of Alitude Hold when in Stabilized Mode - * @param sue_ALTITUDEHOLD_WAYPOINT Serial UDB Extra Type of Alitude Hold when in Waypoint Mode - * @param sue_RACING_MODE Serial UDB Extra Firmware racing mode enabled - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f4_send(mavlink_channel_t chan, uint8_t sue_ROLL_STABILIZATION_AILERONS, uint8_t sue_ROLL_STABILIZATION_RUDDER, uint8_t sue_PITCH_STABILIZATION, uint8_t sue_YAW_STABILIZATION_RUDDER, uint8_t sue_YAW_STABILIZATION_AILERON, uint8_t sue_AILERON_NAVIGATION, uint8_t sue_RUDDER_NAVIGATION, uint8_t sue_ALTITUDEHOLD_STABILIZED, uint8_t sue_ALTITUDEHOLD_WAYPOINT, uint8_t sue_RACING_MODE) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN]; - _mav_put_uint8_t(buf, 0, sue_ROLL_STABILIZATION_AILERONS); - _mav_put_uint8_t(buf, 1, sue_ROLL_STABILIZATION_RUDDER); - _mav_put_uint8_t(buf, 2, sue_PITCH_STABILIZATION); - _mav_put_uint8_t(buf, 3, sue_YAW_STABILIZATION_RUDDER); - _mav_put_uint8_t(buf, 4, sue_YAW_STABILIZATION_AILERON); - _mav_put_uint8_t(buf, 5, sue_AILERON_NAVIGATION); - _mav_put_uint8_t(buf, 6, sue_RUDDER_NAVIGATION); - _mav_put_uint8_t(buf, 7, sue_ALTITUDEHOLD_STABILIZED); - _mav_put_uint8_t(buf, 8, sue_ALTITUDEHOLD_WAYPOINT); - _mav_put_uint8_t(buf, 9, sue_RACING_MODE); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); -#endif -#else - mavlink_serial_udb_extra_f4_t packet; - packet.sue_ROLL_STABILIZATION_AILERONS = sue_ROLL_STABILIZATION_AILERONS; - packet.sue_ROLL_STABILIZATION_RUDDER = sue_ROLL_STABILIZATION_RUDDER; - packet.sue_PITCH_STABILIZATION = sue_PITCH_STABILIZATION; - packet.sue_YAW_STABILIZATION_RUDDER = sue_YAW_STABILIZATION_RUDDER; - packet.sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON; - packet.sue_AILERON_NAVIGATION = sue_AILERON_NAVIGATION; - packet.sue_RUDDER_NAVIGATION = sue_RUDDER_NAVIGATION; - packet.sue_ALTITUDEHOLD_STABILIZED = sue_ALTITUDEHOLD_STABILIZED; - packet.sue_ALTITUDEHOLD_WAYPOINT = sue_ALTITUDEHOLD_WAYPOINT; - packet.sue_RACING_MODE = sue_RACING_MODE; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F4 UNPACKING - - -/** - * @brief Get field sue_ROLL_STABILIZATION_AILERONS from serial_udb_extra_f4 message - * - * @return Serial UDB Extra Roll Stabilization with Ailerons Enabled - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_ROLL_STABILIZATION_AILERONS(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field sue_ROLL_STABILIZATION_RUDDER from serial_udb_extra_f4 message - * - * @return Serial UDB Extra Roll Stabilization with Rudder Enabled - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_ROLL_STABILIZATION_RUDDER(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field sue_PITCH_STABILIZATION from serial_udb_extra_f4 message - * - * @return Serial UDB Extra Pitch Stabilization Enabled - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_PITCH_STABILIZATION(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field sue_YAW_STABILIZATION_RUDDER from serial_udb_extra_f4 message - * - * @return Serial UDB Extra Yaw Stabilization using Rudder Enabled - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_YAW_STABILIZATION_RUDDER(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field sue_YAW_STABILIZATION_AILERON from serial_udb_extra_f4 message - * - * @return Serial UDB Extra Yaw Stabilization using Ailerons Enabled - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_YAW_STABILIZATION_AILERON(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field sue_AILERON_NAVIGATION from serial_udb_extra_f4 message - * - * @return Serial UDB Extra Navigation with Ailerons Enabled - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_AILERON_NAVIGATION(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field sue_RUDDER_NAVIGATION from serial_udb_extra_f4 message - * - * @return Serial UDB Extra Navigation with Rudder Enabled - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_RUDDER_NAVIGATION(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @brief Get field sue_ALTITUDEHOLD_STABILIZED from serial_udb_extra_f4 message - * - * @return Serial UDB Extra Type of Alitude Hold when in Stabilized Mode - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_ALTITUDEHOLD_STABILIZED(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 7); -} - -/** - * @brief Get field sue_ALTITUDEHOLD_WAYPOINT from serial_udb_extra_f4 message - * - * @return Serial UDB Extra Type of Alitude Hold when in Waypoint Mode - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_ALTITUDEHOLD_WAYPOINT(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 8); -} - -/** - * @brief Get field sue_RACING_MODE from serial_udb_extra_f4 message - * - * @return Serial UDB Extra Firmware racing mode enabled - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_RACING_MODE(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 9); -} - -/** - * @brief Decode a serial_udb_extra_f4 message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f4 C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f4_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f4_t* serial_udb_extra_f4) -{ -#if MAVLINK_NEED_BYTE_SWAP - serial_udb_extra_f4->sue_ROLL_STABILIZATION_AILERONS = mavlink_msg_serial_udb_extra_f4_get_sue_ROLL_STABILIZATION_AILERONS(msg); - serial_udb_extra_f4->sue_ROLL_STABILIZATION_RUDDER = mavlink_msg_serial_udb_extra_f4_get_sue_ROLL_STABILIZATION_RUDDER(msg); - serial_udb_extra_f4->sue_PITCH_STABILIZATION = mavlink_msg_serial_udb_extra_f4_get_sue_PITCH_STABILIZATION(msg); - serial_udb_extra_f4->sue_YAW_STABILIZATION_RUDDER = mavlink_msg_serial_udb_extra_f4_get_sue_YAW_STABILIZATION_RUDDER(msg); - serial_udb_extra_f4->sue_YAW_STABILIZATION_AILERON = mavlink_msg_serial_udb_extra_f4_get_sue_YAW_STABILIZATION_AILERON(msg); - serial_udb_extra_f4->sue_AILERON_NAVIGATION = mavlink_msg_serial_udb_extra_f4_get_sue_AILERON_NAVIGATION(msg); - serial_udb_extra_f4->sue_RUDDER_NAVIGATION = mavlink_msg_serial_udb_extra_f4_get_sue_RUDDER_NAVIGATION(msg); - serial_udb_extra_f4->sue_ALTITUDEHOLD_STABILIZED = mavlink_msg_serial_udb_extra_f4_get_sue_ALTITUDEHOLD_STABILIZED(msg); - serial_udb_extra_f4->sue_ALTITUDEHOLD_WAYPOINT = mavlink_msg_serial_udb_extra_f4_get_sue_ALTITUDEHOLD_WAYPOINT(msg); - serial_udb_extra_f4->sue_RACING_MODE = mavlink_msg_serial_udb_extra_f4_get_sue_RACING_MODE(msg); -#else - memcpy(serial_udb_extra_f4, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h deleted file mode 100644 index e115a9b..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h +++ /dev/null @@ -1,287 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F5 PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5 173 - -typedef struct __mavlink_serial_udb_extra_f5_t -{ - float sue_YAWKP_AILERON; ///< Serial UDB YAWKP_AILERON Gain for Proporional control of navigation - float sue_YAWKD_AILERON; ///< Serial UDB YAWKD_AILERON Gain for Rate control of navigation - float sue_ROLLKP; ///< Serial UDB Extra ROLLKP Gain for Proportional control of roll stabilization - float sue_ROLLKD; ///< Serial UDB Extra ROLLKD Gain for Rate control of roll stabilization - float sue_YAW_STABILIZATION_AILERON; ///< YAW_STABILIZATION_AILERON Proportional control - float sue_AILERON_BOOST; ///< Gain For Boosting Manual Aileron control When Plane Stabilized -} mavlink_serial_udb_extra_f5_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN 24 -#define MAVLINK_MSG_ID_173_LEN 24 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_CRC 121 -#define MAVLINK_MSG_ID_173_CRC 121 - - - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F5 { \ - "SERIAL_UDB_EXTRA_F5", \ - 6, \ - { { "sue_YAWKP_AILERON", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_serial_udb_extra_f5_t, sue_YAWKP_AILERON) }, \ - { "sue_YAWKD_AILERON", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_serial_udb_extra_f5_t, sue_YAWKD_AILERON) }, \ - { "sue_ROLLKP", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_serial_udb_extra_f5_t, sue_ROLLKP) }, \ - { "sue_ROLLKD", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_serial_udb_extra_f5_t, sue_ROLLKD) }, \ - { "sue_YAW_STABILIZATION_AILERON", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_serial_udb_extra_f5_t, sue_YAW_STABILIZATION_AILERON) }, \ - { "sue_AILERON_BOOST", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_serial_udb_extra_f5_t, sue_AILERON_BOOST) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f5 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_YAWKP_AILERON Serial UDB YAWKP_AILERON Gain for Proporional control of navigation - * @param sue_YAWKD_AILERON Serial UDB YAWKD_AILERON Gain for Rate control of navigation - * @param sue_ROLLKP Serial UDB Extra ROLLKP Gain for Proportional control of roll stabilization - * @param sue_ROLLKD Serial UDB Extra ROLLKD Gain for Rate control of roll stabilization - * @param sue_YAW_STABILIZATION_AILERON YAW_STABILIZATION_AILERON Proportional control - * @param sue_AILERON_BOOST Gain For Boosting Manual Aileron control When Plane Stabilized - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f5_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float sue_YAWKP_AILERON, float sue_YAWKD_AILERON, float sue_ROLLKP, float sue_ROLLKD, float sue_YAW_STABILIZATION_AILERON, float sue_AILERON_BOOST) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN]; - _mav_put_float(buf, 0, sue_YAWKP_AILERON); - _mav_put_float(buf, 4, sue_YAWKD_AILERON); - _mav_put_float(buf, 8, sue_ROLLKP); - _mav_put_float(buf, 12, sue_ROLLKD); - _mav_put_float(buf, 16, sue_YAW_STABILIZATION_AILERON); - _mav_put_float(buf, 20, sue_AILERON_BOOST); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); -#else - mavlink_serial_udb_extra_f5_t packet; - packet.sue_YAWKP_AILERON = sue_YAWKP_AILERON; - packet.sue_YAWKD_AILERON = sue_YAWKD_AILERON; - packet.sue_ROLLKP = sue_ROLLKP; - packet.sue_ROLLKD = sue_ROLLKD; - packet.sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON; - packet.sue_AILERON_BOOST = sue_AILERON_BOOST; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f5 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_YAWKP_AILERON Serial UDB YAWKP_AILERON Gain for Proporional control of navigation - * @param sue_YAWKD_AILERON Serial UDB YAWKD_AILERON Gain for Rate control of navigation - * @param sue_ROLLKP Serial UDB Extra ROLLKP Gain for Proportional control of roll stabilization - * @param sue_ROLLKD Serial UDB Extra ROLLKD Gain for Rate control of roll stabilization - * @param sue_YAW_STABILIZATION_AILERON YAW_STABILIZATION_AILERON Proportional control - * @param sue_AILERON_BOOST Gain For Boosting Manual Aileron control When Plane Stabilized - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f5_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float sue_YAWKP_AILERON,float sue_YAWKD_AILERON,float sue_ROLLKP,float sue_ROLLKD,float sue_YAW_STABILIZATION_AILERON,float sue_AILERON_BOOST) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN]; - _mav_put_float(buf, 0, sue_YAWKP_AILERON); - _mav_put_float(buf, 4, sue_YAWKD_AILERON); - _mav_put_float(buf, 8, sue_ROLLKP); - _mav_put_float(buf, 12, sue_ROLLKD); - _mav_put_float(buf, 16, sue_YAW_STABILIZATION_AILERON); - _mav_put_float(buf, 20, sue_AILERON_BOOST); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); -#else - mavlink_serial_udb_extra_f5_t packet; - packet.sue_YAWKP_AILERON = sue_YAWKP_AILERON; - packet.sue_YAWKD_AILERON = sue_YAWKD_AILERON; - packet.sue_ROLLKP = sue_ROLLKP; - packet.sue_ROLLKD = sue_ROLLKD; - packet.sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON; - packet.sue_AILERON_BOOST = sue_AILERON_BOOST; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f5 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f5 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f5_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f5_t* serial_udb_extra_f5) -{ - return mavlink_msg_serial_udb_extra_f5_pack(system_id, component_id, msg, serial_udb_extra_f5->sue_YAWKP_AILERON, serial_udb_extra_f5->sue_YAWKD_AILERON, serial_udb_extra_f5->sue_ROLLKP, serial_udb_extra_f5->sue_ROLLKD, serial_udb_extra_f5->sue_YAW_STABILIZATION_AILERON, serial_udb_extra_f5->sue_AILERON_BOOST); -} - -/** - * @brief Encode a serial_udb_extra_f5 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f5 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f5_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f5_t* serial_udb_extra_f5) -{ - return mavlink_msg_serial_udb_extra_f5_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f5->sue_YAWKP_AILERON, serial_udb_extra_f5->sue_YAWKD_AILERON, serial_udb_extra_f5->sue_ROLLKP, serial_udb_extra_f5->sue_ROLLKD, serial_udb_extra_f5->sue_YAW_STABILIZATION_AILERON, serial_udb_extra_f5->sue_AILERON_BOOST); -} - -/** - * @brief Send a serial_udb_extra_f5 message - * @param chan MAVLink channel to send the message - * - * @param sue_YAWKP_AILERON Serial UDB YAWKP_AILERON Gain for Proporional control of navigation - * @param sue_YAWKD_AILERON Serial UDB YAWKD_AILERON Gain for Rate control of navigation - * @param sue_ROLLKP Serial UDB Extra ROLLKP Gain for Proportional control of roll stabilization - * @param sue_ROLLKD Serial UDB Extra ROLLKD Gain for Rate control of roll stabilization - * @param sue_YAW_STABILIZATION_AILERON YAW_STABILIZATION_AILERON Proportional control - * @param sue_AILERON_BOOST Gain For Boosting Manual Aileron control When Plane Stabilized - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f5_send(mavlink_channel_t chan, float sue_YAWKP_AILERON, float sue_YAWKD_AILERON, float sue_ROLLKP, float sue_ROLLKD, float sue_YAW_STABILIZATION_AILERON, float sue_AILERON_BOOST) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN]; - _mav_put_float(buf, 0, sue_YAWKP_AILERON); - _mav_put_float(buf, 4, sue_YAWKD_AILERON); - _mav_put_float(buf, 8, sue_ROLLKP); - _mav_put_float(buf, 12, sue_ROLLKD); - _mav_put_float(buf, 16, sue_YAW_STABILIZATION_AILERON); - _mav_put_float(buf, 20, sue_AILERON_BOOST); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); -#endif -#else - mavlink_serial_udb_extra_f5_t packet; - packet.sue_YAWKP_AILERON = sue_YAWKP_AILERON; - packet.sue_YAWKD_AILERON = sue_YAWKD_AILERON; - packet.sue_ROLLKP = sue_ROLLKP; - packet.sue_ROLLKD = sue_ROLLKD; - packet.sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON; - packet.sue_AILERON_BOOST = sue_AILERON_BOOST; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F5 UNPACKING - - -/** - * @brief Get field sue_YAWKP_AILERON from serial_udb_extra_f5 message - * - * @return Serial UDB YAWKP_AILERON Gain for Proporional control of navigation - */ -static inline float mavlink_msg_serial_udb_extra_f5_get_sue_YAWKP_AILERON(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field sue_YAWKD_AILERON from serial_udb_extra_f5 message - * - * @return Serial UDB YAWKD_AILERON Gain for Rate control of navigation - */ -static inline float mavlink_msg_serial_udb_extra_f5_get_sue_YAWKD_AILERON(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field sue_ROLLKP from serial_udb_extra_f5 message - * - * @return Serial UDB Extra ROLLKP Gain for Proportional control of roll stabilization - */ -static inline float mavlink_msg_serial_udb_extra_f5_get_sue_ROLLKP(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field sue_ROLLKD from serial_udb_extra_f5 message - * - * @return Serial UDB Extra ROLLKD Gain for Rate control of roll stabilization - */ -static inline float mavlink_msg_serial_udb_extra_f5_get_sue_ROLLKD(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field sue_YAW_STABILIZATION_AILERON from serial_udb_extra_f5 message - * - * @return YAW_STABILIZATION_AILERON Proportional control - */ -static inline float mavlink_msg_serial_udb_extra_f5_get_sue_YAW_STABILIZATION_AILERON(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field sue_AILERON_BOOST from serial_udb_extra_f5 message - * - * @return Gain For Boosting Manual Aileron control When Plane Stabilized - */ -static inline float mavlink_msg_serial_udb_extra_f5_get_sue_AILERON_BOOST(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Decode a serial_udb_extra_f5 message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f5 C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f5_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f5_t* serial_udb_extra_f5) -{ -#if MAVLINK_NEED_BYTE_SWAP - serial_udb_extra_f5->sue_YAWKP_AILERON = mavlink_msg_serial_udb_extra_f5_get_sue_YAWKP_AILERON(msg); - serial_udb_extra_f5->sue_YAWKD_AILERON = mavlink_msg_serial_udb_extra_f5_get_sue_YAWKD_AILERON(msg); - serial_udb_extra_f5->sue_ROLLKP = mavlink_msg_serial_udb_extra_f5_get_sue_ROLLKP(msg); - serial_udb_extra_f5->sue_ROLLKD = mavlink_msg_serial_udb_extra_f5_get_sue_ROLLKD(msg); - serial_udb_extra_f5->sue_YAW_STABILIZATION_AILERON = mavlink_msg_serial_udb_extra_f5_get_sue_YAW_STABILIZATION_AILERON(msg); - serial_udb_extra_f5->sue_AILERON_BOOST = mavlink_msg_serial_udb_extra_f5_get_sue_AILERON_BOOST(msg); -#else - memcpy(serial_udb_extra_f5, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h deleted file mode 100644 index 656103d..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h +++ /dev/null @@ -1,265 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F6 PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6 174 - -typedef struct __mavlink_serial_udb_extra_f6_t -{ - float sue_PITCHGAIN; ///< Serial UDB Extra PITCHGAIN Proportional Control - float sue_PITCHKD; ///< Serial UDB Extra Pitch Rate Control - float sue_RUDDER_ELEV_MIX; ///< Serial UDB Extra Rudder to Elevator Mix - float sue_ROLL_ELEV_MIX; ///< Serial UDB Extra Roll to Elevator Mix - float sue_ELEVATOR_BOOST; ///< Gain For Boosting Manual Elevator control When Plane Stabilized -} mavlink_serial_udb_extra_f6_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN 20 -#define MAVLINK_MSG_ID_174_LEN 20 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_CRC 54 -#define MAVLINK_MSG_ID_174_CRC 54 - - - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F6 { \ - "SERIAL_UDB_EXTRA_F6", \ - 5, \ - { { "sue_PITCHGAIN", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_serial_udb_extra_f6_t, sue_PITCHGAIN) }, \ - { "sue_PITCHKD", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_serial_udb_extra_f6_t, sue_PITCHKD) }, \ - { "sue_RUDDER_ELEV_MIX", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_serial_udb_extra_f6_t, sue_RUDDER_ELEV_MIX) }, \ - { "sue_ROLL_ELEV_MIX", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_serial_udb_extra_f6_t, sue_ROLL_ELEV_MIX) }, \ - { "sue_ELEVATOR_BOOST", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_serial_udb_extra_f6_t, sue_ELEVATOR_BOOST) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f6 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_PITCHGAIN Serial UDB Extra PITCHGAIN Proportional Control - * @param sue_PITCHKD Serial UDB Extra Pitch Rate Control - * @param sue_RUDDER_ELEV_MIX Serial UDB Extra Rudder to Elevator Mix - * @param sue_ROLL_ELEV_MIX Serial UDB Extra Roll to Elevator Mix - * @param sue_ELEVATOR_BOOST Gain For Boosting Manual Elevator control When Plane Stabilized - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f6_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float sue_PITCHGAIN, float sue_PITCHKD, float sue_RUDDER_ELEV_MIX, float sue_ROLL_ELEV_MIX, float sue_ELEVATOR_BOOST) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN]; - _mav_put_float(buf, 0, sue_PITCHGAIN); - _mav_put_float(buf, 4, sue_PITCHKD); - _mav_put_float(buf, 8, sue_RUDDER_ELEV_MIX); - _mav_put_float(buf, 12, sue_ROLL_ELEV_MIX); - _mav_put_float(buf, 16, sue_ELEVATOR_BOOST); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); -#else - mavlink_serial_udb_extra_f6_t packet; - packet.sue_PITCHGAIN = sue_PITCHGAIN; - packet.sue_PITCHKD = sue_PITCHKD; - packet.sue_RUDDER_ELEV_MIX = sue_RUDDER_ELEV_MIX; - packet.sue_ROLL_ELEV_MIX = sue_ROLL_ELEV_MIX; - packet.sue_ELEVATOR_BOOST = sue_ELEVATOR_BOOST; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f6 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_PITCHGAIN Serial UDB Extra PITCHGAIN Proportional Control - * @param sue_PITCHKD Serial UDB Extra Pitch Rate Control - * @param sue_RUDDER_ELEV_MIX Serial UDB Extra Rudder to Elevator Mix - * @param sue_ROLL_ELEV_MIX Serial UDB Extra Roll to Elevator Mix - * @param sue_ELEVATOR_BOOST Gain For Boosting Manual Elevator control When Plane Stabilized - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f6_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float sue_PITCHGAIN,float sue_PITCHKD,float sue_RUDDER_ELEV_MIX,float sue_ROLL_ELEV_MIX,float sue_ELEVATOR_BOOST) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN]; - _mav_put_float(buf, 0, sue_PITCHGAIN); - _mav_put_float(buf, 4, sue_PITCHKD); - _mav_put_float(buf, 8, sue_RUDDER_ELEV_MIX); - _mav_put_float(buf, 12, sue_ROLL_ELEV_MIX); - _mav_put_float(buf, 16, sue_ELEVATOR_BOOST); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); -#else - mavlink_serial_udb_extra_f6_t packet; - packet.sue_PITCHGAIN = sue_PITCHGAIN; - packet.sue_PITCHKD = sue_PITCHKD; - packet.sue_RUDDER_ELEV_MIX = sue_RUDDER_ELEV_MIX; - packet.sue_ROLL_ELEV_MIX = sue_ROLL_ELEV_MIX; - packet.sue_ELEVATOR_BOOST = sue_ELEVATOR_BOOST; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f6 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f6 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f6_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f6_t* serial_udb_extra_f6) -{ - return mavlink_msg_serial_udb_extra_f6_pack(system_id, component_id, msg, serial_udb_extra_f6->sue_PITCHGAIN, serial_udb_extra_f6->sue_PITCHKD, serial_udb_extra_f6->sue_RUDDER_ELEV_MIX, serial_udb_extra_f6->sue_ROLL_ELEV_MIX, serial_udb_extra_f6->sue_ELEVATOR_BOOST); -} - -/** - * @brief Encode a serial_udb_extra_f6 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f6 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f6_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f6_t* serial_udb_extra_f6) -{ - return mavlink_msg_serial_udb_extra_f6_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f6->sue_PITCHGAIN, serial_udb_extra_f6->sue_PITCHKD, serial_udb_extra_f6->sue_RUDDER_ELEV_MIX, serial_udb_extra_f6->sue_ROLL_ELEV_MIX, serial_udb_extra_f6->sue_ELEVATOR_BOOST); -} - -/** - * @brief Send a serial_udb_extra_f6 message - * @param chan MAVLink channel to send the message - * - * @param sue_PITCHGAIN Serial UDB Extra PITCHGAIN Proportional Control - * @param sue_PITCHKD Serial UDB Extra Pitch Rate Control - * @param sue_RUDDER_ELEV_MIX Serial UDB Extra Rudder to Elevator Mix - * @param sue_ROLL_ELEV_MIX Serial UDB Extra Roll to Elevator Mix - * @param sue_ELEVATOR_BOOST Gain For Boosting Manual Elevator control When Plane Stabilized - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f6_send(mavlink_channel_t chan, float sue_PITCHGAIN, float sue_PITCHKD, float sue_RUDDER_ELEV_MIX, float sue_ROLL_ELEV_MIX, float sue_ELEVATOR_BOOST) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN]; - _mav_put_float(buf, 0, sue_PITCHGAIN); - _mav_put_float(buf, 4, sue_PITCHKD); - _mav_put_float(buf, 8, sue_RUDDER_ELEV_MIX); - _mav_put_float(buf, 12, sue_ROLL_ELEV_MIX); - _mav_put_float(buf, 16, sue_ELEVATOR_BOOST); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); -#endif -#else - mavlink_serial_udb_extra_f6_t packet; - packet.sue_PITCHGAIN = sue_PITCHGAIN; - packet.sue_PITCHKD = sue_PITCHKD; - packet.sue_RUDDER_ELEV_MIX = sue_RUDDER_ELEV_MIX; - packet.sue_ROLL_ELEV_MIX = sue_ROLL_ELEV_MIX; - packet.sue_ELEVATOR_BOOST = sue_ELEVATOR_BOOST; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F6 UNPACKING - - -/** - * @brief Get field sue_PITCHGAIN from serial_udb_extra_f6 message - * - * @return Serial UDB Extra PITCHGAIN Proportional Control - */ -static inline float mavlink_msg_serial_udb_extra_f6_get_sue_PITCHGAIN(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field sue_PITCHKD from serial_udb_extra_f6 message - * - * @return Serial UDB Extra Pitch Rate Control - */ -static inline float mavlink_msg_serial_udb_extra_f6_get_sue_PITCHKD(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field sue_RUDDER_ELEV_MIX from serial_udb_extra_f6 message - * - * @return Serial UDB Extra Rudder to Elevator Mix - */ -static inline float mavlink_msg_serial_udb_extra_f6_get_sue_RUDDER_ELEV_MIX(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field sue_ROLL_ELEV_MIX from serial_udb_extra_f6 message - * - * @return Serial UDB Extra Roll to Elevator Mix - */ -static inline float mavlink_msg_serial_udb_extra_f6_get_sue_ROLL_ELEV_MIX(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field sue_ELEVATOR_BOOST from serial_udb_extra_f6 message - * - * @return Gain For Boosting Manual Elevator control When Plane Stabilized - */ -static inline float mavlink_msg_serial_udb_extra_f6_get_sue_ELEVATOR_BOOST(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Decode a serial_udb_extra_f6 message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f6 C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f6_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f6_t* serial_udb_extra_f6) -{ -#if MAVLINK_NEED_BYTE_SWAP - serial_udb_extra_f6->sue_PITCHGAIN = mavlink_msg_serial_udb_extra_f6_get_sue_PITCHGAIN(msg); - serial_udb_extra_f6->sue_PITCHKD = mavlink_msg_serial_udb_extra_f6_get_sue_PITCHKD(msg); - serial_udb_extra_f6->sue_RUDDER_ELEV_MIX = mavlink_msg_serial_udb_extra_f6_get_sue_RUDDER_ELEV_MIX(msg); - serial_udb_extra_f6->sue_ROLL_ELEV_MIX = mavlink_msg_serial_udb_extra_f6_get_sue_ROLL_ELEV_MIX(msg); - serial_udb_extra_f6->sue_ELEVATOR_BOOST = mavlink_msg_serial_udb_extra_f6_get_sue_ELEVATOR_BOOST(msg); -#else - memcpy(serial_udb_extra_f6, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h deleted file mode 100644 index 51c98e6..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h +++ /dev/null @@ -1,287 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F7 PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7 175 - -typedef struct __mavlink_serial_udb_extra_f7_t -{ - float sue_YAWKP_RUDDER; ///< Serial UDB YAWKP_RUDDER Gain for Proporional control of navigation - float sue_YAWKD_RUDDER; ///< Serial UDB YAWKD_RUDDER Gain for Rate control of navigation - float sue_ROLLKP_RUDDER; ///< Serial UDB Extra ROLLKP_RUDDER Gain for Proportional control of roll stabilization - float sue_ROLLKD_RUDDER; ///< Serial UDB Extra ROLLKD_RUDDER Gain for Rate control of roll stabilization - float sue_RUDDER_BOOST; ///< SERIAL UDB EXTRA Rudder Boost Gain to Manual Control when stabilized - float sue_RTL_PITCH_DOWN; ///< Serial UDB Extra Return To Landing - Angle to Pitch Plane Down -} mavlink_serial_udb_extra_f7_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN 24 -#define MAVLINK_MSG_ID_175_LEN 24 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_CRC 171 -#define MAVLINK_MSG_ID_175_CRC 171 - - - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F7 { \ - "SERIAL_UDB_EXTRA_F7", \ - 6, \ - { { "sue_YAWKP_RUDDER", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_serial_udb_extra_f7_t, sue_YAWKP_RUDDER) }, \ - { "sue_YAWKD_RUDDER", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_serial_udb_extra_f7_t, sue_YAWKD_RUDDER) }, \ - { "sue_ROLLKP_RUDDER", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_serial_udb_extra_f7_t, sue_ROLLKP_RUDDER) }, \ - { "sue_ROLLKD_RUDDER", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_serial_udb_extra_f7_t, sue_ROLLKD_RUDDER) }, \ - { "sue_RUDDER_BOOST", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_serial_udb_extra_f7_t, sue_RUDDER_BOOST) }, \ - { "sue_RTL_PITCH_DOWN", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_serial_udb_extra_f7_t, sue_RTL_PITCH_DOWN) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f7 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_YAWKP_RUDDER Serial UDB YAWKP_RUDDER Gain for Proporional control of navigation - * @param sue_YAWKD_RUDDER Serial UDB YAWKD_RUDDER Gain for Rate control of navigation - * @param sue_ROLLKP_RUDDER Serial UDB Extra ROLLKP_RUDDER Gain for Proportional control of roll stabilization - * @param sue_ROLLKD_RUDDER Serial UDB Extra ROLLKD_RUDDER Gain for Rate control of roll stabilization - * @param sue_RUDDER_BOOST SERIAL UDB EXTRA Rudder Boost Gain to Manual Control when stabilized - * @param sue_RTL_PITCH_DOWN Serial UDB Extra Return To Landing - Angle to Pitch Plane Down - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f7_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float sue_YAWKP_RUDDER, float sue_YAWKD_RUDDER, float sue_ROLLKP_RUDDER, float sue_ROLLKD_RUDDER, float sue_RUDDER_BOOST, float sue_RTL_PITCH_DOWN) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN]; - _mav_put_float(buf, 0, sue_YAWKP_RUDDER); - _mav_put_float(buf, 4, sue_YAWKD_RUDDER); - _mav_put_float(buf, 8, sue_ROLLKP_RUDDER); - _mav_put_float(buf, 12, sue_ROLLKD_RUDDER); - _mav_put_float(buf, 16, sue_RUDDER_BOOST); - _mav_put_float(buf, 20, sue_RTL_PITCH_DOWN); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); -#else - mavlink_serial_udb_extra_f7_t packet; - packet.sue_YAWKP_RUDDER = sue_YAWKP_RUDDER; - packet.sue_YAWKD_RUDDER = sue_YAWKD_RUDDER; - packet.sue_ROLLKP_RUDDER = sue_ROLLKP_RUDDER; - packet.sue_ROLLKD_RUDDER = sue_ROLLKD_RUDDER; - packet.sue_RUDDER_BOOST = sue_RUDDER_BOOST; - packet.sue_RTL_PITCH_DOWN = sue_RTL_PITCH_DOWN; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f7 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_YAWKP_RUDDER Serial UDB YAWKP_RUDDER Gain for Proporional control of navigation - * @param sue_YAWKD_RUDDER Serial UDB YAWKD_RUDDER Gain for Rate control of navigation - * @param sue_ROLLKP_RUDDER Serial UDB Extra ROLLKP_RUDDER Gain for Proportional control of roll stabilization - * @param sue_ROLLKD_RUDDER Serial UDB Extra ROLLKD_RUDDER Gain for Rate control of roll stabilization - * @param sue_RUDDER_BOOST SERIAL UDB EXTRA Rudder Boost Gain to Manual Control when stabilized - * @param sue_RTL_PITCH_DOWN Serial UDB Extra Return To Landing - Angle to Pitch Plane Down - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f7_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float sue_YAWKP_RUDDER,float sue_YAWKD_RUDDER,float sue_ROLLKP_RUDDER,float sue_ROLLKD_RUDDER,float sue_RUDDER_BOOST,float sue_RTL_PITCH_DOWN) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN]; - _mav_put_float(buf, 0, sue_YAWKP_RUDDER); - _mav_put_float(buf, 4, sue_YAWKD_RUDDER); - _mav_put_float(buf, 8, sue_ROLLKP_RUDDER); - _mav_put_float(buf, 12, sue_ROLLKD_RUDDER); - _mav_put_float(buf, 16, sue_RUDDER_BOOST); - _mav_put_float(buf, 20, sue_RTL_PITCH_DOWN); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); -#else - mavlink_serial_udb_extra_f7_t packet; - packet.sue_YAWKP_RUDDER = sue_YAWKP_RUDDER; - packet.sue_YAWKD_RUDDER = sue_YAWKD_RUDDER; - packet.sue_ROLLKP_RUDDER = sue_ROLLKP_RUDDER; - packet.sue_ROLLKD_RUDDER = sue_ROLLKD_RUDDER; - packet.sue_RUDDER_BOOST = sue_RUDDER_BOOST; - packet.sue_RTL_PITCH_DOWN = sue_RTL_PITCH_DOWN; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f7 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f7 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f7_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f7_t* serial_udb_extra_f7) -{ - return mavlink_msg_serial_udb_extra_f7_pack(system_id, component_id, msg, serial_udb_extra_f7->sue_YAWKP_RUDDER, serial_udb_extra_f7->sue_YAWKD_RUDDER, serial_udb_extra_f7->sue_ROLLKP_RUDDER, serial_udb_extra_f7->sue_ROLLKD_RUDDER, serial_udb_extra_f7->sue_RUDDER_BOOST, serial_udb_extra_f7->sue_RTL_PITCH_DOWN); -} - -/** - * @brief Encode a serial_udb_extra_f7 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f7 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f7_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f7_t* serial_udb_extra_f7) -{ - return mavlink_msg_serial_udb_extra_f7_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f7->sue_YAWKP_RUDDER, serial_udb_extra_f7->sue_YAWKD_RUDDER, serial_udb_extra_f7->sue_ROLLKP_RUDDER, serial_udb_extra_f7->sue_ROLLKD_RUDDER, serial_udb_extra_f7->sue_RUDDER_BOOST, serial_udb_extra_f7->sue_RTL_PITCH_DOWN); -} - -/** - * @brief Send a serial_udb_extra_f7 message - * @param chan MAVLink channel to send the message - * - * @param sue_YAWKP_RUDDER Serial UDB YAWKP_RUDDER Gain for Proporional control of navigation - * @param sue_YAWKD_RUDDER Serial UDB YAWKD_RUDDER Gain for Rate control of navigation - * @param sue_ROLLKP_RUDDER Serial UDB Extra ROLLKP_RUDDER Gain for Proportional control of roll stabilization - * @param sue_ROLLKD_RUDDER Serial UDB Extra ROLLKD_RUDDER Gain for Rate control of roll stabilization - * @param sue_RUDDER_BOOST SERIAL UDB EXTRA Rudder Boost Gain to Manual Control when stabilized - * @param sue_RTL_PITCH_DOWN Serial UDB Extra Return To Landing - Angle to Pitch Plane Down - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f7_send(mavlink_channel_t chan, float sue_YAWKP_RUDDER, float sue_YAWKD_RUDDER, float sue_ROLLKP_RUDDER, float sue_ROLLKD_RUDDER, float sue_RUDDER_BOOST, float sue_RTL_PITCH_DOWN) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN]; - _mav_put_float(buf, 0, sue_YAWKP_RUDDER); - _mav_put_float(buf, 4, sue_YAWKD_RUDDER); - _mav_put_float(buf, 8, sue_ROLLKP_RUDDER); - _mav_put_float(buf, 12, sue_ROLLKD_RUDDER); - _mav_put_float(buf, 16, sue_RUDDER_BOOST); - _mav_put_float(buf, 20, sue_RTL_PITCH_DOWN); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); -#endif -#else - mavlink_serial_udb_extra_f7_t packet; - packet.sue_YAWKP_RUDDER = sue_YAWKP_RUDDER; - packet.sue_YAWKD_RUDDER = sue_YAWKD_RUDDER; - packet.sue_ROLLKP_RUDDER = sue_ROLLKP_RUDDER; - packet.sue_ROLLKD_RUDDER = sue_ROLLKD_RUDDER; - packet.sue_RUDDER_BOOST = sue_RUDDER_BOOST; - packet.sue_RTL_PITCH_DOWN = sue_RTL_PITCH_DOWN; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F7 UNPACKING - - -/** - * @brief Get field sue_YAWKP_RUDDER from serial_udb_extra_f7 message - * - * @return Serial UDB YAWKP_RUDDER Gain for Proporional control of navigation - */ -static inline float mavlink_msg_serial_udb_extra_f7_get_sue_YAWKP_RUDDER(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field sue_YAWKD_RUDDER from serial_udb_extra_f7 message - * - * @return Serial UDB YAWKD_RUDDER Gain for Rate control of navigation - */ -static inline float mavlink_msg_serial_udb_extra_f7_get_sue_YAWKD_RUDDER(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field sue_ROLLKP_RUDDER from serial_udb_extra_f7 message - * - * @return Serial UDB Extra ROLLKP_RUDDER Gain for Proportional control of roll stabilization - */ -static inline float mavlink_msg_serial_udb_extra_f7_get_sue_ROLLKP_RUDDER(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field sue_ROLLKD_RUDDER from serial_udb_extra_f7 message - * - * @return Serial UDB Extra ROLLKD_RUDDER Gain for Rate control of roll stabilization - */ -static inline float mavlink_msg_serial_udb_extra_f7_get_sue_ROLLKD_RUDDER(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field sue_RUDDER_BOOST from serial_udb_extra_f7 message - * - * @return SERIAL UDB EXTRA Rudder Boost Gain to Manual Control when stabilized - */ -static inline float mavlink_msg_serial_udb_extra_f7_get_sue_RUDDER_BOOST(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field sue_RTL_PITCH_DOWN from serial_udb_extra_f7 message - * - * @return Serial UDB Extra Return To Landing - Angle to Pitch Plane Down - */ -static inline float mavlink_msg_serial_udb_extra_f7_get_sue_RTL_PITCH_DOWN(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Decode a serial_udb_extra_f7 message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f7 C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f7_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f7_t* serial_udb_extra_f7) -{ -#if MAVLINK_NEED_BYTE_SWAP - serial_udb_extra_f7->sue_YAWKP_RUDDER = mavlink_msg_serial_udb_extra_f7_get_sue_YAWKP_RUDDER(msg); - serial_udb_extra_f7->sue_YAWKD_RUDDER = mavlink_msg_serial_udb_extra_f7_get_sue_YAWKD_RUDDER(msg); - serial_udb_extra_f7->sue_ROLLKP_RUDDER = mavlink_msg_serial_udb_extra_f7_get_sue_ROLLKP_RUDDER(msg); - serial_udb_extra_f7->sue_ROLLKD_RUDDER = mavlink_msg_serial_udb_extra_f7_get_sue_ROLLKD_RUDDER(msg); - serial_udb_extra_f7->sue_RUDDER_BOOST = mavlink_msg_serial_udb_extra_f7_get_sue_RUDDER_BOOST(msg); - serial_udb_extra_f7->sue_RTL_PITCH_DOWN = mavlink_msg_serial_udb_extra_f7_get_sue_RTL_PITCH_DOWN(msg); -#else - memcpy(serial_udb_extra_f7, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h deleted file mode 100644 index 8557a95..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h +++ /dev/null @@ -1,309 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F8 PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8 176 - -typedef struct __mavlink_serial_udb_extra_f8_t -{ - float sue_HEIGHT_TARGET_MAX; ///< Serial UDB Extra HEIGHT_TARGET_MAX - float sue_HEIGHT_TARGET_MIN; ///< Serial UDB Extra HEIGHT_TARGET_MIN - float sue_ALT_HOLD_THROTTLE_MIN; ///< Serial UDB Extra ALT_HOLD_THROTTLE_MIN - float sue_ALT_HOLD_THROTTLE_MAX; ///< Serial UDB Extra ALT_HOLD_THROTTLE_MAX - float sue_ALT_HOLD_PITCH_MIN; ///< Serial UDB Extra ALT_HOLD_PITCH_MIN - float sue_ALT_HOLD_PITCH_MAX; ///< Serial UDB Extra ALT_HOLD_PITCH_MAX - float sue_ALT_HOLD_PITCH_HIGH; ///< Serial UDB Extra ALT_HOLD_PITCH_HIGH -} mavlink_serial_udb_extra_f8_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN 28 -#define MAVLINK_MSG_ID_176_LEN 28 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_CRC 142 -#define MAVLINK_MSG_ID_176_CRC 142 - - - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F8 { \ - "SERIAL_UDB_EXTRA_F8", \ - 7, \ - { { "sue_HEIGHT_TARGET_MAX", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_serial_udb_extra_f8_t, sue_HEIGHT_TARGET_MAX) }, \ - { "sue_HEIGHT_TARGET_MIN", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_serial_udb_extra_f8_t, sue_HEIGHT_TARGET_MIN) }, \ - { "sue_ALT_HOLD_THROTTLE_MIN", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_serial_udb_extra_f8_t, sue_ALT_HOLD_THROTTLE_MIN) }, \ - { "sue_ALT_HOLD_THROTTLE_MAX", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_serial_udb_extra_f8_t, sue_ALT_HOLD_THROTTLE_MAX) }, \ - { "sue_ALT_HOLD_PITCH_MIN", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_serial_udb_extra_f8_t, sue_ALT_HOLD_PITCH_MIN) }, \ - { "sue_ALT_HOLD_PITCH_MAX", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_serial_udb_extra_f8_t, sue_ALT_HOLD_PITCH_MAX) }, \ - { "sue_ALT_HOLD_PITCH_HIGH", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_serial_udb_extra_f8_t, sue_ALT_HOLD_PITCH_HIGH) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f8 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_HEIGHT_TARGET_MAX Serial UDB Extra HEIGHT_TARGET_MAX - * @param sue_HEIGHT_TARGET_MIN Serial UDB Extra HEIGHT_TARGET_MIN - * @param sue_ALT_HOLD_THROTTLE_MIN Serial UDB Extra ALT_HOLD_THROTTLE_MIN - * @param sue_ALT_HOLD_THROTTLE_MAX Serial UDB Extra ALT_HOLD_THROTTLE_MAX - * @param sue_ALT_HOLD_PITCH_MIN Serial UDB Extra ALT_HOLD_PITCH_MIN - * @param sue_ALT_HOLD_PITCH_MAX Serial UDB Extra ALT_HOLD_PITCH_MAX - * @param sue_ALT_HOLD_PITCH_HIGH Serial UDB Extra ALT_HOLD_PITCH_HIGH - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f8_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float sue_HEIGHT_TARGET_MAX, float sue_HEIGHT_TARGET_MIN, float sue_ALT_HOLD_THROTTLE_MIN, float sue_ALT_HOLD_THROTTLE_MAX, float sue_ALT_HOLD_PITCH_MIN, float sue_ALT_HOLD_PITCH_MAX, float sue_ALT_HOLD_PITCH_HIGH) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN]; - _mav_put_float(buf, 0, sue_HEIGHT_TARGET_MAX); - _mav_put_float(buf, 4, sue_HEIGHT_TARGET_MIN); - _mav_put_float(buf, 8, sue_ALT_HOLD_THROTTLE_MIN); - _mav_put_float(buf, 12, sue_ALT_HOLD_THROTTLE_MAX); - _mav_put_float(buf, 16, sue_ALT_HOLD_PITCH_MIN); - _mav_put_float(buf, 20, sue_ALT_HOLD_PITCH_MAX); - _mav_put_float(buf, 24, sue_ALT_HOLD_PITCH_HIGH); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); -#else - mavlink_serial_udb_extra_f8_t packet; - packet.sue_HEIGHT_TARGET_MAX = sue_HEIGHT_TARGET_MAX; - packet.sue_HEIGHT_TARGET_MIN = sue_HEIGHT_TARGET_MIN; - packet.sue_ALT_HOLD_THROTTLE_MIN = sue_ALT_HOLD_THROTTLE_MIN; - packet.sue_ALT_HOLD_THROTTLE_MAX = sue_ALT_HOLD_THROTTLE_MAX; - packet.sue_ALT_HOLD_PITCH_MIN = sue_ALT_HOLD_PITCH_MIN; - packet.sue_ALT_HOLD_PITCH_MAX = sue_ALT_HOLD_PITCH_MAX; - packet.sue_ALT_HOLD_PITCH_HIGH = sue_ALT_HOLD_PITCH_HIGH; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f8 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_HEIGHT_TARGET_MAX Serial UDB Extra HEIGHT_TARGET_MAX - * @param sue_HEIGHT_TARGET_MIN Serial UDB Extra HEIGHT_TARGET_MIN - * @param sue_ALT_HOLD_THROTTLE_MIN Serial UDB Extra ALT_HOLD_THROTTLE_MIN - * @param sue_ALT_HOLD_THROTTLE_MAX Serial UDB Extra ALT_HOLD_THROTTLE_MAX - * @param sue_ALT_HOLD_PITCH_MIN Serial UDB Extra ALT_HOLD_PITCH_MIN - * @param sue_ALT_HOLD_PITCH_MAX Serial UDB Extra ALT_HOLD_PITCH_MAX - * @param sue_ALT_HOLD_PITCH_HIGH Serial UDB Extra ALT_HOLD_PITCH_HIGH - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f8_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float sue_HEIGHT_TARGET_MAX,float sue_HEIGHT_TARGET_MIN,float sue_ALT_HOLD_THROTTLE_MIN,float sue_ALT_HOLD_THROTTLE_MAX,float sue_ALT_HOLD_PITCH_MIN,float sue_ALT_HOLD_PITCH_MAX,float sue_ALT_HOLD_PITCH_HIGH) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN]; - _mav_put_float(buf, 0, sue_HEIGHT_TARGET_MAX); - _mav_put_float(buf, 4, sue_HEIGHT_TARGET_MIN); - _mav_put_float(buf, 8, sue_ALT_HOLD_THROTTLE_MIN); - _mav_put_float(buf, 12, sue_ALT_HOLD_THROTTLE_MAX); - _mav_put_float(buf, 16, sue_ALT_HOLD_PITCH_MIN); - _mav_put_float(buf, 20, sue_ALT_HOLD_PITCH_MAX); - _mav_put_float(buf, 24, sue_ALT_HOLD_PITCH_HIGH); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); -#else - mavlink_serial_udb_extra_f8_t packet; - packet.sue_HEIGHT_TARGET_MAX = sue_HEIGHT_TARGET_MAX; - packet.sue_HEIGHT_TARGET_MIN = sue_HEIGHT_TARGET_MIN; - packet.sue_ALT_HOLD_THROTTLE_MIN = sue_ALT_HOLD_THROTTLE_MIN; - packet.sue_ALT_HOLD_THROTTLE_MAX = sue_ALT_HOLD_THROTTLE_MAX; - packet.sue_ALT_HOLD_PITCH_MIN = sue_ALT_HOLD_PITCH_MIN; - packet.sue_ALT_HOLD_PITCH_MAX = sue_ALT_HOLD_PITCH_MAX; - packet.sue_ALT_HOLD_PITCH_HIGH = sue_ALT_HOLD_PITCH_HIGH; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f8 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f8 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f8_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f8_t* serial_udb_extra_f8) -{ - return mavlink_msg_serial_udb_extra_f8_pack(system_id, component_id, msg, serial_udb_extra_f8->sue_HEIGHT_TARGET_MAX, serial_udb_extra_f8->sue_HEIGHT_TARGET_MIN, serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MIN, serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MAX, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MIN, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MAX, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_HIGH); -} - -/** - * @brief Encode a serial_udb_extra_f8 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f8 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f8_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f8_t* serial_udb_extra_f8) -{ - return mavlink_msg_serial_udb_extra_f8_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f8->sue_HEIGHT_TARGET_MAX, serial_udb_extra_f8->sue_HEIGHT_TARGET_MIN, serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MIN, serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MAX, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MIN, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MAX, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_HIGH); -} - -/** - * @brief Send a serial_udb_extra_f8 message - * @param chan MAVLink channel to send the message - * - * @param sue_HEIGHT_TARGET_MAX Serial UDB Extra HEIGHT_TARGET_MAX - * @param sue_HEIGHT_TARGET_MIN Serial UDB Extra HEIGHT_TARGET_MIN - * @param sue_ALT_HOLD_THROTTLE_MIN Serial UDB Extra ALT_HOLD_THROTTLE_MIN - * @param sue_ALT_HOLD_THROTTLE_MAX Serial UDB Extra ALT_HOLD_THROTTLE_MAX - * @param sue_ALT_HOLD_PITCH_MIN Serial UDB Extra ALT_HOLD_PITCH_MIN - * @param sue_ALT_HOLD_PITCH_MAX Serial UDB Extra ALT_HOLD_PITCH_MAX - * @param sue_ALT_HOLD_PITCH_HIGH Serial UDB Extra ALT_HOLD_PITCH_HIGH - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f8_send(mavlink_channel_t chan, float sue_HEIGHT_TARGET_MAX, float sue_HEIGHT_TARGET_MIN, float sue_ALT_HOLD_THROTTLE_MIN, float sue_ALT_HOLD_THROTTLE_MAX, float sue_ALT_HOLD_PITCH_MIN, float sue_ALT_HOLD_PITCH_MAX, float sue_ALT_HOLD_PITCH_HIGH) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN]; - _mav_put_float(buf, 0, sue_HEIGHT_TARGET_MAX); - _mav_put_float(buf, 4, sue_HEIGHT_TARGET_MIN); - _mav_put_float(buf, 8, sue_ALT_HOLD_THROTTLE_MIN); - _mav_put_float(buf, 12, sue_ALT_HOLD_THROTTLE_MAX); - _mav_put_float(buf, 16, sue_ALT_HOLD_PITCH_MIN); - _mav_put_float(buf, 20, sue_ALT_HOLD_PITCH_MAX); - _mav_put_float(buf, 24, sue_ALT_HOLD_PITCH_HIGH); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); -#endif -#else - mavlink_serial_udb_extra_f8_t packet; - packet.sue_HEIGHT_TARGET_MAX = sue_HEIGHT_TARGET_MAX; - packet.sue_HEIGHT_TARGET_MIN = sue_HEIGHT_TARGET_MIN; - packet.sue_ALT_HOLD_THROTTLE_MIN = sue_ALT_HOLD_THROTTLE_MIN; - packet.sue_ALT_HOLD_THROTTLE_MAX = sue_ALT_HOLD_THROTTLE_MAX; - packet.sue_ALT_HOLD_PITCH_MIN = sue_ALT_HOLD_PITCH_MIN; - packet.sue_ALT_HOLD_PITCH_MAX = sue_ALT_HOLD_PITCH_MAX; - packet.sue_ALT_HOLD_PITCH_HIGH = sue_ALT_HOLD_PITCH_HIGH; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F8 UNPACKING - - -/** - * @brief Get field sue_HEIGHT_TARGET_MAX from serial_udb_extra_f8 message - * - * @return Serial UDB Extra HEIGHT_TARGET_MAX - */ -static inline float mavlink_msg_serial_udb_extra_f8_get_sue_HEIGHT_TARGET_MAX(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field sue_HEIGHT_TARGET_MIN from serial_udb_extra_f8 message - * - * @return Serial UDB Extra HEIGHT_TARGET_MIN - */ -static inline float mavlink_msg_serial_udb_extra_f8_get_sue_HEIGHT_TARGET_MIN(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field sue_ALT_HOLD_THROTTLE_MIN from serial_udb_extra_f8 message - * - * @return Serial UDB Extra ALT_HOLD_THROTTLE_MIN - */ -static inline float mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_THROTTLE_MIN(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field sue_ALT_HOLD_THROTTLE_MAX from serial_udb_extra_f8 message - * - * @return Serial UDB Extra ALT_HOLD_THROTTLE_MAX - */ -static inline float mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_THROTTLE_MAX(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field sue_ALT_HOLD_PITCH_MIN from serial_udb_extra_f8 message - * - * @return Serial UDB Extra ALT_HOLD_PITCH_MIN - */ -static inline float mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_PITCH_MIN(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field sue_ALT_HOLD_PITCH_MAX from serial_udb_extra_f8 message - * - * @return Serial UDB Extra ALT_HOLD_PITCH_MAX - */ -static inline float mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_PITCH_MAX(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field sue_ALT_HOLD_PITCH_HIGH from serial_udb_extra_f8 message - * - * @return Serial UDB Extra ALT_HOLD_PITCH_HIGH - */ -static inline float mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_PITCH_HIGH(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Decode a serial_udb_extra_f8 message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f8 C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f8_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f8_t* serial_udb_extra_f8) -{ -#if MAVLINK_NEED_BYTE_SWAP - serial_udb_extra_f8->sue_HEIGHT_TARGET_MAX = mavlink_msg_serial_udb_extra_f8_get_sue_HEIGHT_TARGET_MAX(msg); - serial_udb_extra_f8->sue_HEIGHT_TARGET_MIN = mavlink_msg_serial_udb_extra_f8_get_sue_HEIGHT_TARGET_MIN(msg); - serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MIN = mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_THROTTLE_MIN(msg); - serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MAX = mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_THROTTLE_MAX(msg); - serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MIN = mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_PITCH_MIN(msg); - serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MAX = mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_PITCH_MAX(msg); - serial_udb_extra_f8->sue_ALT_HOLD_PITCH_HIGH = mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_PITCH_HIGH(msg); -#else - memcpy(serial_udb_extra_f8, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/testsuite.h b/mavlink/include/mavlink/v1.0/matrixpilot/testsuite.h deleted file mode 100644 index 6856839..0000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/testsuite.h +++ /dev/null @@ -1,1240 +0,0 @@ -/** @file - * @brief MAVLink comm protocol testsuite generated from matrixpilot.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef MATRIXPILOT_TESTSUITE_H -#define MATRIXPILOT_TESTSUITE_H - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef MAVLINK_TEST_ALL -#define MAVLINK_TEST_ALL -static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); -static void mavlink_test_matrixpilot(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_common(system_id, component_id, last_msg); - mavlink_test_matrixpilot(system_id, component_id, last_msg); -} -#endif - -#include "../common/testsuite.h" - - -static void mavlink_test_flexifunction_set(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_flexifunction_set_t packet_in = { - 5, - }72, - }; - mavlink_flexifunction_set_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.target_system = packet_in.target_system; - packet1.target_component = packet_in.target_component; - - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_flexifunction_set_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_flexifunction_set_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_flexifunction_set_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component ); - mavlink_msg_flexifunction_set_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_flexifunction_set_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component ); - mavlink_msg_flexifunction_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; imagic = MAVLINK_STX; msg->len = length; msg->sysid = system_id; @@ -81,12 +80,13 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui // One sequence number per component msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; - checksum = crc_calculate((uint8_t*)&msg->len, length + MAVLINK_CORE_HEADER_LEN); + 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, &checksum); + crc_accumulate(crc_extra, &msg->checksum); #endif - mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF); - mavlink_ck_b(msg) = (uint8_t)(checksum >> 8); + 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; } @@ -133,7 +133,7 @@ MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint buf[4] = mavlink_system.compid; buf[5] = msgid; status->current_tx_seq++; - checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); crc_accumulate_buffer(&checksum, packet, length); #if MAVLINK_CRC_EXTRA crc_accumulate(crc_extra, &checksum); @@ -158,6 +158,7 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m ck[0] = (uint8_t)(msg->checksum & 0xFF); ck[1] = (uint8_t)(msg->checksum >> 8); + // XXX use the right sequence here MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len); _mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES); @@ -172,7 +173,13 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m */ MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg) { - memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len); + memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len); + + uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len); + + ck[0] = (uint8_t)(msg->checksum & 0xFF); + ck[1] = (uint8_t)(msg->checksum >> 8); + return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len; } @@ -202,13 +209,18 @@ MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) * it could be successfully decoded. Checksum and other failures will be silently * ignored. * + * 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. + * * @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 * 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 barse + * @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 * @return 0 if no message could be decoded, 1 else * * A typical use scenario of this function call is: @@ -251,7 +263,7 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa and out). Only use if the channel will only contain messages types listed in the headers. */ -#if MAVLINK_CHECK_MESSAGE_LENGTH +#ifdef MAVLINK_CHECK_MESSAGE_LENGTH #ifndef MAVLINK_MESSAGE_LENGTH static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; #define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid] @@ -320,7 +332,7 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa break; case MAVLINK_PARSE_STATE_GOT_COMPID: -#if MAVLINK_CHECK_MESSAGE_LENGTH +#ifdef MAVLINK_CHECK_MESSAGE_LENGTH if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(c)) { status->parse_error++; diff --git a/mavlink/include/mavlink/v1.0/mavlink_types.h b/mavlink/include/mavlink/v1.0/mavlink_types.h index 4019c61..de98622 100644 --- a/mavlink/include/mavlink/v1.0/mavlink_types.h +++ b/mavlink/include/mavlink/v1.0/mavlink_types.h @@ -1,7 +1,12 @@ #ifndef MAVLINK_TYPES_H_ #define MAVLINK_TYPES_H_ +// Visual Studio versions before 2013 don't conform to C99. +#if (defined _MSC_VER) && (_MSC_VER < 1800) +#include +#else #include +#endif #ifndef MAVLINK_MAX_PAYLOAD_LEN // it is possible to override this, but be careful! @@ -18,7 +23,7 @@ #define MAVLINK_MSG_ID_EXTENDED_MESSAGE 255 #define MAVLINK_EXTENDED_HEADER_LEN 14 -#if (defined _MSC_VER) | ((defined __APPLE__) & (defined __MACH__)) | (defined __linux__) +#if (defined _MSC_VER) || ((defined __APPLE__) && (defined __MACH__)) || (defined __linux__) /* full fledged 32bit++ OS */ #define MAVLINK_MAX_EXTENDED_PACKET_LEN 65507 #else @@ -28,6 +33,17 @@ #define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES) +#pragma pack(push, 1) + +/** + * Old-style 4 byte param union + * + * This struct is the data format to be used when sending + * parameters. The parameter should be copied to the native + * type (without type conversion) + * and re-instanted on the receiving side using the + * native type as well. + */ typedef struct param_union { union { float param_float; @@ -42,6 +58,39 @@ typedef struct param_union { uint8_t type; } mavlink_param_union_t; + +/** + * New-style 8 byte param union + * mavlink_param_union_double_t will be 8 bytes long, and treated as needing 8 byte alignment for the purposes of MAVLink 1.0 field ordering. + * The mavlink_param_union_double_t will be treated as a little-endian structure. + * + * If is_double is 1 then the type is a double, and the remaining 63 bits are the double, with the lowest bit of the mantissa zero. + * The intention is that by replacing the is_double bit with 0 the type can be directly used as a double (as the is_double bit corresponds to the + * lowest mantissa bit of a double). If is_double is 0 then mavlink_type gives the type in the union. + * The mavlink_types.h header will also need to have shifts/masks to define the bit boundaries in the above, + * as bitfield ordering isn’t consistent between platforms. The above is intended to be for gcc on x86, + * which should be the same as gcc on little-endian arm. When using shifts/masks the value will be treated as a 64 bit unsigned number, + * and the bits pulled out using the shifts/masks. +*/ +typedef union { + struct { + uint8_t is_double:1; + uint8_t mavlink_type:7; + union { + char c; + uint8_t uint8; + int8_t int8; + uint16_t uint16; + int16_t int16; + uint32_t uint32; + int32_t int32; + float f; + uint8_t align[7]; + }; + }; + uint8_t data[8]; +} mavlink_param_union_double_t; + typedef struct __mavlink_system { uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function @@ -52,7 +101,7 @@ typedef struct __mavlink_system { } mavlink_system_t; typedef struct __mavlink_message { - uint16_t checksum; /// sent at end of packet + uint16_t checksum; ///< sent at end of packet uint8_t magic; ///< protocol magic marker uint8_t len; ///< Length of payload uint8_t seq; ///< Sequence of packet @@ -62,13 +111,12 @@ typedef struct __mavlink_message { uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; } mavlink_message_t; - typedef struct __mavlink_extended_message { mavlink_message_t base_msg; int32_t extended_payload_len; ///< Length of extended payload if any uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN]; } mavlink_extended_message_t; - +#pragma pack(pop) typedef enum { MAVLINK_TYPE_CHAR = 0, diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink.h b/mavlink/include/mavlink/v1.0/minimal/mavlink.h similarity index 74% rename from mavlink/include/mavlink/v1.0/pixhawk/mavlink.h rename to mavlink/include/mavlink/v1.0/minimal/mavlink.h index f062233..396f7da 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink.h +++ b/mavlink/include/mavlink/v1.0/minimal/mavlink.h @@ -1,6 +1,6 @@ /** @file - * @brief MAVLink comm protocol built from pixhawk.xml - * @see http://pixhawk.ethz.ch/software/mavlink + * @brief MAVLink comm protocol built from minimal.xml + * @see http://mavlink.org */ #ifndef MAVLINK_H #define MAVLINK_H @@ -22,6 +22,6 @@ #endif #include "version.h" -#include "pixhawk.h" +#include "minimal.h" #endif // MAVLINK_H diff --git a/mavlink/include/mavlink/v1.0/minimal/mavlink_msg_heartbeat.h b/mavlink/include/mavlink/v1.0/minimal/mavlink_msg_heartbeat.h new file mode 100644 index 0000000..37e682a --- /dev/null +++ b/mavlink/include/mavlink/v1.0/minimal/mavlink_msg_heartbeat.h @@ -0,0 +1,326 @@ +// 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_FLAGS ENUM in mavlink/include/mavlink_types.h + uint8_t system_status; ///< System status flag, see MAV_STATE ENUM + uint8_t mavlink_version; ///< MAVLink version +} mavlink_heartbeat_t; + +#define MAVLINK_MSG_ID_HEARTBEAT_LEN 9 +#define MAVLINK_MSG_ID_0_LEN 9 + +#define MAVLINK_MSG_ID_HEARTBEAT_CRC 50 +#define MAVLINK_MSG_ID_0_CRC 50 + + + +#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) }, \ + { "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) }, \ + { "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) }, \ + } \ +} + + +/** + * @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 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_FLAGS 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) +{ +#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); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif +} + +/** + * @brief Pack a heartbeat message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param 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_FLAGS 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) +{ +#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_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 +} + +/** + * @brief Encode a heartbeat struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param heartbeat C-struct to read the message contents from + */ +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); +} + +/** + * @brief Encode a heartbeat struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param heartbeat C-struct to read the message contents from + */ +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); +} + +/** + * @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_FLAGS 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); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#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); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif +#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 + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_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); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE HEARTBEAT UNPACKING + + +/** + * @brief Get field type from heartbeat message + * + * @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); +} + +/** + * @brief Get field autopilot from heartbeat message + * + * @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); +} + +/** + * @brief Get field base_mode from heartbeat message + * + * @return System mode bitfield, see MAV_MODE_FLAGS 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); +} + +/** + * @brief Get field custom_mode from heartbeat message + * + * @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); +} + +/** + * @brief Get field system_status from heartbeat message + * + * @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); +} + +/** + * @brief Get field mavlink_version from heartbeat message + * + * @return MAVLink version + */ +static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Decode a heartbeat message into a struct + * + * @param msg The message to decode + * @param heartbeat C-struct to decode the message contents into + */ +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); +#else + memcpy(heartbeat, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/minimal/minimal.h b/mavlink/include/mavlink/v1.0/minimal/minimal.h new file mode 100644 index 0000000..6935d82 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/minimal/minimal.h @@ -0,0 +1,154 @@ +/** @file + * @brief MAVLink comm protocol generated from minimal.xml + * @see http://mavlink.org + */ +#ifndef MAVLINK_MINIMAL_H +#define MAVLINK_MINIMAL_H + +#ifndef MAVLINK_H + #error Wrong include order: MAVLINK_MINIMAL.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call. +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +// MESSAGE LENGTHS AND CRCS + +#ifndef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS {9, 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, 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, 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, 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} +#endif + +#ifndef MAVLINK_MESSAGE_CRCS +#define MAVLINK_MESSAGE_CRCS {50, 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, 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, 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, 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} +#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}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#endif + +#include "../protocol.h" + +#define MAVLINK_ENABLED_MINIMAL + +// ENUM DEFINITIONS + + +/** @brief Micro air vehicle / autopilot classes. This identifies the individual model. */ +#ifndef HAVE_ENUM_MAV_AUTOPILOT +#define HAVE_ENUM_MAV_AUTOPILOT +typedef enum MAV_AUTOPILOT +{ + MAV_AUTOPILOT_GENERIC=0, /* Generic autopilot, full support for everything | */ + MAV_AUTOPILOT_PIXHAWK=1, /* PIXHAWK autopilot, http://pixhawk.ethz.ch | */ + MAV_AUTOPILOT_SLUGS=2, /* SLUGS autopilot, http://slugsuav.soe.ucsc.edu | */ + MAV_AUTOPILOT_ARDUPILOTMEGA=3, /* ArduPilotMega / ArduCopter, http://diydrones.com | */ + MAV_AUTOPILOT_OPENPILOT=4, /* OpenPilot, http://openpilot.org | */ + MAV_AUTOPILOT_GENERIC_WAYPOINTS_ONLY=5, /* Generic autopilot only supporting simple waypoints | */ + MAV_AUTOPILOT_GENERIC_WAYPOINTS_AND_SIMPLE_NAVIGATION_ONLY=6, /* Generic autopilot supporting waypoints and other simple navigation commands | */ + MAV_AUTOPILOT_GENERIC_MISSION_FULL=7, /* Generic autopilot supporting the full mission command set | */ + MAV_AUTOPILOT_INVALID=8, /* No valid autopilot, e.g. a GCS or other MAVLink component | */ + MAV_AUTOPILOT_PPZ=9, /* PPZ UAV - http://nongnu.org/paparazzi | */ + MAV_AUTOPILOT_UDB=10, /* UAV Dev Board | */ + MAV_AUTOPILOT_FP=11, /* FlexiPilot | */ + MAV_AUTOPILOT_ENUM_END=12, /* | */ +} MAV_AUTOPILOT; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_TYPE +#define HAVE_ENUM_MAV_TYPE +typedef enum MAV_TYPE +{ + MAV_TYPE_GENERIC=0, /* Generic micro air vehicle. | */ + MAV_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */ + MAV_TYPE_QUADROTOR=2, /* Quadrotor | */ + MAV_TYPE_COAXIAL=3, /* Coaxial helicopter | */ + MAV_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */ + MAV_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */ + MAV_TYPE_GCS=6, /* Operator control unit / ground control station | */ + MAV_TYPE_AIRSHIP=7, /* Airship, controlled | */ + MAV_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */ + MAV_TYPE_ROCKET=9, /* Rocket | */ + MAV_TYPE_GROUND_ROVER=10, /* Ground rover | */ + MAV_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */ + MAV_TYPE_SUBMARINE=12, /* Submarine | */ + MAV_TYPE_HEXAROTOR=13, /* Hexarotor | */ + MAV_TYPE_OCTOROTOR=14, /* Octorotor | */ + MAV_TYPE_TRICOPTER=15, /* Octorotor | */ + MAV_TYPE_FLAPPING_WING=16, /* Flapping wing | */ + MAV_TYPE_ENUM_END=17, /* | */ +} MAV_TYPE; +#endif + +/** @brief These flags encode the MAV mode. */ +#ifndef HAVE_ENUM_MAV_MODE_FLAG +#define HAVE_ENUM_MAV_MODE_FLAG +typedef enum MAV_MODE_FLAG +{ + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED=1, /* 0b00000001 Reserved for future use. | */ + MAV_MODE_FLAG_TEST_ENABLED=2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ + MAV_MODE_FLAG_AUTO_ENABLED=4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ + MAV_MODE_FLAG_GUIDED_ENABLED=8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */ + MAV_MODE_FLAG_STABILIZE_ENABLED=16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ + MAV_MODE_FLAG_HIL_ENABLED=32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ + MAV_MODE_FLAG_MANUAL_INPUT_ENABLED=64, /* 0b01000000 remote control input is enabled. | */ + MAV_MODE_FLAG_SAFETY_ARMED=128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */ + MAV_MODE_FLAG_ENUM_END=129, /* | */ +} MAV_MODE_FLAG; +#endif + +/** @brief These values encode the bit positions of the decode position. These values can be used to read the value of a flag bit by combining the base_mode variable with AND with the flag position value. The result will be either 0 or 1, depending on if the flag is set or not. */ +#ifndef HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION +#define HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION +typedef enum MAV_MODE_FLAG_DECODE_POSITION +{ + MAV_MODE_FLAG_DECODE_POSITION_CUSTOM_MODE=1, /* Eighth bit: 00000001 | */ + MAV_MODE_FLAG_DECODE_POSITION_TEST=2, /* Seventh bit: 00000010 | */ + MAV_MODE_FLAG_DECODE_POSITION_AUTO=4, /* Sixt bit: 00000100 | */ + MAV_MODE_FLAG_DECODE_POSITION_GUIDED=8, /* Fifth bit: 00001000 | */ + MAV_MODE_FLAG_DECODE_POSITION_STABILIZE=16, /* Fourth bit: 00010000 | */ + MAV_MODE_FLAG_DECODE_POSITION_HIL=32, /* Third bit: 00100000 | */ + MAV_MODE_FLAG_DECODE_POSITION_MANUAL=64, /* Second bit: 01000000 | */ + MAV_MODE_FLAG_DECODE_POSITION_SAFETY=128, /* First bit: 10000000 | */ + MAV_MODE_FLAG_DECODE_POSITION_ENUM_END=129, /* | */ +} MAV_MODE_FLAG_DECODE_POSITION; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_STATE +#define HAVE_ENUM_MAV_STATE +typedef enum MAV_STATE +{ + MAV_STATE_UNINIT=0, /* Uninitialized system, state is unknown. | */ + MAV_STATE_BOOT=1, /* System is booting up. | */ + MAV_STATE_CALIBRATING=2, /* System is calibrating and not flight-ready. | */ + MAV_STATE_STANDBY=3, /* System is grounded and on standby. It can be launched any time. | */ + MAV_STATE_ACTIVE=4, /* System is active and might be already airborne. Motors are engaged. | */ + MAV_STATE_CRITICAL=5, /* System is in a non-normal flight mode. It can however still navigate. | */ + MAV_STATE_EMERGENCY=6, /* System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. | */ + MAV_STATE_POWEROFF=7, /* System just initialized its power-down sequence, will shut down now. | */ + MAV_STATE_ENUM_END=8, /* | */ +} MAV_STATE; +#endif + + + +// MAVLINK VERSION + +#ifndef MAVLINK_VERSION +#define MAVLINK_VERSION 2 +#endif + +#if (MAVLINK_VERSION == 0) +#undef MAVLINK_VERSION +#define MAVLINK_VERSION 2 +#endif + +// MESSAGE DEFINITIONS +#include "./mavlink_msg_heartbeat.h" + +#ifdef __cplusplus +} +#endif // __cplusplus +#endif // MAVLINK_MINIMAL_H diff --git a/mavlink/include/mavlink/v1.0/minimal/testsuite.h b/mavlink/include/mavlink/v1.0/minimal/testsuite.h new file mode 100644 index 0000000..8e02237 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/minimal/testsuite.h @@ -0,0 +1,83 @@ +/** @file + * @brief MAVLink comm protocol testsuite generated from minimal.xml + * @see http://qgroundcontrol.org/mavlink/ + */ +#ifndef MINIMAL_TESTSUITE_H +#define MINIMAL_TESTSUITE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef MAVLINK_TEST_ALL +#define MAVLINK_TEST_ALL + +static void mavlink_test_minimal(uint8_t, uint8_t, mavlink_message_t *last_msg); + +static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + + mavlink_test_minimal(system_id, component_id, last_msg); +} +#endif + + + + +static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + 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 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; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_heartbeat_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_heartbeat_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_heartbeat_pack(system_id, component_id, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status ); + mavlink_msg_heartbeat_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_heartbeat_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status ); + mavlink_msg_heartbeat_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; imsgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN, MAVLINK_MSG_ID_ATTITUDE_CONTROL_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); -#endif -} - -/** - * @brief Pack a attitude_control message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target The system to be controlled - * @param roll roll - * @param pitch pitch - * @param yaw yaw - * @param thrust thrust - * @param roll_manual roll control enabled auto:0, manual:1 - * @param pitch_manual pitch auto:0, manual:1 - * @param yaw_manual yaw auto:0, manual:1 - * @param thrust_manual thrust auto:0, manual:1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_attitude_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target,float roll,float pitch,float yaw,float thrust,uint8_t roll_manual,uint8_t pitch_manual,uint8_t yaw_manual,uint8_t thrust_manual) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, thrust); - _mav_put_uint8_t(buf, 16, target); - _mav_put_uint8_t(buf, 17, roll_manual); - _mav_put_uint8_t(buf, 18, pitch_manual); - _mav_put_uint8_t(buf, 19, yaw_manual); - _mav_put_uint8_t(buf, 20, thrust_manual); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); -#else - mavlink_attitude_control_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.thrust = thrust; - packet.target = target; - packet.roll_manual = roll_manual; - packet.pitch_manual = pitch_manual; - packet.yaw_manual = yaw_manual; - packet.thrust_manual = thrust_manual; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN, MAVLINK_MSG_ID_ATTITUDE_CONTROL_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); -#endif -} - -/** - * @brief Encode a attitude_control struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param attitude_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_attitude_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_control_t* attitude_control) -{ - return mavlink_msg_attitude_control_pack(system_id, component_id, msg, attitude_control->target, attitude_control->roll, attitude_control->pitch, attitude_control->yaw, attitude_control->thrust, attitude_control->roll_manual, attitude_control->pitch_manual, attitude_control->yaw_manual, attitude_control->thrust_manual); -} - -/** - * @brief Encode a attitude_control struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param attitude_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_attitude_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_control_t* attitude_control) -{ - return mavlink_msg_attitude_control_pack_chan(system_id, component_id, chan, msg, attitude_control->target, attitude_control->roll, attitude_control->pitch, attitude_control->yaw, attitude_control->thrust, attitude_control->roll_manual, attitude_control->pitch_manual, attitude_control->yaw_manual, attitude_control->thrust_manual); -} - -/** - * @brief Send a attitude_control message - * @param chan MAVLink channel to send the message - * - * @param target The system to be controlled - * @param roll roll - * @param pitch pitch - * @param yaw yaw - * @param thrust thrust - * @param roll_manual roll control enabled auto:0, manual:1 - * @param pitch_manual pitch auto:0, manual:1 - * @param yaw_manual yaw auto:0, manual:1 - * @param thrust_manual thrust auto:0, manual:1 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_attitude_control_send(mavlink_channel_t chan, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, thrust); - _mav_put_uint8_t(buf, 16, target); - _mav_put_uint8_t(buf, 17, roll_manual); - _mav_put_uint8_t(buf, 18, pitch_manual); - _mav_put_uint8_t(buf, 19, yaw_manual); - _mav_put_uint8_t(buf, 20, thrust_manual); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, buf, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN, MAVLINK_MSG_ID_ATTITUDE_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, buf, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); -#endif -#else - mavlink_attitude_control_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.thrust = thrust; - packet.target = target; - packet.roll_manual = roll_manual; - packet.pitch_manual = pitch_manual; - packet.yaw_manual = yaw_manual; - packet.thrust_manual = thrust_manual; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN, MAVLINK_MSG_ID_ATTITUDE_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); -#endif -#endif -} - -#endif - -// MESSAGE ATTITUDE_CONTROL UNPACKING - - -/** - * @brief Get field target from attitude_control message - * - * @return The system to be controlled - */ -static inline uint8_t mavlink_msg_attitude_control_get_target(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Get field roll from attitude_control message - * - * @return roll - */ -static inline float mavlink_msg_attitude_control_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field pitch from attitude_control message - * - * @return pitch - */ -static inline float mavlink_msg_attitude_control_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field yaw from attitude_control message - * - * @return yaw - */ -static inline float mavlink_msg_attitude_control_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field thrust from attitude_control message - * - * @return thrust - */ -static inline float mavlink_msg_attitude_control_get_thrust(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field roll_manual from attitude_control message - * - * @return roll control enabled auto:0, manual:1 - */ -static inline uint8_t mavlink_msg_attitude_control_get_roll_manual(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 17); -} - -/** - * @brief Get field pitch_manual from attitude_control message - * - * @return pitch auto:0, manual:1 - */ -static inline uint8_t mavlink_msg_attitude_control_get_pitch_manual(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 18); -} - -/** - * @brief Get field yaw_manual from attitude_control message - * - * @return yaw auto:0, manual:1 - */ -static inline uint8_t mavlink_msg_attitude_control_get_yaw_manual(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 19); -} - -/** - * @brief Get field thrust_manual from attitude_control message - * - * @return thrust auto:0, manual:1 - */ -static inline uint8_t mavlink_msg_attitude_control_get_thrust_manual(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 20); -} - -/** - * @brief Decode a attitude_control message into a struct - * - * @param msg The message to decode - * @param attitude_control C-struct to decode the message contents into - */ -static inline void mavlink_msg_attitude_control_decode(const mavlink_message_t* msg, mavlink_attitude_control_t* attitude_control) -{ -#if MAVLINK_NEED_BYTE_SWAP - attitude_control->roll = mavlink_msg_attitude_control_get_roll(msg); - attitude_control->pitch = mavlink_msg_attitude_control_get_pitch(msg); - attitude_control->yaw = mavlink_msg_attitude_control_get_yaw(msg); - attitude_control->thrust = mavlink_msg_attitude_control_get_thrust(msg); - attitude_control->target = mavlink_msg_attitude_control_get_target(msg); - attitude_control->roll_manual = mavlink_msg_attitude_control_get_roll_manual(msg); - attitude_control->pitch_manual = mavlink_msg_attitude_control_get_pitch_manual(msg); - attitude_control->yaw_manual = mavlink_msg_attitude_control_get_yaw_manual(msg); - attitude_control->thrust_manual = mavlink_msg_attitude_control_get_thrust_manual(msg); -#else - memcpy(attitude_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h deleted file mode 100644 index d410937..0000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h +++ /dev/null @@ -1,325 +0,0 @@ -// MESSAGE BRIEF_FEATURE PACKING - -#define MAVLINK_MSG_ID_BRIEF_FEATURE 195 - -typedef struct __mavlink_brief_feature_t -{ - float x; ///< x position in m - float y; ///< y position in m - float z; ///< z position in m - float response; ///< Harris operator response at this location - uint16_t size; ///< Size in pixels - uint16_t orientation; ///< Orientation - uint8_t orientation_assignment; ///< Orientation assignment 0: false, 1:true - uint8_t descriptor[32]; ///< Descriptor -} mavlink_brief_feature_t; - -#define MAVLINK_MSG_ID_BRIEF_FEATURE_LEN 53 -#define MAVLINK_MSG_ID_195_LEN 53 - -#define MAVLINK_MSG_ID_BRIEF_FEATURE_CRC 88 -#define MAVLINK_MSG_ID_195_CRC 88 - -#define MAVLINK_MSG_BRIEF_FEATURE_FIELD_DESCRIPTOR_LEN 32 - -#define MAVLINK_MESSAGE_INFO_BRIEF_FEATURE { \ - "BRIEF_FEATURE", \ - 8, \ - { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_brief_feature_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_brief_feature_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_brief_feature_t, z) }, \ - { "response", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_brief_feature_t, response) }, \ - { "size", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_brief_feature_t, size) }, \ - { "orientation", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_brief_feature_t, orientation) }, \ - { "orientation_assignment", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_brief_feature_t, orientation_assignment) }, \ - { "descriptor", NULL, MAVLINK_TYPE_UINT8_T, 32, 21, offsetof(mavlink_brief_feature_t, descriptor) }, \ - } \ -} - - -/** - * @brief Pack a brief_feature message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param x x position in m - * @param y y position in m - * @param z z position in m - * @param orientation_assignment Orientation assignment 0: false, 1:true - * @param size Size in pixels - * @param orientation Orientation - * @param descriptor Descriptor - * @param response Harris operator response at this location - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const uint8_t *descriptor, float response) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_BRIEF_FEATURE_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, response); - _mav_put_uint16_t(buf, 16, size); - _mav_put_uint16_t(buf, 18, orientation); - _mav_put_uint8_t(buf, 20, orientation_assignment); - _mav_put_uint8_t_array(buf, 21, descriptor, 32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); -#else - mavlink_brief_feature_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.response = response; - packet.size = size; - packet.orientation = orientation; - packet.orientation_assignment = orientation_assignment; - mav_array_memcpy(packet.descriptor, descriptor, sizeof(uint8_t)*32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN, MAVLINK_MSG_ID_BRIEF_FEATURE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); -#endif -} - -/** - * @brief Pack a brief_feature message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param x x position in m - * @param y y position in m - * @param z z position in m - * @param orientation_assignment Orientation assignment 0: false, 1:true - * @param size Size in pixels - * @param orientation Orientation - * @param descriptor Descriptor - * @param response Harris operator response at this location - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_brief_feature_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float x,float y,float z,uint8_t orientation_assignment,uint16_t size,uint16_t orientation,const uint8_t *descriptor,float response) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_BRIEF_FEATURE_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, response); - _mav_put_uint16_t(buf, 16, size); - _mav_put_uint16_t(buf, 18, orientation); - _mav_put_uint8_t(buf, 20, orientation_assignment); - _mav_put_uint8_t_array(buf, 21, descriptor, 32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); -#else - mavlink_brief_feature_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.response = response; - packet.size = size; - packet.orientation = orientation; - packet.orientation_assignment = orientation_assignment; - mav_array_memcpy(packet.descriptor, descriptor, sizeof(uint8_t)*32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN, MAVLINK_MSG_ID_BRIEF_FEATURE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); -#endif -} - -/** - * @brief Encode a brief_feature struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param brief_feature C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_brief_feature_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_brief_feature_t* brief_feature) -{ - return mavlink_msg_brief_feature_pack(system_id, component_id, msg, brief_feature->x, brief_feature->y, brief_feature->z, brief_feature->orientation_assignment, brief_feature->size, brief_feature->orientation, brief_feature->descriptor, brief_feature->response); -} - -/** - * @brief Encode a brief_feature struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param brief_feature C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_brief_feature_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_brief_feature_t* brief_feature) -{ - return mavlink_msg_brief_feature_pack_chan(system_id, component_id, chan, msg, brief_feature->x, brief_feature->y, brief_feature->z, brief_feature->orientation_assignment, brief_feature->size, brief_feature->orientation, brief_feature->descriptor, brief_feature->response); -} - -/** - * @brief Send a brief_feature message - * @param chan MAVLink channel to send the message - * - * @param x x position in m - * @param y y position in m - * @param z z position in m - * @param orientation_assignment Orientation assignment 0: false, 1:true - * @param size Size in pixels - * @param orientation Orientation - * @param descriptor Descriptor - * @param response Harris operator response at this location - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const uint8_t *descriptor, float response) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_BRIEF_FEATURE_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, response); - _mav_put_uint16_t(buf, 16, size); - _mav_put_uint16_t(buf, 18, orientation); - _mav_put_uint8_t(buf, 20, orientation_assignment); - _mav_put_uint8_t_array(buf, 21, descriptor, 32); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, buf, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN, MAVLINK_MSG_ID_BRIEF_FEATURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, buf, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); -#endif -#else - mavlink_brief_feature_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.response = response; - packet.size = size; - packet.orientation = orientation; - packet.orientation_assignment = orientation_assignment; - mav_array_memcpy(packet.descriptor, descriptor, sizeof(uint8_t)*32); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, (const char *)&packet, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN, MAVLINK_MSG_ID_BRIEF_FEATURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, (const char *)&packet, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); -#endif -#endif -} - -#endif - -// MESSAGE BRIEF_FEATURE UNPACKING - - -/** - * @brief Get field x from brief_feature message - * - * @return x position in m - */ -static inline float mavlink_msg_brief_feature_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field y from brief_feature message - * - * @return y position in m - */ -static inline float mavlink_msg_brief_feature_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field z from brief_feature message - * - * @return z position in m - */ -static inline float mavlink_msg_brief_feature_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field orientation_assignment from brief_feature message - * - * @return Orientation assignment 0: false, 1:true - */ -static inline uint8_t mavlink_msg_brief_feature_get_orientation_assignment(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 20); -} - -/** - * @brief Get field size from brief_feature message - * - * @return Size in pixels - */ -static inline uint16_t mavlink_msg_brief_feature_get_size(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 16); -} - -/** - * @brief Get field orientation from brief_feature message - * - * @return Orientation - */ -static inline uint16_t mavlink_msg_brief_feature_get_orientation(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 18); -} - -/** - * @brief Get field descriptor from brief_feature message - * - * @return Descriptor - */ -static inline uint16_t mavlink_msg_brief_feature_get_descriptor(const mavlink_message_t* msg, uint8_t *descriptor) -{ - return _MAV_RETURN_uint8_t_array(msg, descriptor, 32, 21); -} - -/** - * @brief Get field response from brief_feature message - * - * @return Harris operator response at this location - */ -static inline float mavlink_msg_brief_feature_get_response(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a brief_feature message into a struct - * - * @param msg The message to decode - * @param brief_feature C-struct to decode the message contents into - */ -static inline void mavlink_msg_brief_feature_decode(const mavlink_message_t* msg, mavlink_brief_feature_t* brief_feature) -{ -#if MAVLINK_NEED_BYTE_SWAP - brief_feature->x = mavlink_msg_brief_feature_get_x(msg); - brief_feature->y = mavlink_msg_brief_feature_get_y(msg); - brief_feature->z = mavlink_msg_brief_feature_get_z(msg); - brief_feature->response = mavlink_msg_brief_feature_get_response(msg); - brief_feature->size = mavlink_msg_brief_feature_get_size(msg); - brief_feature->orientation = mavlink_msg_brief_feature_get_orientation(msg); - brief_feature->orientation_assignment = mavlink_msg_brief_feature_get_orientation_assignment(msg); - mavlink_msg_brief_feature_get_descriptor(msg, brief_feature->descriptor); -#else - memcpy(brief_feature, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h deleted file mode 100644 index ae4db82..0000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h +++ /dev/null @@ -1,661 +0,0 @@ -// MESSAGE IMAGE_AVAILABLE PACKING - -#define MAVLINK_MSG_ID_IMAGE_AVAILABLE 154 - -typedef struct __mavlink_image_available_t -{ - uint64_t cam_id; ///< Camera id - uint64_t timestamp; ///< Timestamp - uint64_t valid_until; ///< Until which timestamp this buffer will stay valid - uint32_t img_seq; ///< The image sequence number - uint32_t img_buf_index; ///< Position of the image in the buffer, starts with 0 - uint32_t key; ///< Shared memory area key - uint32_t exposure; ///< Exposure time, in microseconds - float gain; ///< Camera gain - float roll; ///< Roll angle in rad - float pitch; ///< Pitch angle in rad - float yaw; ///< Yaw angle in rad - float local_z; ///< Local frame Z coordinate (height over ground) - float lat; ///< GPS X coordinate - float lon; ///< GPS Y coordinate - float alt; ///< Global frame altitude - float ground_x; ///< Ground truth X - float ground_y; ///< Ground truth Y - float ground_z; ///< Ground truth Z - uint16_t width; ///< Image width - uint16_t height; ///< Image height - uint16_t depth; ///< Image depth - uint8_t cam_no; ///< Camera # (starts with 0) - uint8_t channels; ///< Image channels -} mavlink_image_available_t; - -#define MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN 92 -#define MAVLINK_MSG_ID_154_LEN 92 - -#define MAVLINK_MSG_ID_IMAGE_AVAILABLE_CRC 224 -#define MAVLINK_MSG_ID_154_CRC 224 - - - -#define MAVLINK_MESSAGE_INFO_IMAGE_AVAILABLE { \ - "IMAGE_AVAILABLE", \ - 23, \ - { { "cam_id", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_image_available_t, cam_id) }, \ - { "timestamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 8, offsetof(mavlink_image_available_t, timestamp) }, \ - { "valid_until", NULL, MAVLINK_TYPE_UINT64_T, 0, 16, offsetof(mavlink_image_available_t, valid_until) }, \ - { "img_seq", NULL, MAVLINK_TYPE_UINT32_T, 0, 24, offsetof(mavlink_image_available_t, img_seq) }, \ - { "img_buf_index", NULL, MAVLINK_TYPE_UINT32_T, 0, 28, offsetof(mavlink_image_available_t, img_buf_index) }, \ - { "key", NULL, MAVLINK_TYPE_UINT32_T, 0, 32, offsetof(mavlink_image_available_t, key) }, \ - { "exposure", NULL, MAVLINK_TYPE_UINT32_T, 0, 36, offsetof(mavlink_image_available_t, exposure) }, \ - { "gain", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_image_available_t, gain) }, \ - { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_image_available_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_image_available_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_image_available_t, yaw) }, \ - { "local_z", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_image_available_t, local_z) }, \ - { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_image_available_t, lat) }, \ - { "lon", NULL, MAVLINK_TYPE_FLOAT, 0, 64, offsetof(mavlink_image_available_t, lon) }, \ - { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 68, offsetof(mavlink_image_available_t, alt) }, \ - { "ground_x", NULL, MAVLINK_TYPE_FLOAT, 0, 72, offsetof(mavlink_image_available_t, ground_x) }, \ - { "ground_y", NULL, MAVLINK_TYPE_FLOAT, 0, 76, offsetof(mavlink_image_available_t, ground_y) }, \ - { "ground_z", NULL, MAVLINK_TYPE_FLOAT, 0, 80, offsetof(mavlink_image_available_t, ground_z) }, \ - { "width", NULL, MAVLINK_TYPE_UINT16_T, 0, 84, offsetof(mavlink_image_available_t, width) }, \ - { "height", NULL, MAVLINK_TYPE_UINT16_T, 0, 86, offsetof(mavlink_image_available_t, height) }, \ - { "depth", NULL, MAVLINK_TYPE_UINT16_T, 0, 88, offsetof(mavlink_image_available_t, depth) }, \ - { "cam_no", NULL, MAVLINK_TYPE_UINT8_T, 0, 90, offsetof(mavlink_image_available_t, cam_no) }, \ - { "channels", NULL, MAVLINK_TYPE_UINT8_T, 0, 91, offsetof(mavlink_image_available_t, channels) }, \ - } \ -} - - -/** - * @brief Pack a image_available message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param cam_id Camera id - * @param cam_no Camera # (starts with 0) - * @param timestamp Timestamp - * @param valid_until Until which timestamp this buffer will stay valid - * @param img_seq The image sequence number - * @param img_buf_index Position of the image in the buffer, starts with 0 - * @param width Image width - * @param height Image height - * @param depth Image depth - * @param channels Image channels - * @param key Shared memory area key - * @param exposure Exposure time, in microseconds - * @param gain Camera gain - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @param local_z Local frame Z coordinate (height over ground) - * @param lat GPS X coordinate - * @param lon GPS Y coordinate - * @param alt Global frame altitude - * @param ground_x Ground truth X - * @param ground_y Ground truth Y - * @param ground_z Ground truth Z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN]; - _mav_put_uint64_t(buf, 0, cam_id); - _mav_put_uint64_t(buf, 8, timestamp); - _mav_put_uint64_t(buf, 16, valid_until); - _mav_put_uint32_t(buf, 24, img_seq); - _mav_put_uint32_t(buf, 28, img_buf_index); - _mav_put_uint32_t(buf, 32, key); - _mav_put_uint32_t(buf, 36, exposure); - _mav_put_float(buf, 40, gain); - _mav_put_float(buf, 44, roll); - _mav_put_float(buf, 48, pitch); - _mav_put_float(buf, 52, yaw); - _mav_put_float(buf, 56, local_z); - _mav_put_float(buf, 60, lat); - _mav_put_float(buf, 64, lon); - _mav_put_float(buf, 68, alt); - _mav_put_float(buf, 72, ground_x); - _mav_put_float(buf, 76, ground_y); - _mav_put_float(buf, 80, ground_z); - _mav_put_uint16_t(buf, 84, width); - _mav_put_uint16_t(buf, 86, height); - _mav_put_uint16_t(buf, 88, depth); - _mav_put_uint8_t(buf, 90, cam_no); - _mav_put_uint8_t(buf, 91, channels); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); -#else - mavlink_image_available_t packet; - packet.cam_id = cam_id; - packet.timestamp = timestamp; - packet.valid_until = valid_until; - packet.img_seq = img_seq; - packet.img_buf_index = img_buf_index; - packet.key = key; - packet.exposure = exposure; - packet.gain = gain; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.local_z = local_z; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.ground_x = ground_x; - packet.ground_y = ground_y; - packet.ground_z = ground_z; - packet.width = width; - packet.height = height; - packet.depth = depth; - packet.cam_no = cam_no; - packet.channels = channels; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN, MAVLINK_MSG_ID_IMAGE_AVAILABLE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); -#endif -} - -/** - * @brief Pack a image_available message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param cam_id Camera id - * @param cam_no Camera # (starts with 0) - * @param timestamp Timestamp - * @param valid_until Until which timestamp this buffer will stay valid - * @param img_seq The image sequence number - * @param img_buf_index Position of the image in the buffer, starts with 0 - * @param width Image width - * @param height Image height - * @param depth Image depth - * @param channels Image channels - * @param key Shared memory area key - * @param exposure Exposure time, in microseconds - * @param gain Camera gain - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @param local_z Local frame Z coordinate (height over ground) - * @param lat GPS X coordinate - * @param lon GPS Y coordinate - * @param alt Global frame altitude - * @param ground_x Ground truth X - * @param ground_y Ground truth Y - * @param ground_z Ground truth Z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_image_available_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t cam_id,uint8_t cam_no,uint64_t timestamp,uint64_t valid_until,uint32_t img_seq,uint32_t img_buf_index,uint16_t width,uint16_t height,uint16_t depth,uint8_t channels,uint32_t key,uint32_t exposure,float gain,float roll,float pitch,float yaw,float local_z,float lat,float lon,float alt,float ground_x,float ground_y,float ground_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN]; - _mav_put_uint64_t(buf, 0, cam_id); - _mav_put_uint64_t(buf, 8, timestamp); - _mav_put_uint64_t(buf, 16, valid_until); - _mav_put_uint32_t(buf, 24, img_seq); - _mav_put_uint32_t(buf, 28, img_buf_index); - _mav_put_uint32_t(buf, 32, key); - _mav_put_uint32_t(buf, 36, exposure); - _mav_put_float(buf, 40, gain); - _mav_put_float(buf, 44, roll); - _mav_put_float(buf, 48, pitch); - _mav_put_float(buf, 52, yaw); - _mav_put_float(buf, 56, local_z); - _mav_put_float(buf, 60, lat); - _mav_put_float(buf, 64, lon); - _mav_put_float(buf, 68, alt); - _mav_put_float(buf, 72, ground_x); - _mav_put_float(buf, 76, ground_y); - _mav_put_float(buf, 80, ground_z); - _mav_put_uint16_t(buf, 84, width); - _mav_put_uint16_t(buf, 86, height); - _mav_put_uint16_t(buf, 88, depth); - _mav_put_uint8_t(buf, 90, cam_no); - _mav_put_uint8_t(buf, 91, channels); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); -#else - mavlink_image_available_t packet; - packet.cam_id = cam_id; - packet.timestamp = timestamp; - packet.valid_until = valid_until; - packet.img_seq = img_seq; - packet.img_buf_index = img_buf_index; - packet.key = key; - packet.exposure = exposure; - packet.gain = gain; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.local_z = local_z; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.ground_x = ground_x; - packet.ground_y = ground_y; - packet.ground_z = ground_z; - packet.width = width; - packet.height = height; - packet.depth = depth; - packet.cam_no = cam_no; - packet.channels = channels; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN, MAVLINK_MSG_ID_IMAGE_AVAILABLE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); -#endif -} - -/** - * @brief Encode a image_available struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param image_available C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_image_available_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_image_available_t* image_available) -{ - return mavlink_msg_image_available_pack(system_id, component_id, msg, image_available->cam_id, image_available->cam_no, image_available->timestamp, image_available->valid_until, image_available->img_seq, image_available->img_buf_index, image_available->width, image_available->height, image_available->depth, image_available->channels, image_available->key, image_available->exposure, image_available->gain, image_available->roll, image_available->pitch, image_available->yaw, image_available->local_z, image_available->lat, image_available->lon, image_available->alt, image_available->ground_x, image_available->ground_y, image_available->ground_z); -} - -/** - * @brief Encode a image_available struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param image_available C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_image_available_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_image_available_t* image_available) -{ - return mavlink_msg_image_available_pack_chan(system_id, component_id, chan, msg, image_available->cam_id, image_available->cam_no, image_available->timestamp, image_available->valid_until, image_available->img_seq, image_available->img_buf_index, image_available->width, image_available->height, image_available->depth, image_available->channels, image_available->key, image_available->exposure, image_available->gain, image_available->roll, image_available->pitch, image_available->yaw, image_available->local_z, image_available->lat, image_available->lon, image_available->alt, image_available->ground_x, image_available->ground_y, image_available->ground_z); -} - -/** - * @brief Send a image_available message - * @param chan MAVLink channel to send the message - * - * @param cam_id Camera id - * @param cam_no Camera # (starts with 0) - * @param timestamp Timestamp - * @param valid_until Until which timestamp this buffer will stay valid - * @param img_seq The image sequence number - * @param img_buf_index Position of the image in the buffer, starts with 0 - * @param width Image width - * @param height Image height - * @param depth Image depth - * @param channels Image channels - * @param key Shared memory area key - * @param exposure Exposure time, in microseconds - * @param gain Camera gain - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @param local_z Local frame Z coordinate (height over ground) - * @param lat GPS X coordinate - * @param lon GPS Y coordinate - * @param alt Global frame altitude - * @param ground_x Ground truth X - * @param ground_y Ground truth Y - * @param ground_z Ground truth Z - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN]; - _mav_put_uint64_t(buf, 0, cam_id); - _mav_put_uint64_t(buf, 8, timestamp); - _mav_put_uint64_t(buf, 16, valid_until); - _mav_put_uint32_t(buf, 24, img_seq); - _mav_put_uint32_t(buf, 28, img_buf_index); - _mav_put_uint32_t(buf, 32, key); - _mav_put_uint32_t(buf, 36, exposure); - _mav_put_float(buf, 40, gain); - _mav_put_float(buf, 44, roll); - _mav_put_float(buf, 48, pitch); - _mav_put_float(buf, 52, yaw); - _mav_put_float(buf, 56, local_z); - _mav_put_float(buf, 60, lat); - _mav_put_float(buf, 64, lon); - _mav_put_float(buf, 68, alt); - _mav_put_float(buf, 72, ground_x); - _mav_put_float(buf, 76, ground_y); - _mav_put_float(buf, 80, ground_z); - _mav_put_uint16_t(buf, 84, width); - _mav_put_uint16_t(buf, 86, height); - _mav_put_uint16_t(buf, 88, depth); - _mav_put_uint8_t(buf, 90, cam_no); - _mav_put_uint8_t(buf, 91, channels); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, buf, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN, MAVLINK_MSG_ID_IMAGE_AVAILABLE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, buf, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); -#endif -#else - mavlink_image_available_t packet; - packet.cam_id = cam_id; - packet.timestamp = timestamp; - packet.valid_until = valid_until; - packet.img_seq = img_seq; - packet.img_buf_index = img_buf_index; - packet.key = key; - packet.exposure = exposure; - packet.gain = gain; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.local_z = local_z; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.ground_x = ground_x; - packet.ground_y = ground_y; - packet.ground_z = ground_z; - packet.width = width; - packet.height = height; - packet.depth = depth; - packet.cam_no = cam_no; - packet.channels = channels; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, (const char *)&packet, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN, MAVLINK_MSG_ID_IMAGE_AVAILABLE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, (const char *)&packet, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); -#endif -#endif -} - -#endif - -// MESSAGE IMAGE_AVAILABLE UNPACKING - - -/** - * @brief Get field cam_id from image_available message - * - * @return Camera id - */ -static inline uint64_t mavlink_msg_image_available_get_cam_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field cam_no from image_available message - * - * @return Camera # (starts with 0) - */ -static inline uint8_t mavlink_msg_image_available_get_cam_no(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 90); -} - -/** - * @brief Get field timestamp from image_available message - * - * @return Timestamp - */ -static inline uint64_t mavlink_msg_image_available_get_timestamp(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 8); -} - -/** - * @brief Get field valid_until from image_available message - * - * @return Until which timestamp this buffer will stay valid - */ -static inline uint64_t mavlink_msg_image_available_get_valid_until(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 16); -} - -/** - * @brief Get field img_seq from image_available message - * - * @return The image sequence number - */ -static inline uint32_t mavlink_msg_image_available_get_img_seq(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 24); -} - -/** - * @brief Get field img_buf_index from image_available message - * - * @return Position of the image in the buffer, starts with 0 - */ -static inline uint32_t mavlink_msg_image_available_get_img_buf_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 28); -} - -/** - * @brief Get field width from image_available message - * - * @return Image width - */ -static inline uint16_t mavlink_msg_image_available_get_width(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 84); -} - -/** - * @brief Get field height from image_available message - * - * @return Image height - */ -static inline uint16_t mavlink_msg_image_available_get_height(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 86); -} - -/** - * @brief Get field depth from image_available message - * - * @return Image depth - */ -static inline uint16_t mavlink_msg_image_available_get_depth(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 88); -} - -/** - * @brief Get field channels from image_available message - * - * @return Image channels - */ -static inline uint8_t mavlink_msg_image_available_get_channels(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 91); -} - -/** - * @brief Get field key from image_available message - * - * @return Shared memory area key - */ -static inline uint32_t mavlink_msg_image_available_get_key(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 32); -} - -/** - * @brief Get field exposure from image_available message - * - * @return Exposure time, in microseconds - */ -static inline uint32_t mavlink_msg_image_available_get_exposure(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 36); -} - -/** - * @brief Get field gain from image_available message - * - * @return Camera gain - */ -static inline float mavlink_msg_image_available_get_gain(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 40); -} - -/** - * @brief Get field roll from image_available message - * - * @return Roll angle in rad - */ -static inline float mavlink_msg_image_available_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 44); -} - -/** - * @brief Get field pitch from image_available message - * - * @return Pitch angle in rad - */ -static inline float mavlink_msg_image_available_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 48); -} - -/** - * @brief Get field yaw from image_available message - * - * @return Yaw angle in rad - */ -static inline float mavlink_msg_image_available_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 52); -} - -/** - * @brief Get field local_z from image_available message - * - * @return Local frame Z coordinate (height over ground) - */ -static inline float mavlink_msg_image_available_get_local_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 56); -} - -/** - * @brief Get field lat from image_available message - * - * @return GPS X coordinate - */ -static inline float mavlink_msg_image_available_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 60); -} - -/** - * @brief Get field lon from image_available message - * - * @return GPS Y coordinate - */ -static inline float mavlink_msg_image_available_get_lon(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 64); -} - -/** - * @brief Get field alt from image_available message - * - * @return Global frame altitude - */ -static inline float mavlink_msg_image_available_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 68); -} - -/** - * @brief Get field ground_x from image_available message - * - * @return Ground truth X - */ -static inline float mavlink_msg_image_available_get_ground_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 72); -} - -/** - * @brief Get field ground_y from image_available message - * - * @return Ground truth Y - */ -static inline float mavlink_msg_image_available_get_ground_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 76); -} - -/** - * @brief Get field ground_z from image_available message - * - * @return Ground truth Z - */ -static inline float mavlink_msg_image_available_get_ground_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 80); -} - -/** - * @brief Decode a image_available message into a struct - * - * @param msg The message to decode - * @param image_available C-struct to decode the message contents into - */ -static inline void mavlink_msg_image_available_decode(const mavlink_message_t* msg, mavlink_image_available_t* image_available) -{ -#if MAVLINK_NEED_BYTE_SWAP - image_available->cam_id = mavlink_msg_image_available_get_cam_id(msg); - image_available->timestamp = mavlink_msg_image_available_get_timestamp(msg); - image_available->valid_until = mavlink_msg_image_available_get_valid_until(msg); - image_available->img_seq = mavlink_msg_image_available_get_img_seq(msg); - image_available->img_buf_index = mavlink_msg_image_available_get_img_buf_index(msg); - image_available->key = mavlink_msg_image_available_get_key(msg); - image_available->exposure = mavlink_msg_image_available_get_exposure(msg); - image_available->gain = mavlink_msg_image_available_get_gain(msg); - image_available->roll = mavlink_msg_image_available_get_roll(msg); - image_available->pitch = mavlink_msg_image_available_get_pitch(msg); - image_available->yaw = mavlink_msg_image_available_get_yaw(msg); - image_available->local_z = mavlink_msg_image_available_get_local_z(msg); - image_available->lat = mavlink_msg_image_available_get_lat(msg); - image_available->lon = mavlink_msg_image_available_get_lon(msg); - image_available->alt = mavlink_msg_image_available_get_alt(msg); - image_available->ground_x = mavlink_msg_image_available_get_ground_x(msg); - image_available->ground_y = mavlink_msg_image_available_get_ground_y(msg); - image_available->ground_z = mavlink_msg_image_available_get_ground_z(msg); - image_available->width = mavlink_msg_image_available_get_width(msg); - image_available->height = mavlink_msg_image_available_get_height(msg); - image_available->depth = mavlink_msg_image_available_get_depth(msg); - image_available->cam_no = mavlink_msg_image_available_get_cam_no(msg); - image_available->channels = mavlink_msg_image_available_get_channels(msg); -#else - memcpy(image_available, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h deleted file mode 100644 index 664574b..0000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h +++ /dev/null @@ -1,177 +0,0 @@ -// MESSAGE IMAGE_TRIGGER_CONTROL PACKING - -#define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL 153 - -typedef struct __mavlink_image_trigger_control_t -{ - uint8_t enable; ///< 0 to disable, 1 to enable -} mavlink_image_trigger_control_t; - -#define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN 1 -#define MAVLINK_MSG_ID_153_LEN 1 - -#define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_CRC 95 -#define MAVLINK_MSG_ID_153_CRC 95 - - - -#define MAVLINK_MESSAGE_INFO_IMAGE_TRIGGER_CONTROL { \ - "IMAGE_TRIGGER_CONTROL", \ - 1, \ - { { "enable", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_image_trigger_control_t, enable) }, \ - } \ -} - - -/** - * @brief Pack a image_trigger_control message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param enable 0 to disable, 1 to enable - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_image_trigger_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t enable) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN]; - _mav_put_uint8_t(buf, 0, enable); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); -#else - mavlink_image_trigger_control_t packet; - packet.enable = enable; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); -#endif -} - -/** - * @brief Pack a image_trigger_control message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param enable 0 to disable, 1 to enable - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_image_trigger_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t enable) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN]; - _mav_put_uint8_t(buf, 0, enable); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); -#else - mavlink_image_trigger_control_t packet; - packet.enable = enable; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); -#endif -} - -/** - * @brief Encode a image_trigger_control struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param image_trigger_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_image_trigger_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_image_trigger_control_t* image_trigger_control) -{ - return mavlink_msg_image_trigger_control_pack(system_id, component_id, msg, image_trigger_control->enable); -} - -/** - * @brief Encode a image_trigger_control struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param image_trigger_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_image_trigger_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_image_trigger_control_t* image_trigger_control) -{ - return mavlink_msg_image_trigger_control_pack_chan(system_id, component_id, chan, msg, image_trigger_control->enable); -} - -/** - * @brief Send a image_trigger_control message - * @param chan MAVLink channel to send the message - * - * @param enable 0 to disable, 1 to enable - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan, uint8_t enable) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN]; - _mav_put_uint8_t(buf, 0, enable); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, buf, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, buf, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); -#endif -#else - mavlink_image_trigger_control_t packet; - packet.enable = enable; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); -#endif -#endif -} - -#endif - -// MESSAGE IMAGE_TRIGGER_CONTROL UNPACKING - - -/** - * @brief Get field enable from image_trigger_control message - * - * @return 0 to disable, 1 to enable - */ -static inline uint8_t mavlink_msg_image_trigger_control_get_enable(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Decode a image_trigger_control message into a struct - * - * @param msg The message to decode - * @param image_trigger_control C-struct to decode the message contents into - */ -static inline void mavlink_msg_image_trigger_control_decode(const mavlink_message_t* msg, mavlink_image_trigger_control_t* image_trigger_control) -{ -#if MAVLINK_NEED_BYTE_SWAP - image_trigger_control->enable = mavlink_msg_image_trigger_control_get_enable(msg); -#else - memcpy(image_trigger_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h deleted file mode 100644 index f3a2243..0000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h +++ /dev/null @@ -1,419 +0,0 @@ -// MESSAGE IMAGE_TRIGGERED PACKING - -#define MAVLINK_MSG_ID_IMAGE_TRIGGERED 152 - -typedef struct __mavlink_image_triggered_t -{ - uint64_t timestamp; ///< Timestamp - uint32_t seq; ///< IMU seq - float roll; ///< Roll angle in rad - float pitch; ///< Pitch angle in rad - float yaw; ///< Yaw angle in rad - float local_z; ///< Local frame Z coordinate (height over ground) - float lat; ///< GPS X coordinate - float lon; ///< GPS Y coordinate - float alt; ///< Global frame altitude - float ground_x; ///< Ground truth X - float ground_y; ///< Ground truth Y - float ground_z; ///< Ground truth Z -} mavlink_image_triggered_t; - -#define MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN 52 -#define MAVLINK_MSG_ID_152_LEN 52 - -#define MAVLINK_MSG_ID_IMAGE_TRIGGERED_CRC 86 -#define MAVLINK_MSG_ID_152_CRC 86 - - - -#define MAVLINK_MESSAGE_INFO_IMAGE_TRIGGERED { \ - "IMAGE_TRIGGERED", \ - 12, \ - { { "timestamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_image_triggered_t, timestamp) }, \ - { "seq", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_image_triggered_t, seq) }, \ - { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_image_triggered_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_image_triggered_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_image_triggered_t, yaw) }, \ - { "local_z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_image_triggered_t, local_z) }, \ - { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_image_triggered_t, lat) }, \ - { "lon", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_image_triggered_t, lon) }, \ - { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_image_triggered_t, alt) }, \ - { "ground_x", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_image_triggered_t, ground_x) }, \ - { "ground_y", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_image_triggered_t, ground_y) }, \ - { "ground_z", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_image_triggered_t, ground_z) }, \ - } \ -} - - -/** - * @brief Pack a image_triggered message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param timestamp Timestamp - * @param seq IMU seq - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @param local_z Local frame Z coordinate (height over ground) - * @param lat GPS X coordinate - * @param lon GPS Y coordinate - * @param alt Global frame altitude - * @param ground_x Ground truth X - * @param ground_y Ground truth Y - * @param ground_z Ground truth Z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN]; - _mav_put_uint64_t(buf, 0, timestamp); - _mav_put_uint32_t(buf, 8, seq); - _mav_put_float(buf, 12, roll); - _mav_put_float(buf, 16, pitch); - _mav_put_float(buf, 20, yaw); - _mav_put_float(buf, 24, local_z); - _mav_put_float(buf, 28, lat); - _mav_put_float(buf, 32, lon); - _mav_put_float(buf, 36, alt); - _mav_put_float(buf, 40, ground_x); - _mav_put_float(buf, 44, ground_y); - _mav_put_float(buf, 48, ground_z); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); -#else - mavlink_image_triggered_t packet; - packet.timestamp = timestamp; - packet.seq = seq; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.local_z = local_z; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.ground_x = ground_x; - packet.ground_y = ground_y; - packet.ground_z = ground_z; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGERED_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); -#endif -} - -/** - * @brief Pack a image_triggered message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param timestamp Timestamp - * @param seq IMU seq - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @param local_z Local frame Z coordinate (height over ground) - * @param lat GPS X coordinate - * @param lon GPS Y coordinate - * @param alt Global frame altitude - * @param ground_x Ground truth X - * @param ground_y Ground truth Y - * @param ground_z Ground truth Z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_image_triggered_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t timestamp,uint32_t seq,float roll,float pitch,float yaw,float local_z,float lat,float lon,float alt,float ground_x,float ground_y,float ground_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN]; - _mav_put_uint64_t(buf, 0, timestamp); - _mav_put_uint32_t(buf, 8, seq); - _mav_put_float(buf, 12, roll); - _mav_put_float(buf, 16, pitch); - _mav_put_float(buf, 20, yaw); - _mav_put_float(buf, 24, local_z); - _mav_put_float(buf, 28, lat); - _mav_put_float(buf, 32, lon); - _mav_put_float(buf, 36, alt); - _mav_put_float(buf, 40, ground_x); - _mav_put_float(buf, 44, ground_y); - _mav_put_float(buf, 48, ground_z); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); -#else - mavlink_image_triggered_t packet; - packet.timestamp = timestamp; - packet.seq = seq; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.local_z = local_z; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.ground_x = ground_x; - packet.ground_y = ground_y; - packet.ground_z = ground_z; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGERED_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); -#endif -} - -/** - * @brief Encode a image_triggered struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param image_triggered C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_image_triggered_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_image_triggered_t* image_triggered) -{ - return mavlink_msg_image_triggered_pack(system_id, component_id, msg, image_triggered->timestamp, image_triggered->seq, image_triggered->roll, image_triggered->pitch, image_triggered->yaw, image_triggered->local_z, image_triggered->lat, image_triggered->lon, image_triggered->alt, image_triggered->ground_x, image_triggered->ground_y, image_triggered->ground_z); -} - -/** - * @brief Encode a image_triggered struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param image_triggered C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_image_triggered_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_image_triggered_t* image_triggered) -{ - return mavlink_msg_image_triggered_pack_chan(system_id, component_id, chan, msg, image_triggered->timestamp, image_triggered->seq, image_triggered->roll, image_triggered->pitch, image_triggered->yaw, image_triggered->local_z, image_triggered->lat, image_triggered->lon, image_triggered->alt, image_triggered->ground_x, image_triggered->ground_y, image_triggered->ground_z); -} - -/** - * @brief Send a image_triggered message - * @param chan MAVLink channel to send the message - * - * @param timestamp Timestamp - * @param seq IMU seq - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @param local_z Local frame Z coordinate (height over ground) - * @param lat GPS X coordinate - * @param lon GPS Y coordinate - * @param alt Global frame altitude - * @param ground_x Ground truth X - * @param ground_y Ground truth Y - * @param ground_z Ground truth Z - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_image_triggered_send(mavlink_channel_t chan, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN]; - _mav_put_uint64_t(buf, 0, timestamp); - _mav_put_uint32_t(buf, 8, seq); - _mav_put_float(buf, 12, roll); - _mav_put_float(buf, 16, pitch); - _mav_put_float(buf, 20, yaw); - _mav_put_float(buf, 24, local_z); - _mav_put_float(buf, 28, lat); - _mav_put_float(buf, 32, lon); - _mav_put_float(buf, 36, alt); - _mav_put_float(buf, 40, ground_x); - _mav_put_float(buf, 44, ground_y); - _mav_put_float(buf, 48, ground_z); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, buf, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGERED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, buf, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); -#endif -#else - mavlink_image_triggered_t packet; - packet.timestamp = timestamp; - packet.seq = seq; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.local_z = local_z; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.ground_x = ground_x; - packet.ground_y = ground_y; - packet.ground_z = ground_z; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, (const char *)&packet, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGERED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, (const char *)&packet, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); -#endif -#endif -} - -#endif - -// MESSAGE IMAGE_TRIGGERED UNPACKING - - -/** - * @brief Get field timestamp from image_triggered message - * - * @return Timestamp - */ -static inline uint64_t mavlink_msg_image_triggered_get_timestamp(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field seq from image_triggered message - * - * @return IMU seq - */ -static inline uint32_t mavlink_msg_image_triggered_get_seq(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 8); -} - -/** - * @brief Get field roll from image_triggered message - * - * @return Roll angle in rad - */ -static inline float mavlink_msg_image_triggered_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field pitch from image_triggered message - * - * @return Pitch angle in rad - */ -static inline float mavlink_msg_image_triggered_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field yaw from image_triggered message - * - * @return Yaw angle in rad - */ -static inline float mavlink_msg_image_triggered_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field local_z from image_triggered message - * - * @return Local frame Z coordinate (height over ground) - */ -static inline float mavlink_msg_image_triggered_get_local_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field lat from image_triggered message - * - * @return GPS X coordinate - */ -static inline float mavlink_msg_image_triggered_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Get field lon from image_triggered message - * - * @return GPS Y coordinate - */ -static inline float mavlink_msg_image_triggered_get_lon(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 32); -} - -/** - * @brief Get field alt from image_triggered message - * - * @return Global frame altitude - */ -static inline float mavlink_msg_image_triggered_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 36); -} - -/** - * @brief Get field ground_x from image_triggered message - * - * @return Ground truth X - */ -static inline float mavlink_msg_image_triggered_get_ground_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 40); -} - -/** - * @brief Get field ground_y from image_triggered message - * - * @return Ground truth Y - */ -static inline float mavlink_msg_image_triggered_get_ground_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 44); -} - -/** - * @brief Get field ground_z from image_triggered message - * - * @return Ground truth Z - */ -static inline float mavlink_msg_image_triggered_get_ground_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 48); -} - -/** - * @brief Decode a image_triggered message into a struct - * - * @param msg The message to decode - * @param image_triggered C-struct to decode the message contents into - */ -static inline void mavlink_msg_image_triggered_decode(const mavlink_message_t* msg, mavlink_image_triggered_t* image_triggered) -{ -#if MAVLINK_NEED_BYTE_SWAP - image_triggered->timestamp = mavlink_msg_image_triggered_get_timestamp(msg); - image_triggered->seq = mavlink_msg_image_triggered_get_seq(msg); - image_triggered->roll = mavlink_msg_image_triggered_get_roll(msg); - image_triggered->pitch = mavlink_msg_image_triggered_get_pitch(msg); - image_triggered->yaw = mavlink_msg_image_triggered_get_yaw(msg); - image_triggered->local_z = mavlink_msg_image_triggered_get_local_z(msg); - image_triggered->lat = mavlink_msg_image_triggered_get_lat(msg); - image_triggered->lon = mavlink_msg_image_triggered_get_lon(msg); - image_triggered->alt = mavlink_msg_image_triggered_get_alt(msg); - image_triggered->ground_x = mavlink_msg_image_triggered_get_ground_x(msg); - image_triggered->ground_y = mavlink_msg_image_triggered_get_ground_y(msg); - image_triggered->ground_z = mavlink_msg_image_triggered_get_ground_z(msg); -#else - memcpy(image_triggered, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h deleted file mode 100644 index 6bdccea..0000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h +++ /dev/null @@ -1,309 +0,0 @@ -// MESSAGE MARKER PACKING - -#define MAVLINK_MSG_ID_MARKER 171 - -typedef struct __mavlink_marker_t -{ - float x; ///< x position - float y; ///< y position - float z; ///< z position - float roll; ///< roll orientation - float pitch; ///< pitch orientation - float yaw; ///< yaw orientation - uint16_t id; ///< ID -} mavlink_marker_t; - -#define MAVLINK_MSG_ID_MARKER_LEN 26 -#define MAVLINK_MSG_ID_171_LEN 26 - -#define MAVLINK_MSG_ID_MARKER_CRC 249 -#define MAVLINK_MSG_ID_171_CRC 249 - - - -#define MAVLINK_MESSAGE_INFO_MARKER { \ - "MARKER", \ - 7, \ - { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_marker_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_marker_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_marker_t, z) }, \ - { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_marker_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_marker_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_marker_t, yaw) }, \ - { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_marker_t, id) }, \ - } \ -} - - -/** - * @brief Pack a marker message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param id ID - * @param x x position - * @param y y position - * @param z z position - * @param roll roll orientation - * @param pitch pitch orientation - * @param yaw yaw orientation - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_marker_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t id, float x, float y, float z, float roll, float pitch, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MARKER_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, roll); - _mav_put_float(buf, 16, pitch); - _mav_put_float(buf, 20, yaw); - _mav_put_uint16_t(buf, 24, id); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MARKER_LEN); -#else - mavlink_marker_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.id = id; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MARKER_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MARKER; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MARKER_LEN, MAVLINK_MSG_ID_MARKER_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MARKER_LEN); -#endif -} - -/** - * @brief Pack a marker message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param id ID - * @param x x position - * @param y y position - * @param z z position - * @param roll roll orientation - * @param pitch pitch orientation - * @param yaw yaw orientation - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_marker_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t id,float x,float y,float z,float roll,float pitch,float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MARKER_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, roll); - _mav_put_float(buf, 16, pitch); - _mav_put_float(buf, 20, yaw); - _mav_put_uint16_t(buf, 24, id); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MARKER_LEN); -#else - mavlink_marker_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.id = id; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MARKER_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MARKER; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MARKER_LEN, MAVLINK_MSG_ID_MARKER_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MARKER_LEN); -#endif -} - -/** - * @brief Encode a marker struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param marker C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_marker_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_marker_t* marker) -{ - return mavlink_msg_marker_pack(system_id, component_id, msg, marker->id, marker->x, marker->y, marker->z, marker->roll, marker->pitch, marker->yaw); -} - -/** - * @brief Encode a marker struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param marker C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_marker_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_marker_t* marker) -{ - return mavlink_msg_marker_pack_chan(system_id, component_id, chan, msg, marker->id, marker->x, marker->y, marker->z, marker->roll, marker->pitch, marker->yaw); -} - -/** - * @brief Send a marker message - * @param chan MAVLink channel to send the message - * - * @param id ID - * @param x x position - * @param y y position - * @param z z position - * @param roll roll orientation - * @param pitch pitch orientation - * @param yaw yaw orientation - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_marker_send(mavlink_channel_t chan, uint16_t id, float x, float y, float z, float roll, float pitch, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MARKER_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, roll); - _mav_put_float(buf, 16, pitch); - _mav_put_float(buf, 20, yaw); - _mav_put_uint16_t(buf, 24, id); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, buf, MAVLINK_MSG_ID_MARKER_LEN, MAVLINK_MSG_ID_MARKER_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, buf, MAVLINK_MSG_ID_MARKER_LEN); -#endif -#else - mavlink_marker_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.id = id; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, (const char *)&packet, MAVLINK_MSG_ID_MARKER_LEN, MAVLINK_MSG_ID_MARKER_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, (const char *)&packet, MAVLINK_MSG_ID_MARKER_LEN); -#endif -#endif -} - -#endif - -// MESSAGE MARKER UNPACKING - - -/** - * @brief Get field id from marker message - * - * @return ID - */ -static inline uint16_t mavlink_msg_marker_get_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 24); -} - -/** - * @brief Get field x from marker message - * - * @return x position - */ -static inline float mavlink_msg_marker_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field y from marker message - * - * @return y position - */ -static inline float mavlink_msg_marker_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field z from marker message - * - * @return z position - */ -static inline float mavlink_msg_marker_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field roll from marker message - * - * @return roll orientation - */ -static inline float mavlink_msg_marker_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field pitch from marker message - * - * @return pitch orientation - */ -static inline float mavlink_msg_marker_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field yaw from marker message - * - * @return yaw orientation - */ -static inline float mavlink_msg_marker_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Decode a marker message into a struct - * - * @param msg The message to decode - * @param marker C-struct to decode the message contents into - */ -static inline void mavlink_msg_marker_decode(const mavlink_message_t* msg, mavlink_marker_t* marker) -{ -#if MAVLINK_NEED_BYTE_SWAP - marker->x = mavlink_msg_marker_get_x(msg); - marker->y = mavlink_msg_marker_get_y(msg); - marker->z = mavlink_msg_marker_get_z(msg); - marker->roll = mavlink_msg_marker_get_roll(msg); - marker->pitch = mavlink_msg_marker_get_pitch(msg); - marker->yaw = mavlink_msg_marker_get_yaw(msg); - marker->id = mavlink_msg_marker_get_id(msg); -#else - memcpy(marker, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MARKER_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h deleted file mode 100644 index aa7b827..0000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h +++ /dev/null @@ -1,237 +0,0 @@ -// MESSAGE PATTERN_DETECTED PACKING - -#define MAVLINK_MSG_ID_PATTERN_DETECTED 190 - -typedef struct __mavlink_pattern_detected_t -{ - float confidence; ///< Confidence of detection - uint8_t type; ///< 0: Pattern, 1: Letter - char file[100]; ///< Pattern file name - uint8_t detected; ///< Accepted as true detection, 0 no, 1 yes -} mavlink_pattern_detected_t; - -#define MAVLINK_MSG_ID_PATTERN_DETECTED_LEN 106 -#define MAVLINK_MSG_ID_190_LEN 106 - -#define MAVLINK_MSG_ID_PATTERN_DETECTED_CRC 90 -#define MAVLINK_MSG_ID_190_CRC 90 - -#define MAVLINK_MSG_PATTERN_DETECTED_FIELD_FILE_LEN 100 - -#define MAVLINK_MESSAGE_INFO_PATTERN_DETECTED { \ - "PATTERN_DETECTED", \ - 4, \ - { { "confidence", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_pattern_detected_t, confidence) }, \ - { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_pattern_detected_t, type) }, \ - { "file", NULL, MAVLINK_TYPE_CHAR, 100, 5, offsetof(mavlink_pattern_detected_t, file) }, \ - { "detected", NULL, MAVLINK_TYPE_UINT8_T, 0, 105, offsetof(mavlink_pattern_detected_t, detected) }, \ - } \ -} - - -/** - * @brief Pack a pattern_detected message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param type 0: Pattern, 1: Letter - * @param confidence Confidence of detection - * @param file Pattern file name - * @param detected Accepted as true detection, 0 no, 1 yes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t type, float confidence, const char *file, uint8_t detected) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PATTERN_DETECTED_LEN]; - _mav_put_float(buf, 0, confidence); - _mav_put_uint8_t(buf, 4, type); - _mav_put_uint8_t(buf, 105, detected); - _mav_put_char_array(buf, 5, file, 100); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); -#else - mavlink_pattern_detected_t packet; - packet.confidence = confidence; - packet.type = type; - packet.detected = detected; - mav_array_memcpy(packet.file, file, sizeof(char)*100); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN, MAVLINK_MSG_ID_PATTERN_DETECTED_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); -#endif -} - -/** - * @brief Pack a pattern_detected message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param type 0: Pattern, 1: Letter - * @param confidence Confidence of detection - * @param file Pattern file name - * @param detected Accepted as true detection, 0 no, 1 yes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_pattern_detected_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t type,float confidence,const char *file,uint8_t detected) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PATTERN_DETECTED_LEN]; - _mav_put_float(buf, 0, confidence); - _mav_put_uint8_t(buf, 4, type); - _mav_put_uint8_t(buf, 105, detected); - _mav_put_char_array(buf, 5, file, 100); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); -#else - mavlink_pattern_detected_t packet; - packet.confidence = confidence; - packet.type = type; - packet.detected = detected; - mav_array_memcpy(packet.file, file, sizeof(char)*100); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN, MAVLINK_MSG_ID_PATTERN_DETECTED_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); -#endif -} - -/** - * @brief Encode a pattern_detected struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param pattern_detected C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_pattern_detected_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_pattern_detected_t* pattern_detected) -{ - return mavlink_msg_pattern_detected_pack(system_id, component_id, msg, pattern_detected->type, pattern_detected->confidence, pattern_detected->file, pattern_detected->detected); -} - -/** - * @brief Encode a pattern_detected struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param pattern_detected C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_pattern_detected_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_pattern_detected_t* pattern_detected) -{ - return mavlink_msg_pattern_detected_pack_chan(system_id, component_id, chan, msg, pattern_detected->type, pattern_detected->confidence, pattern_detected->file, pattern_detected->detected); -} - -/** - * @brief Send a pattern_detected message - * @param chan MAVLink channel to send the message - * - * @param type 0: Pattern, 1: Letter - * @param confidence Confidence of detection - * @param file Pattern file name - * @param detected Accepted as true detection, 0 no, 1 yes - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uint8_t type, float confidence, const char *file, uint8_t detected) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PATTERN_DETECTED_LEN]; - _mav_put_float(buf, 0, confidence); - _mav_put_uint8_t(buf, 4, type); - _mav_put_uint8_t(buf, 105, detected); - _mav_put_char_array(buf, 5, file, 100); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, buf, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN, MAVLINK_MSG_ID_PATTERN_DETECTED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, buf, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); -#endif -#else - mavlink_pattern_detected_t packet; - packet.confidence = confidence; - packet.type = type; - packet.detected = detected; - mav_array_memcpy(packet.file, file, sizeof(char)*100); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, (const char *)&packet, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN, MAVLINK_MSG_ID_PATTERN_DETECTED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, (const char *)&packet, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); -#endif -#endif -} - -#endif - -// MESSAGE PATTERN_DETECTED UNPACKING - - -/** - * @brief Get field type from pattern_detected message - * - * @return 0: Pattern, 1: Letter - */ -static inline uint8_t mavlink_msg_pattern_detected_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field confidence from pattern_detected message - * - * @return Confidence of detection - */ -static inline float mavlink_msg_pattern_detected_get_confidence(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field file from pattern_detected message - * - * @return Pattern file name - */ -static inline uint16_t mavlink_msg_pattern_detected_get_file(const mavlink_message_t* msg, char *file) -{ - return _MAV_RETURN_char_array(msg, file, 100, 5); -} - -/** - * @brief Get field detected from pattern_detected message - * - * @return Accepted as true detection, 0 no, 1 yes - */ -static inline uint8_t mavlink_msg_pattern_detected_get_detected(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 105); -} - -/** - * @brief Decode a pattern_detected message into a struct - * - * @param msg The message to decode - * @param pattern_detected C-struct to decode the message contents into - */ -static inline void mavlink_msg_pattern_detected_decode(const mavlink_message_t* msg, mavlink_pattern_detected_t* pattern_detected) -{ -#if MAVLINK_NEED_BYTE_SWAP - pattern_detected->confidence = mavlink_msg_pattern_detected_get_confidence(msg); - pattern_detected->type = mavlink_msg_pattern_detected_get_type(msg); - mavlink_msg_pattern_detected_get_file(msg, pattern_detected->file); - pattern_detected->detected = mavlink_msg_pattern_detected_get_detected(msg); -#else - memcpy(pattern_detected, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h deleted file mode 100644 index 913a528..0000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h +++ /dev/null @@ -1,325 +0,0 @@ -// MESSAGE POINT_OF_INTEREST PACKING - -#define MAVLINK_MSG_ID_POINT_OF_INTEREST 191 - -typedef struct __mavlink_point_of_interest_t -{ - float x; ///< X Position - float y; ///< Y Position - float z; ///< Z Position - uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds - uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - uint8_t coordinate_system; ///< 0: global, 1:local - char name[26]; ///< POI name -} mavlink_point_of_interest_t; - -#define MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN 43 -#define MAVLINK_MSG_ID_191_LEN 43 - -#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CRC 95 -#define MAVLINK_MSG_ID_191_CRC 95 - -#define MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN 26 - -#define MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST { \ - "POINT_OF_INTEREST", \ - 8, \ - { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_point_of_interest_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_point_of_interest_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_point_of_interest_t, z) }, \ - { "timeout", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_point_of_interest_t, timeout) }, \ - { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_point_of_interest_t, type) }, \ - { "color", NULL, MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_point_of_interest_t, color) }, \ - { "coordinate_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_point_of_interest_t, coordinate_system) }, \ - { "name", NULL, MAVLINK_TYPE_CHAR, 26, 17, offsetof(mavlink_point_of_interest_t, name) }, \ - } \ -} - - -/** - * @brief Pack a point_of_interest message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - * @param coordinate_system 0: global, 1:local - * @param timeout 0: no timeout, >1: timeout in seconds - * @param x X Position - * @param y Y Position - * @param z Z Position - * @param name POI name - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char *name) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_uint16_t(buf, 12, timeout); - _mav_put_uint8_t(buf, 14, type); - _mav_put_uint8_t(buf, 15, color); - _mav_put_uint8_t(buf, 16, coordinate_system); - _mav_put_char_array(buf, 17, name, 26); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); -#else - mavlink_point_of_interest_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.timeout = timeout; - packet.type = type; - packet.color = color; - packet.coordinate_system = coordinate_system; - mav_array_memcpy(packet.name, name, sizeof(char)*26); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); -#endif -} - -/** - * @brief Pack a point_of_interest message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - * @param coordinate_system 0: global, 1:local - * @param timeout 0: no timeout, >1: timeout in seconds - * @param x X Position - * @param y Y Position - * @param z Z Position - * @param name POI name - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t type,uint8_t color,uint8_t coordinate_system,uint16_t timeout,float x,float y,float z,const char *name) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_uint16_t(buf, 12, timeout); - _mav_put_uint8_t(buf, 14, type); - _mav_put_uint8_t(buf, 15, color); - _mav_put_uint8_t(buf, 16, coordinate_system); - _mav_put_char_array(buf, 17, name, 26); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); -#else - mavlink_point_of_interest_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.timeout = timeout; - packet.type = type; - packet.color = color; - packet.coordinate_system = coordinate_system; - mav_array_memcpy(packet.name, name, sizeof(char)*26); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); -#endif -} - -/** - * @brief Encode a point_of_interest struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param point_of_interest C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_point_of_interest_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_point_of_interest_t* point_of_interest) -{ - return mavlink_msg_point_of_interest_pack(system_id, component_id, msg, point_of_interest->type, point_of_interest->color, point_of_interest->coordinate_system, point_of_interest->timeout, point_of_interest->x, point_of_interest->y, point_of_interest->z, point_of_interest->name); -} - -/** - * @brief Encode a point_of_interest struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param point_of_interest C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_point_of_interest_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_point_of_interest_t* point_of_interest) -{ - return mavlink_msg_point_of_interest_pack_chan(system_id, component_id, chan, msg, point_of_interest->type, point_of_interest->color, point_of_interest->coordinate_system, point_of_interest->timeout, point_of_interest->x, point_of_interest->y, point_of_interest->z, point_of_interest->name); -} - -/** - * @brief Send a point_of_interest message - * @param chan MAVLink channel to send the message - * - * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - * @param coordinate_system 0: global, 1:local - * @param timeout 0: no timeout, >1: timeout in seconds - * @param x X Position - * @param y Y Position - * @param z Z Position - * @param name POI name - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char *name) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_uint16_t(buf, 12, timeout); - _mav_put_uint8_t(buf, 14, type); - _mav_put_uint8_t(buf, 15, color); - _mav_put_uint8_t(buf, 16, coordinate_system); - _mav_put_char_array(buf, 17, name, 26); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); -#endif -#else - mavlink_point_of_interest_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.timeout = timeout; - packet.type = type; - packet.color = color; - packet.coordinate_system = coordinate_system; - mav_array_memcpy(packet.name, name, sizeof(char)*26); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, (const char *)&packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, (const char *)&packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); -#endif -#endif -} - -#endif - -// MESSAGE POINT_OF_INTEREST UNPACKING - - -/** - * @brief Get field type from point_of_interest message - * - * @return 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - */ -static inline uint8_t mavlink_msg_point_of_interest_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 14); -} - -/** - * @brief Get field color from point_of_interest message - * - * @return 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - */ -static inline uint8_t mavlink_msg_point_of_interest_get_color(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 15); -} - -/** - * @brief Get field coordinate_system from point_of_interest message - * - * @return 0: global, 1:local - */ -static inline uint8_t mavlink_msg_point_of_interest_get_coordinate_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Get field timeout from point_of_interest message - * - * @return 0: no timeout, >1: timeout in seconds - */ -static inline uint16_t mavlink_msg_point_of_interest_get_timeout(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 12); -} - -/** - * @brief Get field x from point_of_interest message - * - * @return X Position - */ -static inline float mavlink_msg_point_of_interest_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field y from point_of_interest message - * - * @return Y Position - */ -static inline float mavlink_msg_point_of_interest_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field z from point_of_interest message - * - * @return Z Position - */ -static inline float mavlink_msg_point_of_interest_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field name from point_of_interest message - * - * @return POI name - */ -static inline uint16_t mavlink_msg_point_of_interest_get_name(const mavlink_message_t* msg, char *name) -{ - return _MAV_RETURN_char_array(msg, name, 26, 17); -} - -/** - * @brief Decode a point_of_interest message into a struct - * - * @param msg The message to decode - * @param point_of_interest C-struct to decode the message contents into - */ -static inline void mavlink_msg_point_of_interest_decode(const mavlink_message_t* msg, mavlink_point_of_interest_t* point_of_interest) -{ -#if MAVLINK_NEED_BYTE_SWAP - point_of_interest->x = mavlink_msg_point_of_interest_get_x(msg); - point_of_interest->y = mavlink_msg_point_of_interest_get_y(msg); - point_of_interest->z = mavlink_msg_point_of_interest_get_z(msg); - point_of_interest->timeout = mavlink_msg_point_of_interest_get_timeout(msg); - point_of_interest->type = mavlink_msg_point_of_interest_get_type(msg); - point_of_interest->color = mavlink_msg_point_of_interest_get_color(msg); - point_of_interest->coordinate_system = mavlink_msg_point_of_interest_get_coordinate_system(msg); - mavlink_msg_point_of_interest_get_name(msg, point_of_interest->name); -#else - memcpy(point_of_interest, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h deleted file mode 100644 index e244364..0000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h +++ /dev/null @@ -1,391 +0,0 @@ -// MESSAGE POINT_OF_INTEREST_CONNECTION PACKING - -#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION 192 - -typedef struct __mavlink_point_of_interest_connection_t -{ - float xp1; ///< X1 Position - float yp1; ///< Y1 Position - float zp1; ///< Z1 Position - float xp2; ///< X2 Position - float yp2; ///< Y2 Position - float zp2; ///< Z2 Position - uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds - uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - uint8_t coordinate_system; ///< 0: global, 1:local - char name[26]; ///< POI connection name -} mavlink_point_of_interest_connection_t; - -#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN 55 -#define MAVLINK_MSG_ID_192_LEN 55 - -#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_CRC 36 -#define MAVLINK_MSG_ID_192_CRC 36 - -#define MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN 26 - -#define MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST_CONNECTION { \ - "POINT_OF_INTEREST_CONNECTION", \ - 11, \ - { { "xp1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_point_of_interest_connection_t, xp1) }, \ - { "yp1", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_point_of_interest_connection_t, yp1) }, \ - { "zp1", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_point_of_interest_connection_t, zp1) }, \ - { "xp2", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_point_of_interest_connection_t, xp2) }, \ - { "yp2", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_point_of_interest_connection_t, yp2) }, \ - { "zp2", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_point_of_interest_connection_t, zp2) }, \ - { "timeout", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_point_of_interest_connection_t, timeout) }, \ - { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_point_of_interest_connection_t, type) }, \ - { "color", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_point_of_interest_connection_t, color) }, \ - { "coordinate_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_point_of_interest_connection_t, coordinate_system) }, \ - { "name", NULL, MAVLINK_TYPE_CHAR, 26, 29, offsetof(mavlink_point_of_interest_connection_t, name) }, \ - } \ -} - - -/** - * @brief Pack a point_of_interest_connection message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - * @param coordinate_system 0: global, 1:local - * @param timeout 0: no timeout, >1: timeout in seconds - * @param xp1 X1 Position - * @param yp1 Y1 Position - * @param zp1 Z1 Position - * @param xp2 X2 Position - * @param yp2 Y2 Position - * @param zp2 Z2 Position - * @param name POI connection name - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char *name) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN]; - _mav_put_float(buf, 0, xp1); - _mav_put_float(buf, 4, yp1); - _mav_put_float(buf, 8, zp1); - _mav_put_float(buf, 12, xp2); - _mav_put_float(buf, 16, yp2); - _mav_put_float(buf, 20, zp2); - _mav_put_uint16_t(buf, 24, timeout); - _mav_put_uint8_t(buf, 26, type); - _mav_put_uint8_t(buf, 27, color); - _mav_put_uint8_t(buf, 28, coordinate_system); - _mav_put_char_array(buf, 29, name, 26); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); -#else - mavlink_point_of_interest_connection_t packet; - packet.xp1 = xp1; - packet.yp1 = yp1; - packet.zp1 = zp1; - packet.xp2 = xp2; - packet.yp2 = yp2; - packet.zp2 = zp2; - packet.timeout = timeout; - packet.type = type; - packet.color = color; - packet.coordinate_system = coordinate_system; - mav_array_memcpy(packet.name, name, sizeof(char)*26); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); -#endif -} - -/** - * @brief Pack a point_of_interest_connection message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - * @param coordinate_system 0: global, 1:local - * @param timeout 0: no timeout, >1: timeout in seconds - * @param xp1 X1 Position - * @param yp1 Y1 Position - * @param zp1 Z1 Position - * @param xp2 X2 Position - * @param yp2 Y2 Position - * @param zp2 Z2 Position - * @param name POI connection name - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t type,uint8_t color,uint8_t coordinate_system,uint16_t timeout,float xp1,float yp1,float zp1,float xp2,float yp2,float zp2,const char *name) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN]; - _mav_put_float(buf, 0, xp1); - _mav_put_float(buf, 4, yp1); - _mav_put_float(buf, 8, zp1); - _mav_put_float(buf, 12, xp2); - _mav_put_float(buf, 16, yp2); - _mav_put_float(buf, 20, zp2); - _mav_put_uint16_t(buf, 24, timeout); - _mav_put_uint8_t(buf, 26, type); - _mav_put_uint8_t(buf, 27, color); - _mav_put_uint8_t(buf, 28, coordinate_system); - _mav_put_char_array(buf, 29, name, 26); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); -#else - mavlink_point_of_interest_connection_t packet; - packet.xp1 = xp1; - packet.yp1 = yp1; - packet.zp1 = zp1; - packet.xp2 = xp2; - packet.yp2 = yp2; - packet.zp2 = zp2; - packet.timeout = timeout; - packet.type = type; - packet.color = color; - packet.coordinate_system = coordinate_system; - mav_array_memcpy(packet.name, name, sizeof(char)*26); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); -#endif -} - -/** - * @brief Encode a point_of_interest_connection struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param point_of_interest_connection C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_point_of_interest_connection_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_point_of_interest_connection_t* point_of_interest_connection) -{ - return mavlink_msg_point_of_interest_connection_pack(system_id, component_id, msg, point_of_interest_connection->type, point_of_interest_connection->color, point_of_interest_connection->coordinate_system, point_of_interest_connection->timeout, point_of_interest_connection->xp1, point_of_interest_connection->yp1, point_of_interest_connection->zp1, point_of_interest_connection->xp2, point_of_interest_connection->yp2, point_of_interest_connection->zp2, point_of_interest_connection->name); -} - -/** - * @brief Encode a point_of_interest_connection struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param point_of_interest_connection C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_point_of_interest_connection_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_point_of_interest_connection_t* point_of_interest_connection) -{ - return mavlink_msg_point_of_interest_connection_pack_chan(system_id, component_id, chan, msg, point_of_interest_connection->type, point_of_interest_connection->color, point_of_interest_connection->coordinate_system, point_of_interest_connection->timeout, point_of_interest_connection->xp1, point_of_interest_connection->yp1, point_of_interest_connection->zp1, point_of_interest_connection->xp2, point_of_interest_connection->yp2, point_of_interest_connection->zp2, point_of_interest_connection->name); -} - -/** - * @brief Send a point_of_interest_connection message - * @param chan MAVLink channel to send the message - * - * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - * @param coordinate_system 0: global, 1:local - * @param timeout 0: no timeout, >1: timeout in seconds - * @param xp1 X1 Position - * @param yp1 Y1 Position - * @param zp1 Z1 Position - * @param xp2 X2 Position - * @param yp2 Y2 Position - * @param zp2 Z2 Position - * @param name POI connection name - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char *name) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN]; - _mav_put_float(buf, 0, xp1); - _mav_put_float(buf, 4, yp1); - _mav_put_float(buf, 8, zp1); - _mav_put_float(buf, 12, xp2); - _mav_put_float(buf, 16, yp2); - _mav_put_float(buf, 20, zp2); - _mav_put_uint16_t(buf, 24, timeout); - _mav_put_uint8_t(buf, 26, type); - _mav_put_uint8_t(buf, 27, color); - _mav_put_uint8_t(buf, 28, coordinate_system); - _mav_put_char_array(buf, 29, name, 26); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); -#endif -#else - mavlink_point_of_interest_connection_t packet; - packet.xp1 = xp1; - packet.yp1 = yp1; - packet.zp1 = zp1; - packet.xp2 = xp2; - packet.yp2 = yp2; - packet.zp2 = zp2; - packet.timeout = timeout; - packet.type = type; - packet.color = color; - packet.coordinate_system = coordinate_system; - mav_array_memcpy(packet.name, name, sizeof(char)*26); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, (const char *)&packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, (const char *)&packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); -#endif -#endif -} - -#endif - -// MESSAGE POINT_OF_INTEREST_CONNECTION UNPACKING - - -/** - * @brief Get field type from point_of_interest_connection message - * - * @return 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - */ -static inline uint8_t mavlink_msg_point_of_interest_connection_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 26); -} - -/** - * @brief Get field color from point_of_interest_connection message - * - * @return 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - */ -static inline uint8_t mavlink_msg_point_of_interest_connection_get_color(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 27); -} - -/** - * @brief Get field coordinate_system from point_of_interest_connection message - * - * @return 0: global, 1:local - */ -static inline uint8_t mavlink_msg_point_of_interest_connection_get_coordinate_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 28); -} - -/** - * @brief Get field timeout from point_of_interest_connection message - * - * @return 0: no timeout, >1: timeout in seconds - */ -static inline uint16_t mavlink_msg_point_of_interest_connection_get_timeout(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 24); -} - -/** - * @brief Get field xp1 from point_of_interest_connection message - * - * @return X1 Position - */ -static inline float mavlink_msg_point_of_interest_connection_get_xp1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field yp1 from point_of_interest_connection message - * - * @return Y1 Position - */ -static inline float mavlink_msg_point_of_interest_connection_get_yp1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field zp1 from point_of_interest_connection message - * - * @return Z1 Position - */ -static inline float mavlink_msg_point_of_interest_connection_get_zp1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field xp2 from point_of_interest_connection message - * - * @return X2 Position - */ -static inline float mavlink_msg_point_of_interest_connection_get_xp2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field yp2 from point_of_interest_connection message - * - * @return Y2 Position - */ -static inline float mavlink_msg_point_of_interest_connection_get_yp2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field zp2 from point_of_interest_connection message - * - * @return Z2 Position - */ -static inline float mavlink_msg_point_of_interest_connection_get_zp2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field name from point_of_interest_connection message - * - * @return POI connection name - */ -static inline uint16_t mavlink_msg_point_of_interest_connection_get_name(const mavlink_message_t* msg, char *name) -{ - return _MAV_RETURN_char_array(msg, name, 26, 29); -} - -/** - * @brief Decode a point_of_interest_connection message into a struct - * - * @param msg The message to decode - * @param point_of_interest_connection C-struct to decode the message contents into - */ -static inline void mavlink_msg_point_of_interest_connection_decode(const mavlink_message_t* msg, mavlink_point_of_interest_connection_t* point_of_interest_connection) -{ -#if MAVLINK_NEED_BYTE_SWAP - point_of_interest_connection->xp1 = mavlink_msg_point_of_interest_connection_get_xp1(msg); - point_of_interest_connection->yp1 = mavlink_msg_point_of_interest_connection_get_yp1(msg); - point_of_interest_connection->zp1 = mavlink_msg_point_of_interest_connection_get_zp1(msg); - point_of_interest_connection->xp2 = mavlink_msg_point_of_interest_connection_get_xp2(msg); - point_of_interest_connection->yp2 = mavlink_msg_point_of_interest_connection_get_yp2(msg); - point_of_interest_connection->zp2 = mavlink_msg_point_of_interest_connection_get_zp2(msg); - point_of_interest_connection->timeout = mavlink_msg_point_of_interest_connection_get_timeout(msg); - point_of_interest_connection->type = mavlink_msg_point_of_interest_connection_get_type(msg); - point_of_interest_connection->color = mavlink_msg_point_of_interest_connection_get_color(msg); - point_of_interest_connection->coordinate_system = mavlink_msg_point_of_interest_connection_get_coordinate_system(msg); - mavlink_msg_point_of_interest_connection_get_name(msg, point_of_interest_connection->name); -#else - memcpy(point_of_interest_connection, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h deleted file mode 100644 index 6f4ca51..0000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h +++ /dev/null @@ -1,265 +0,0 @@ -// MESSAGE POSITION_CONTROL_SETPOINT PACKING - -#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT 170 - -typedef struct __mavlink_position_control_setpoint_t -{ - float x; ///< x position - float y; ///< y position - float z; ///< z position - float yaw; ///< yaw orientation in radians, 0 = NORTH - uint16_t id; ///< ID of waypoint, 0 for plain position -} mavlink_position_control_setpoint_t; - -#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN 18 -#define MAVLINK_MSG_ID_170_LEN 18 - -#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_CRC 28 -#define MAVLINK_MSG_ID_170_CRC 28 - - - -#define MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT { \ - "POSITION_CONTROL_SETPOINT", \ - 5, \ - { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_position_control_setpoint_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_position_control_setpoint_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_position_control_setpoint_t, z) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_position_control_setpoint_t, yaw) }, \ - { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_position_control_setpoint_t, id) }, \ - } \ -} - - -/** - * @brief Pack a position_control_setpoint message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param id ID of waypoint, 0 for plain position - * @param x x position - * @param y y position - * @param z z position - * @param yaw yaw orientation in radians, 0 = NORTH - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_position_control_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t id, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint16_t(buf, 16, id); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); -#else - mavlink_position_control_setpoint_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.id = id; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); -#endif -} - -/** - * @brief Pack a position_control_setpoint message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param id ID of waypoint, 0 for plain position - * @param x x position - * @param y y position - * @param z z position - * @param yaw yaw orientation in radians, 0 = NORTH - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_position_control_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t id,float x,float y,float z,float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint16_t(buf, 16, id); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); -#else - mavlink_position_control_setpoint_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.id = id; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); -#endif -} - -/** - * @brief Encode a position_control_setpoint struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param position_control_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_position_control_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_position_control_setpoint_t* position_control_setpoint) -{ - return mavlink_msg_position_control_setpoint_pack(system_id, component_id, msg, position_control_setpoint->id, position_control_setpoint->x, position_control_setpoint->y, position_control_setpoint->z, position_control_setpoint->yaw); -} - -/** - * @brief Encode a position_control_setpoint struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param position_control_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_position_control_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_position_control_setpoint_t* position_control_setpoint) -{ - return mavlink_msg_position_control_setpoint_pack_chan(system_id, component_id, chan, msg, position_control_setpoint->id, position_control_setpoint->x, position_control_setpoint->y, position_control_setpoint->z, position_control_setpoint->yaw); -} - -/** - * @brief Send a position_control_setpoint message - * @param chan MAVLink channel to send the message - * - * @param id ID of waypoint, 0 for plain position - * @param x x position - * @param y y position - * @param z z position - * @param yaw yaw orientation in radians, 0 = NORTH - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_position_control_setpoint_send(mavlink_channel_t chan, uint16_t id, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint16_t(buf, 16, id); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, buf, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, buf, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); -#endif -#else - mavlink_position_control_setpoint_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.id = id; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); -#endif -#endif -} - -#endif - -// MESSAGE POSITION_CONTROL_SETPOINT UNPACKING - - -/** - * @brief Get field id from position_control_setpoint message - * - * @return ID of waypoint, 0 for plain position - */ -static inline uint16_t mavlink_msg_position_control_setpoint_get_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 16); -} - -/** - * @brief Get field x from position_control_setpoint message - * - * @return x position - */ -static inline float mavlink_msg_position_control_setpoint_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field y from position_control_setpoint message - * - * @return y position - */ -static inline float mavlink_msg_position_control_setpoint_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field z from position_control_setpoint message - * - * @return z position - */ -static inline float mavlink_msg_position_control_setpoint_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yaw from position_control_setpoint message - * - * @return yaw orientation in radians, 0 = NORTH - */ -static inline float mavlink_msg_position_control_setpoint_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a position_control_setpoint message into a struct - * - * @param msg The message to decode - * @param position_control_setpoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_position_control_setpoint_decode(const mavlink_message_t* msg, mavlink_position_control_setpoint_t* position_control_setpoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - position_control_setpoint->x = mavlink_msg_position_control_setpoint_get_x(msg); - position_control_setpoint->y = mavlink_msg_position_control_setpoint_get_y(msg); - position_control_setpoint->z = mavlink_msg_position_control_setpoint_get_z(msg); - position_control_setpoint->yaw = mavlink_msg_position_control_setpoint_get_yaw(msg); - position_control_setpoint->id = mavlink_msg_position_control_setpoint_get_id(msg); -#else - memcpy(position_control_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h deleted file mode 100644 index 08cedbb..0000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h +++ /dev/null @@ -1,309 +0,0 @@ -// MESSAGE RAW_AUX PACKING - -#define MAVLINK_MSG_ID_RAW_AUX 172 - -typedef struct __mavlink_raw_aux_t -{ - int32_t baro; ///< Barometric pressure (hecto Pascal) - uint16_t adc1; ///< ADC1 (J405 ADC3, LPC2148 AD0.6) - uint16_t adc2; ///< ADC2 (J405 ADC5, LPC2148 AD0.2) - uint16_t adc3; ///< ADC3 (J405 ADC6, LPC2148 AD0.1) - uint16_t adc4; ///< ADC4 (J405 ADC7, LPC2148 AD1.3) - uint16_t vbat; ///< Battery voltage - int16_t temp; ///< Temperature (degrees celcius) -} mavlink_raw_aux_t; - -#define MAVLINK_MSG_ID_RAW_AUX_LEN 16 -#define MAVLINK_MSG_ID_172_LEN 16 - -#define MAVLINK_MSG_ID_RAW_AUX_CRC 182 -#define MAVLINK_MSG_ID_172_CRC 182 - - - -#define MAVLINK_MESSAGE_INFO_RAW_AUX { \ - "RAW_AUX", \ - 7, \ - { { "baro", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_raw_aux_t, baro) }, \ - { "adc1", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_raw_aux_t, adc1) }, \ - { "adc2", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_raw_aux_t, adc2) }, \ - { "adc3", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_raw_aux_t, adc3) }, \ - { "adc4", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_raw_aux_t, adc4) }, \ - { "vbat", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_raw_aux_t, vbat) }, \ - { "temp", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_raw_aux_t, temp) }, \ - } \ -} - - -/** - * @brief Pack a raw_aux message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param adc1 ADC1 (J405 ADC3, LPC2148 AD0.6) - * @param adc2 ADC2 (J405 ADC5, LPC2148 AD0.2) - * @param adc3 ADC3 (J405 ADC6, LPC2148 AD0.1) - * @param adc4 ADC4 (J405 ADC7, LPC2148 AD1.3) - * @param vbat Battery voltage - * @param temp Temperature (degrees celcius) - * @param baro Barometric pressure (hecto Pascal) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_raw_aux_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t vbat, int16_t temp, int32_t baro) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RAW_AUX_LEN]; - _mav_put_int32_t(buf, 0, baro); - _mav_put_uint16_t(buf, 4, adc1); - _mav_put_uint16_t(buf, 6, adc2); - _mav_put_uint16_t(buf, 8, adc3); - _mav_put_uint16_t(buf, 10, adc4); - _mav_put_uint16_t(buf, 12, vbat); - _mav_put_int16_t(buf, 14, temp); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_AUX_LEN); -#else - mavlink_raw_aux_t packet; - packet.baro = baro; - packet.adc1 = adc1; - packet.adc2 = adc2; - packet.adc3 = adc3; - packet.adc4 = adc4; - packet.vbat = vbat; - packet.temp = temp; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_AUX_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RAW_AUX; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_AUX_LEN, MAVLINK_MSG_ID_RAW_AUX_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_AUX_LEN); -#endif -} - -/** - * @brief Pack a raw_aux message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param adc1 ADC1 (J405 ADC3, LPC2148 AD0.6) - * @param adc2 ADC2 (J405 ADC5, LPC2148 AD0.2) - * @param adc3 ADC3 (J405 ADC6, LPC2148 AD0.1) - * @param adc4 ADC4 (J405 ADC7, LPC2148 AD1.3) - * @param vbat Battery voltage - * @param temp Temperature (degrees celcius) - * @param baro Barometric pressure (hecto Pascal) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_raw_aux_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t adc1,uint16_t adc2,uint16_t adc3,uint16_t adc4,uint16_t vbat,int16_t temp,int32_t baro) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RAW_AUX_LEN]; - _mav_put_int32_t(buf, 0, baro); - _mav_put_uint16_t(buf, 4, adc1); - _mav_put_uint16_t(buf, 6, adc2); - _mav_put_uint16_t(buf, 8, adc3); - _mav_put_uint16_t(buf, 10, adc4); - _mav_put_uint16_t(buf, 12, vbat); - _mav_put_int16_t(buf, 14, temp); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_AUX_LEN); -#else - mavlink_raw_aux_t packet; - packet.baro = baro; - packet.adc1 = adc1; - packet.adc2 = adc2; - packet.adc3 = adc3; - packet.adc4 = adc4; - packet.vbat = vbat; - packet.temp = temp; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_AUX_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RAW_AUX; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_AUX_LEN, MAVLINK_MSG_ID_RAW_AUX_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_AUX_LEN); -#endif -} - -/** - * @brief Encode a raw_aux struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param raw_aux C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_raw_aux_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_raw_aux_t* raw_aux) -{ - return mavlink_msg_raw_aux_pack(system_id, component_id, msg, raw_aux->adc1, raw_aux->adc2, raw_aux->adc3, raw_aux->adc4, raw_aux->vbat, raw_aux->temp, raw_aux->baro); -} - -/** - * @brief Encode a raw_aux struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param raw_aux C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_raw_aux_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_aux_t* raw_aux) -{ - return mavlink_msg_raw_aux_pack_chan(system_id, component_id, chan, msg, raw_aux->adc1, raw_aux->adc2, raw_aux->adc3, raw_aux->adc4, raw_aux->vbat, raw_aux->temp, raw_aux->baro); -} - -/** - * @brief Send a raw_aux message - * @param chan MAVLink channel to send the message - * - * @param adc1 ADC1 (J405 ADC3, LPC2148 AD0.6) - * @param adc2 ADC2 (J405 ADC5, LPC2148 AD0.2) - * @param adc3 ADC3 (J405 ADC6, LPC2148 AD0.1) - * @param adc4 ADC4 (J405 ADC7, LPC2148 AD1.3) - * @param vbat Battery voltage - * @param temp Temperature (degrees celcius) - * @param baro Barometric pressure (hecto Pascal) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_raw_aux_send(mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t vbat, int16_t temp, int32_t baro) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RAW_AUX_LEN]; - _mav_put_int32_t(buf, 0, baro); - _mav_put_uint16_t(buf, 4, adc1); - _mav_put_uint16_t(buf, 6, adc2); - _mav_put_uint16_t(buf, 8, adc3); - _mav_put_uint16_t(buf, 10, adc4); - _mav_put_uint16_t(buf, 12, vbat); - _mav_put_int16_t(buf, 14, temp); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, buf, MAVLINK_MSG_ID_RAW_AUX_LEN, MAVLINK_MSG_ID_RAW_AUX_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, buf, MAVLINK_MSG_ID_RAW_AUX_LEN); -#endif -#else - mavlink_raw_aux_t packet; - packet.baro = baro; - packet.adc1 = adc1; - packet.adc2 = adc2; - packet.adc3 = adc3; - packet.adc4 = adc4; - packet.vbat = vbat; - packet.temp = temp; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, (const char *)&packet, MAVLINK_MSG_ID_RAW_AUX_LEN, MAVLINK_MSG_ID_RAW_AUX_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, (const char *)&packet, MAVLINK_MSG_ID_RAW_AUX_LEN); -#endif -#endif -} - -#endif - -// MESSAGE RAW_AUX UNPACKING - - -/** - * @brief Get field adc1 from raw_aux message - * - * @return ADC1 (J405 ADC3, LPC2148 AD0.6) - */ -static inline uint16_t mavlink_msg_raw_aux_get_adc1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field adc2 from raw_aux message - * - * @return ADC2 (J405 ADC5, LPC2148 AD0.2) - */ -static inline uint16_t mavlink_msg_raw_aux_get_adc2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Get field adc3 from raw_aux message - * - * @return ADC3 (J405 ADC6, LPC2148 AD0.1) - */ -static inline uint16_t mavlink_msg_raw_aux_get_adc3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} - -/** - * @brief Get field adc4 from raw_aux message - * - * @return ADC4 (J405 ADC7, LPC2148 AD1.3) - */ -static inline uint16_t mavlink_msg_raw_aux_get_adc4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 10); -} - -/** - * @brief Get field vbat from raw_aux message - * - * @return Battery voltage - */ -static inline uint16_t mavlink_msg_raw_aux_get_vbat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 12); -} - -/** - * @brief Get field temp from raw_aux message - * - * @return Temperature (degrees celcius) - */ -static inline int16_t mavlink_msg_raw_aux_get_temp(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 14); -} - -/** - * @brief Get field baro from raw_aux message - * - * @return Barometric pressure (hecto Pascal) - */ -static inline int32_t mavlink_msg_raw_aux_get_baro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Decode a raw_aux message into a struct - * - * @param msg The message to decode - * @param raw_aux C-struct to decode the message contents into - */ -static inline void mavlink_msg_raw_aux_decode(const mavlink_message_t* msg, mavlink_raw_aux_t* raw_aux) -{ -#if MAVLINK_NEED_BYTE_SWAP - raw_aux->baro = mavlink_msg_raw_aux_get_baro(msg); - raw_aux->adc1 = mavlink_msg_raw_aux_get_adc1(msg); - raw_aux->adc2 = mavlink_msg_raw_aux_get_adc2(msg); - raw_aux->adc3 = mavlink_msg_raw_aux_get_adc3(msg); - raw_aux->adc4 = mavlink_msg_raw_aux_get_adc4(msg); - raw_aux->vbat = mavlink_msg_raw_aux_get_vbat(msg); - raw_aux->temp = mavlink_msg_raw_aux_get_temp(msg); -#else - memcpy(raw_aux, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RAW_AUX_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h deleted file mode 100644 index b26d748..0000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h +++ /dev/null @@ -1,287 +0,0 @@ -// MESSAGE SET_CAM_SHUTTER PACKING - -#define MAVLINK_MSG_ID_SET_CAM_SHUTTER 151 - -typedef struct __mavlink_set_cam_shutter_t -{ - float gain; ///< Camera gain - uint16_t interval; ///< Shutter interval, in microseconds - uint16_t exposure; ///< Exposure time, in microseconds - uint8_t cam_no; ///< Camera id - uint8_t cam_mode; ///< Camera mode: 0 = auto, 1 = manual - uint8_t trigger_pin; ///< Trigger pin, 0-3 for PtGrey FireFly -} mavlink_set_cam_shutter_t; - -#define MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN 11 -#define MAVLINK_MSG_ID_151_LEN 11 - -#define MAVLINK_MSG_ID_SET_CAM_SHUTTER_CRC 108 -#define MAVLINK_MSG_ID_151_CRC 108 - - - -#define MAVLINK_MESSAGE_INFO_SET_CAM_SHUTTER { \ - "SET_CAM_SHUTTER", \ - 6, \ - { { "gain", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_set_cam_shutter_t, gain) }, \ - { "interval", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_set_cam_shutter_t, interval) }, \ - { "exposure", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_set_cam_shutter_t, exposure) }, \ - { "cam_no", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_set_cam_shutter_t, cam_no) }, \ - { "cam_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_set_cam_shutter_t, cam_mode) }, \ - { "trigger_pin", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_set_cam_shutter_t, trigger_pin) }, \ - } \ -} - - -/** - * @brief Pack a set_cam_shutter message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param cam_no Camera id - * @param cam_mode Camera mode: 0 = auto, 1 = manual - * @param trigger_pin Trigger pin, 0-3 for PtGrey FireFly - * @param interval Shutter interval, in microseconds - * @param exposure Exposure time, in microseconds - * @param gain Camera gain - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_cam_shutter_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t cam_no, uint8_t cam_mode, uint8_t trigger_pin, uint16_t interval, uint16_t exposure, float gain) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN]; - _mav_put_float(buf, 0, gain); - _mav_put_uint16_t(buf, 4, interval); - _mav_put_uint16_t(buf, 6, exposure); - _mav_put_uint8_t(buf, 8, cam_no); - _mav_put_uint8_t(buf, 9, cam_mode); - _mav_put_uint8_t(buf, 10, trigger_pin); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); -#else - mavlink_set_cam_shutter_t packet; - packet.gain = gain; - packet.interval = interval; - packet.exposure = exposure; - packet.cam_no = cam_no; - packet.cam_mode = cam_mode; - packet.trigger_pin = trigger_pin; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN, MAVLINK_MSG_ID_SET_CAM_SHUTTER_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); -#endif -} - -/** - * @brief Pack a set_cam_shutter message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param cam_no Camera id - * @param cam_mode Camera mode: 0 = auto, 1 = manual - * @param trigger_pin Trigger pin, 0-3 for PtGrey FireFly - * @param interval Shutter interval, in microseconds - * @param exposure Exposure time, in microseconds - * @param gain Camera gain - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_cam_shutter_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t cam_no,uint8_t cam_mode,uint8_t trigger_pin,uint16_t interval,uint16_t exposure,float gain) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN]; - _mav_put_float(buf, 0, gain); - _mav_put_uint16_t(buf, 4, interval); - _mav_put_uint16_t(buf, 6, exposure); - _mav_put_uint8_t(buf, 8, cam_no); - _mav_put_uint8_t(buf, 9, cam_mode); - _mav_put_uint8_t(buf, 10, trigger_pin); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); -#else - mavlink_set_cam_shutter_t packet; - packet.gain = gain; - packet.interval = interval; - packet.exposure = exposure; - packet.cam_no = cam_no; - packet.cam_mode = cam_mode; - packet.trigger_pin = trigger_pin; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN, MAVLINK_MSG_ID_SET_CAM_SHUTTER_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); -#endif -} - -/** - * @brief Encode a set_cam_shutter struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_cam_shutter C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_cam_shutter_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_cam_shutter_t* set_cam_shutter) -{ - return mavlink_msg_set_cam_shutter_pack(system_id, component_id, msg, set_cam_shutter->cam_no, set_cam_shutter->cam_mode, set_cam_shutter->trigger_pin, set_cam_shutter->interval, set_cam_shutter->exposure, set_cam_shutter->gain); -} - -/** - * @brief Encode a set_cam_shutter struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_cam_shutter C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_cam_shutter_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_cam_shutter_t* set_cam_shutter) -{ - return mavlink_msg_set_cam_shutter_pack_chan(system_id, component_id, chan, msg, set_cam_shutter->cam_no, set_cam_shutter->cam_mode, set_cam_shutter->trigger_pin, set_cam_shutter->interval, set_cam_shutter->exposure, set_cam_shutter->gain); -} - -/** - * @brief Send a set_cam_shutter message - * @param chan MAVLink channel to send the message - * - * @param cam_no Camera id - * @param cam_mode Camera mode: 0 = auto, 1 = manual - * @param trigger_pin Trigger pin, 0-3 for PtGrey FireFly - * @param interval Shutter interval, in microseconds - * @param exposure Exposure time, in microseconds - * @param gain Camera gain - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_cam_shutter_send(mavlink_channel_t chan, uint8_t cam_no, uint8_t cam_mode, uint8_t trigger_pin, uint16_t interval, uint16_t exposure, float gain) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN]; - _mav_put_float(buf, 0, gain); - _mav_put_uint16_t(buf, 4, interval); - _mav_put_uint16_t(buf, 6, exposure); - _mav_put_uint8_t(buf, 8, cam_no); - _mav_put_uint8_t(buf, 9, cam_mode); - _mav_put_uint8_t(buf, 10, trigger_pin); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, buf, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN, MAVLINK_MSG_ID_SET_CAM_SHUTTER_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, buf, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); -#endif -#else - mavlink_set_cam_shutter_t packet; - packet.gain = gain; - packet.interval = interval; - packet.exposure = exposure; - packet.cam_no = cam_no; - packet.cam_mode = cam_mode; - packet.trigger_pin = trigger_pin; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, (const char *)&packet, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN, MAVLINK_MSG_ID_SET_CAM_SHUTTER_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, (const char *)&packet, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SET_CAM_SHUTTER UNPACKING - - -/** - * @brief Get field cam_no from set_cam_shutter message - * - * @return Camera id - */ -static inline uint8_t mavlink_msg_set_cam_shutter_get_cam_no(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 8); -} - -/** - * @brief Get field cam_mode from set_cam_shutter message - * - * @return Camera mode: 0 = auto, 1 = manual - */ -static inline uint8_t mavlink_msg_set_cam_shutter_get_cam_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 9); -} - -/** - * @brief Get field trigger_pin from set_cam_shutter message - * - * @return Trigger pin, 0-3 for PtGrey FireFly - */ -static inline uint8_t mavlink_msg_set_cam_shutter_get_trigger_pin(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 10); -} - -/** - * @brief Get field interval from set_cam_shutter message - * - * @return Shutter interval, in microseconds - */ -static inline uint16_t mavlink_msg_set_cam_shutter_get_interval(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field exposure from set_cam_shutter message - * - * @return Exposure time, in microseconds - */ -static inline uint16_t mavlink_msg_set_cam_shutter_get_exposure(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Get field gain from set_cam_shutter message - * - * @return Camera gain - */ -static inline float mavlink_msg_set_cam_shutter_get_gain(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Decode a set_cam_shutter message into a struct - * - * @param msg The message to decode - * @param set_cam_shutter C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_cam_shutter_decode(const mavlink_message_t* msg, mavlink_set_cam_shutter_t* set_cam_shutter) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_cam_shutter->gain = mavlink_msg_set_cam_shutter_get_gain(msg); - set_cam_shutter->interval = mavlink_msg_set_cam_shutter_get_interval(msg); - set_cam_shutter->exposure = mavlink_msg_set_cam_shutter_get_exposure(msg); - set_cam_shutter->cam_no = mavlink_msg_set_cam_shutter_get_cam_no(msg); - set_cam_shutter->cam_mode = mavlink_msg_set_cam_shutter_get_cam_mode(msg); - set_cam_shutter->trigger_pin = mavlink_msg_set_cam_shutter_get_trigger_pin(msg); -#else - memcpy(set_cam_shutter, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h deleted file mode 100644 index f1f9e69..0000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h +++ /dev/null @@ -1,287 +0,0 @@ -// MESSAGE SET_POSITION_CONTROL_OFFSET PACKING - -#define MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET 160 - -typedef struct __mavlink_set_position_control_offset_t -{ - float x; ///< x position offset - float y; ///< y position offset - float z; ///< z position offset - float yaw; ///< yaw orientation offset in radians, 0 = NORTH - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_set_position_control_offset_t; - -#define MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN 18 -#define MAVLINK_MSG_ID_160_LEN 18 - -#define MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_CRC 22 -#define MAVLINK_MSG_ID_160_CRC 22 - - - -#define MAVLINK_MESSAGE_INFO_SET_POSITION_CONTROL_OFFSET { \ - "SET_POSITION_CONTROL_OFFSET", \ - 6, \ - { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_set_position_control_offset_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_set_position_control_offset_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_set_position_control_offset_t, z) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_set_position_control_offset_t, yaw) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_set_position_control_offset_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_set_position_control_offset_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a set_position_control_offset message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param x x position offset - * @param y y position offset - * @param z z position offset - * @param yaw yaw orientation offset in radians, 0 = NORTH - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_position_control_offset_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); -#else - mavlink_set_position_control_offset_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); -#endif -} - -/** - * @brief Pack a set_position_control_offset message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param x x position offset - * @param y y position offset - * @param z z position offset - * @param yaw yaw orientation offset in radians, 0 = NORTH - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_position_control_offset_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,float x,float y,float z,float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); -#else - mavlink_set_position_control_offset_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); -#endif -} - -/** - * @brief Encode a set_position_control_offset struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_position_control_offset C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_position_control_offset_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_position_control_offset_t* set_position_control_offset) -{ - return mavlink_msg_set_position_control_offset_pack(system_id, component_id, msg, set_position_control_offset->target_system, set_position_control_offset->target_component, set_position_control_offset->x, set_position_control_offset->y, set_position_control_offset->z, set_position_control_offset->yaw); -} - -/** - * @brief Encode a set_position_control_offset struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_position_control_offset C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_position_control_offset_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_position_control_offset_t* set_position_control_offset) -{ - return mavlink_msg_set_position_control_offset_pack_chan(system_id, component_id, chan, msg, set_position_control_offset->target_system, set_position_control_offset->target_component, set_position_control_offset->x, set_position_control_offset->y, set_position_control_offset->z, set_position_control_offset->yaw); -} - -/** - * @brief Send a set_position_control_offset message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param x x position offset - * @param y y position offset - * @param z z position offset - * @param yaw yaw orientation offset in radians, 0 = NORTH - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_position_control_offset_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, buf, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, buf, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); -#endif -#else - mavlink_set_position_control_offset_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, (const char *)&packet, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, (const char *)&packet, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SET_POSITION_CONTROL_OFFSET UNPACKING - - -/** - * @brief Get field target_system from set_position_control_offset message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_set_position_control_offset_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Get field target_component from set_position_control_offset message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_set_position_control_offset_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 17); -} - -/** - * @brief Get field x from set_position_control_offset message - * - * @return x position offset - */ -static inline float mavlink_msg_set_position_control_offset_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field y from set_position_control_offset message - * - * @return y position offset - */ -static inline float mavlink_msg_set_position_control_offset_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field z from set_position_control_offset message - * - * @return z position offset - */ -static inline float mavlink_msg_set_position_control_offset_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yaw from set_position_control_offset message - * - * @return yaw orientation offset in radians, 0 = NORTH - */ -static inline float mavlink_msg_set_position_control_offset_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a set_position_control_offset message into a struct - * - * @param msg The message to decode - * @param set_position_control_offset C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_position_control_offset_decode(const mavlink_message_t* msg, mavlink_set_position_control_offset_t* set_position_control_offset) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_position_control_offset->x = mavlink_msg_set_position_control_offset_get_x(msg); - set_position_control_offset->y = mavlink_msg_set_position_control_offset_get_y(msg); - set_position_control_offset->z = mavlink_msg_set_position_control_offset_get_z(msg); - set_position_control_offset->yaw = mavlink_msg_set_position_control_offset_get_yaw(msg); - set_position_control_offset->target_system = mavlink_msg_set_position_control_offset_get_target_system(msg); - set_position_control_offset->target_component = mavlink_msg_set_position_control_offset_get_target_component(msg); -#else - memcpy(set_position_control_offset, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h deleted file mode 100644 index 9458087..0000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h +++ /dev/null @@ -1,243 +0,0 @@ -// MESSAGE WATCHDOG_COMMAND PACKING - -#define MAVLINK_MSG_ID_WATCHDOG_COMMAND 183 - -typedef struct __mavlink_watchdog_command_t -{ - uint16_t watchdog_id; ///< Watchdog ID - uint16_t process_id; ///< Process ID - uint8_t target_system_id; ///< Target system ID - uint8_t command_id; ///< Command ID -} mavlink_watchdog_command_t; - -#define MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN 6 -#define MAVLINK_MSG_ID_183_LEN 6 - -#define MAVLINK_MSG_ID_WATCHDOG_COMMAND_CRC 162 -#define MAVLINK_MSG_ID_183_CRC 162 - - - -#define MAVLINK_MESSAGE_INFO_WATCHDOG_COMMAND { \ - "WATCHDOG_COMMAND", \ - 4, \ - { { "watchdog_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_watchdog_command_t, watchdog_id) }, \ - { "process_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_watchdog_command_t, process_id) }, \ - { "target_system_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_watchdog_command_t, target_system_id) }, \ - { "command_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_watchdog_command_t, command_id) }, \ - } \ -} - - -/** - * @brief Pack a watchdog_command message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system_id Target system ID - * @param watchdog_id Watchdog ID - * @param process_id Process ID - * @param command_id Command ID - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_watchdog_command_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system_id, uint16_t watchdog_id, uint16_t process_id, uint8_t command_id) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN]; - _mav_put_uint16_t(buf, 0, watchdog_id); - _mav_put_uint16_t(buf, 2, process_id); - _mav_put_uint8_t(buf, 4, target_system_id); - _mav_put_uint8_t(buf, 5, command_id); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); -#else - mavlink_watchdog_command_t packet; - packet.watchdog_id = watchdog_id; - packet.process_id = process_id; - packet.target_system_id = target_system_id; - packet.command_id = command_id; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN, MAVLINK_MSG_ID_WATCHDOG_COMMAND_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); -#endif -} - -/** - * @brief Pack a watchdog_command message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system_id Target system ID - * @param watchdog_id Watchdog ID - * @param process_id Process ID - * @param command_id Command ID - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_watchdog_command_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system_id,uint16_t watchdog_id,uint16_t process_id,uint8_t command_id) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN]; - _mav_put_uint16_t(buf, 0, watchdog_id); - _mav_put_uint16_t(buf, 2, process_id); - _mav_put_uint8_t(buf, 4, target_system_id); - _mav_put_uint8_t(buf, 5, command_id); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); -#else - mavlink_watchdog_command_t packet; - packet.watchdog_id = watchdog_id; - packet.process_id = process_id; - packet.target_system_id = target_system_id; - packet.command_id = command_id; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN, MAVLINK_MSG_ID_WATCHDOG_COMMAND_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); -#endif -} - -/** - * @brief Encode a watchdog_command struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param watchdog_command C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_watchdog_command_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_watchdog_command_t* watchdog_command) -{ - return mavlink_msg_watchdog_command_pack(system_id, component_id, msg, watchdog_command->target_system_id, watchdog_command->watchdog_id, watchdog_command->process_id, watchdog_command->command_id); -} - -/** - * @brief Encode a watchdog_command struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param watchdog_command C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_watchdog_command_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_command_t* watchdog_command) -{ - return mavlink_msg_watchdog_command_pack_chan(system_id, component_id, chan, msg, watchdog_command->target_system_id, watchdog_command->watchdog_id, watchdog_command->process_id, watchdog_command->command_id); -} - -/** - * @brief Send a watchdog_command message - * @param chan MAVLink channel to send the message - * - * @param target_system_id Target system ID - * @param watchdog_id Watchdog ID - * @param process_id Process ID - * @param command_id Command ID - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_watchdog_command_send(mavlink_channel_t chan, uint8_t target_system_id, uint16_t watchdog_id, uint16_t process_id, uint8_t command_id) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN]; - _mav_put_uint16_t(buf, 0, watchdog_id); - _mav_put_uint16_t(buf, 2, process_id); - _mav_put_uint8_t(buf, 4, target_system_id); - _mav_put_uint8_t(buf, 5, command_id); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, buf, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN, MAVLINK_MSG_ID_WATCHDOG_COMMAND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, buf, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); -#endif -#else - mavlink_watchdog_command_t packet; - packet.watchdog_id = watchdog_id; - packet.process_id = process_id; - packet.target_system_id = target_system_id; - packet.command_id = command_id; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, (const char *)&packet, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN, MAVLINK_MSG_ID_WATCHDOG_COMMAND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, (const char *)&packet, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); -#endif -#endif -} - -#endif - -// MESSAGE WATCHDOG_COMMAND UNPACKING - - -/** - * @brief Get field target_system_id from watchdog_command message - * - * @return Target system ID - */ -static inline uint8_t mavlink_msg_watchdog_command_get_target_system_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field watchdog_id from watchdog_command message - * - * @return Watchdog ID - */ -static inline uint16_t mavlink_msg_watchdog_command_get_watchdog_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field process_id from watchdog_command message - * - * @return Process ID - */ -static inline uint16_t mavlink_msg_watchdog_command_get_process_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Get field command_id from watchdog_command message - * - * @return Command ID - */ -static inline uint8_t mavlink_msg_watchdog_command_get_command_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Decode a watchdog_command message into a struct - * - * @param msg The message to decode - * @param watchdog_command C-struct to decode the message contents into - */ -static inline void mavlink_msg_watchdog_command_decode(const mavlink_message_t* msg, mavlink_watchdog_command_t* watchdog_command) -{ -#if MAVLINK_NEED_BYTE_SWAP - watchdog_command->watchdog_id = mavlink_msg_watchdog_command_get_watchdog_id(msg); - watchdog_command->process_id = mavlink_msg_watchdog_command_get_process_id(msg); - watchdog_command->target_system_id = mavlink_msg_watchdog_command_get_target_system_id(msg); - watchdog_command->command_id = mavlink_msg_watchdog_command_get_command_id(msg); -#else - memcpy(watchdog_command, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h deleted file mode 100644 index 3f4295e..0000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h +++ /dev/null @@ -1,199 +0,0 @@ -// MESSAGE WATCHDOG_HEARTBEAT PACKING - -#define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT 180 - -typedef struct __mavlink_watchdog_heartbeat_t -{ - uint16_t watchdog_id; ///< Watchdog ID - uint16_t process_count; ///< Number of processes -} mavlink_watchdog_heartbeat_t; - -#define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN 4 -#define MAVLINK_MSG_ID_180_LEN 4 - -#define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_CRC 153 -#define MAVLINK_MSG_ID_180_CRC 153 - - - -#define MAVLINK_MESSAGE_INFO_WATCHDOG_HEARTBEAT { \ - "WATCHDOG_HEARTBEAT", \ - 2, \ - { { "watchdog_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_watchdog_heartbeat_t, watchdog_id) }, \ - { "process_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_watchdog_heartbeat_t, process_count) }, \ - } \ -} - - -/** - * @brief Pack a watchdog_heartbeat message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param watchdog_id Watchdog ID - * @param process_count Number of processes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_watchdog_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t watchdog_id, uint16_t process_count) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN]; - _mav_put_uint16_t(buf, 0, watchdog_id); - _mav_put_uint16_t(buf, 2, process_count); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); -#else - mavlink_watchdog_heartbeat_t packet; - packet.watchdog_id = watchdog_id; - packet.process_count = process_count; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); -#endif -} - -/** - * @brief Pack a watchdog_heartbeat message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param watchdog_id Watchdog ID - * @param process_count Number of processes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_watchdog_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t watchdog_id,uint16_t process_count) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN]; - _mav_put_uint16_t(buf, 0, watchdog_id); - _mav_put_uint16_t(buf, 2, process_count); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); -#else - mavlink_watchdog_heartbeat_t packet; - packet.watchdog_id = watchdog_id; - packet.process_count = process_count; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); -#endif -} - -/** - * @brief Encode a watchdog_heartbeat struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param watchdog_heartbeat C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_watchdog_heartbeat_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_watchdog_heartbeat_t* watchdog_heartbeat) -{ - return mavlink_msg_watchdog_heartbeat_pack(system_id, component_id, msg, watchdog_heartbeat->watchdog_id, watchdog_heartbeat->process_count); -} - -/** - * @brief Encode a watchdog_heartbeat struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param watchdog_heartbeat C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_watchdog_heartbeat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_heartbeat_t* watchdog_heartbeat) -{ - return mavlink_msg_watchdog_heartbeat_pack_chan(system_id, component_id, chan, msg, watchdog_heartbeat->watchdog_id, watchdog_heartbeat->process_count); -} - -/** - * @brief Send a watchdog_heartbeat message - * @param chan MAVLink channel to send the message - * - * @param watchdog_id Watchdog ID - * @param process_count Number of processes - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_watchdog_heartbeat_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_count) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN]; - _mav_put_uint16_t(buf, 0, watchdog_id); - _mav_put_uint16_t(buf, 2, process_count); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, buf, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, buf, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); -#endif -#else - mavlink_watchdog_heartbeat_t packet; - packet.watchdog_id = watchdog_id; - packet.process_count = process_count; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); -#endif -#endif -} - -#endif - -// MESSAGE WATCHDOG_HEARTBEAT UNPACKING - - -/** - * @brief Get field watchdog_id from watchdog_heartbeat message - * - * @return Watchdog ID - */ -static inline uint16_t mavlink_msg_watchdog_heartbeat_get_watchdog_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field process_count from watchdog_heartbeat message - * - * @return Number of processes - */ -static inline uint16_t mavlink_msg_watchdog_heartbeat_get_process_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Decode a watchdog_heartbeat message into a struct - * - * @param msg The message to decode - * @param watchdog_heartbeat C-struct to decode the message contents into - */ -static inline void mavlink_msg_watchdog_heartbeat_decode(const mavlink_message_t* msg, mavlink_watchdog_heartbeat_t* watchdog_heartbeat) -{ -#if MAVLINK_NEED_BYTE_SWAP - watchdog_heartbeat->watchdog_id = mavlink_msg_watchdog_heartbeat_get_watchdog_id(msg); - watchdog_heartbeat->process_count = mavlink_msg_watchdog_heartbeat_get_process_count(msg); -#else - memcpy(watchdog_heartbeat, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h deleted file mode 100644 index 55853cd..0000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h +++ /dev/null @@ -1,260 +0,0 @@ -// MESSAGE WATCHDOG_PROCESS_INFO PACKING - -#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO 181 - -typedef struct __mavlink_watchdog_process_info_t -{ - int32_t timeout; ///< Timeout (seconds) - uint16_t watchdog_id; ///< Watchdog ID - uint16_t process_id; ///< Process ID - char name[100]; ///< Process name - char arguments[147]; ///< Process arguments -} mavlink_watchdog_process_info_t; - -#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN 255 -#define MAVLINK_MSG_ID_181_LEN 255 - -#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_CRC 16 -#define MAVLINK_MSG_ID_181_CRC 16 - -#define MAVLINK_MSG_WATCHDOG_PROCESS_INFO_FIELD_NAME_LEN 100 -#define MAVLINK_MSG_WATCHDOG_PROCESS_INFO_FIELD_ARGUMENTS_LEN 147 - -#define MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_INFO { \ - "WATCHDOG_PROCESS_INFO", \ - 5, \ - { { "timeout", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_watchdog_process_info_t, timeout) }, \ - { "watchdog_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_watchdog_process_info_t, watchdog_id) }, \ - { "process_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_watchdog_process_info_t, process_id) }, \ - { "name", NULL, MAVLINK_TYPE_CHAR, 100, 8, offsetof(mavlink_watchdog_process_info_t, name) }, \ - { "arguments", NULL, MAVLINK_TYPE_CHAR, 147, 108, offsetof(mavlink_watchdog_process_info_t, arguments) }, \ - } \ -} - - -/** - * @brief Pack a watchdog_process_info message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param watchdog_id Watchdog ID - * @param process_id Process ID - * @param name Process name - * @param arguments Process arguments - * @param timeout Timeout (seconds) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t watchdog_id, uint16_t process_id, const char *name, const char *arguments, int32_t timeout) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN]; - _mav_put_int32_t(buf, 0, timeout); - _mav_put_uint16_t(buf, 4, watchdog_id); - _mav_put_uint16_t(buf, 6, process_id); - _mav_put_char_array(buf, 8, name, 100); - _mav_put_char_array(buf, 108, arguments, 147); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); -#else - mavlink_watchdog_process_info_t packet; - packet.timeout = timeout; - packet.watchdog_id = watchdog_id; - packet.process_id = process_id; - mav_array_memcpy(packet.name, name, sizeof(char)*100); - mav_array_memcpy(packet.arguments, arguments, sizeof(char)*147); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); -#endif -} - -/** - * @brief Pack a watchdog_process_info message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param watchdog_id Watchdog ID - * @param process_id Process ID - * @param name Process name - * @param arguments Process arguments - * @param timeout Timeout (seconds) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_watchdog_process_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t watchdog_id,uint16_t process_id,const char *name,const char *arguments,int32_t timeout) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN]; - _mav_put_int32_t(buf, 0, timeout); - _mav_put_uint16_t(buf, 4, watchdog_id); - _mav_put_uint16_t(buf, 6, process_id); - _mav_put_char_array(buf, 8, name, 100); - _mav_put_char_array(buf, 108, arguments, 147); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); -#else - mavlink_watchdog_process_info_t packet; - packet.timeout = timeout; - packet.watchdog_id = watchdog_id; - packet.process_id = process_id; - mav_array_memcpy(packet.name, name, sizeof(char)*100); - mav_array_memcpy(packet.arguments, arguments, sizeof(char)*147); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); -#endif -} - -/** - * @brief Encode a watchdog_process_info struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param watchdog_process_info C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_watchdog_process_info_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_watchdog_process_info_t* watchdog_process_info) -{ - return mavlink_msg_watchdog_process_info_pack(system_id, component_id, msg, watchdog_process_info->watchdog_id, watchdog_process_info->process_id, watchdog_process_info->name, watchdog_process_info->arguments, watchdog_process_info->timeout); -} - -/** - * @brief Encode a watchdog_process_info struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param watchdog_process_info C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_watchdog_process_info_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_process_info_t* watchdog_process_info) -{ - return mavlink_msg_watchdog_process_info_pack_chan(system_id, component_id, chan, msg, watchdog_process_info->watchdog_id, watchdog_process_info->process_id, watchdog_process_info->name, watchdog_process_info->arguments, watchdog_process_info->timeout); -} - -/** - * @brief Send a watchdog_process_info message - * @param chan MAVLink channel to send the message - * - * @param watchdog_id Watchdog ID - * @param process_id Process ID - * @param name Process name - * @param arguments Process arguments - * @param timeout Timeout (seconds) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, const char *name, const char *arguments, int32_t timeout) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN]; - _mav_put_int32_t(buf, 0, timeout); - _mav_put_uint16_t(buf, 4, watchdog_id); - _mav_put_uint16_t(buf, 6, process_id); - _mav_put_char_array(buf, 8, name, 100); - _mav_put_char_array(buf, 108, arguments, 147); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); -#endif -#else - mavlink_watchdog_process_info_t packet; - packet.timeout = timeout; - packet.watchdog_id = watchdog_id; - packet.process_id = process_id; - mav_array_memcpy(packet.name, name, sizeof(char)*100); - mav_array_memcpy(packet.arguments, arguments, sizeof(char)*147); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, (const char *)&packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, (const char *)&packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); -#endif -#endif -} - -#endif - -// MESSAGE WATCHDOG_PROCESS_INFO UNPACKING - - -/** - * @brief Get field watchdog_id from watchdog_process_info message - * - * @return Watchdog ID - */ -static inline uint16_t mavlink_msg_watchdog_process_info_get_watchdog_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field process_id from watchdog_process_info message - * - * @return Process ID - */ -static inline uint16_t mavlink_msg_watchdog_process_info_get_process_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Get field name from watchdog_process_info message - * - * @return Process name - */ -static inline uint16_t mavlink_msg_watchdog_process_info_get_name(const mavlink_message_t* msg, char *name) -{ - return _MAV_RETURN_char_array(msg, name, 100, 8); -} - -/** - * @brief Get field arguments from watchdog_process_info message - * - * @return Process arguments - */ -static inline uint16_t mavlink_msg_watchdog_process_info_get_arguments(const mavlink_message_t* msg, char *arguments) -{ - return _MAV_RETURN_char_array(msg, arguments, 147, 108); -} - -/** - * @brief Get field timeout from watchdog_process_info message - * - * @return Timeout (seconds) - */ -static inline int32_t mavlink_msg_watchdog_process_info_get_timeout(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Decode a watchdog_process_info message into a struct - * - * @param msg The message to decode - * @param watchdog_process_info C-struct to decode the message contents into - */ -static inline void mavlink_msg_watchdog_process_info_decode(const mavlink_message_t* msg, mavlink_watchdog_process_info_t* watchdog_process_info) -{ -#if MAVLINK_NEED_BYTE_SWAP - watchdog_process_info->timeout = mavlink_msg_watchdog_process_info_get_timeout(msg); - watchdog_process_info->watchdog_id = mavlink_msg_watchdog_process_info_get_watchdog_id(msg); - watchdog_process_info->process_id = mavlink_msg_watchdog_process_info_get_process_id(msg); - mavlink_msg_watchdog_process_info_get_name(msg, watchdog_process_info->name); - mavlink_msg_watchdog_process_info_get_arguments(msg, watchdog_process_info->arguments); -#else - memcpy(watchdog_process_info, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h deleted file mode 100644 index a0410d8..0000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h +++ /dev/null @@ -1,287 +0,0 @@ -// MESSAGE WATCHDOG_PROCESS_STATUS PACKING - -#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS 182 - -typedef struct __mavlink_watchdog_process_status_t -{ - int32_t pid; ///< PID - uint16_t watchdog_id; ///< Watchdog ID - uint16_t process_id; ///< Process ID - uint16_t crashes; ///< Number of crashes - uint8_t state; ///< Is running / finished / suspended / crashed - uint8_t muted; ///< Is muted -} mavlink_watchdog_process_status_t; - -#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN 12 -#define MAVLINK_MSG_ID_182_LEN 12 - -#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_CRC 29 -#define MAVLINK_MSG_ID_182_CRC 29 - - - -#define MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_STATUS { \ - "WATCHDOG_PROCESS_STATUS", \ - 6, \ - { { "pid", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_watchdog_process_status_t, pid) }, \ - { "watchdog_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_watchdog_process_status_t, watchdog_id) }, \ - { "process_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_watchdog_process_status_t, process_id) }, \ - { "crashes", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_watchdog_process_status_t, crashes) }, \ - { "state", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_watchdog_process_status_t, state) }, \ - { "muted", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_watchdog_process_status_t, muted) }, \ - } \ -} - - -/** - * @brief Pack a watchdog_process_status message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param watchdog_id Watchdog ID - * @param process_id Process ID - * @param state Is running / finished / suspended / crashed - * @param muted Is muted - * @param pid PID - * @param crashes Number of crashes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_watchdog_process_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t watchdog_id, uint16_t process_id, uint8_t state, uint8_t muted, int32_t pid, uint16_t crashes) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN]; - _mav_put_int32_t(buf, 0, pid); - _mav_put_uint16_t(buf, 4, watchdog_id); - _mav_put_uint16_t(buf, 6, process_id); - _mav_put_uint16_t(buf, 8, crashes); - _mav_put_uint8_t(buf, 10, state); - _mav_put_uint8_t(buf, 11, muted); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); -#else - mavlink_watchdog_process_status_t packet; - packet.pid = pid; - packet.watchdog_id = watchdog_id; - packet.process_id = process_id; - packet.crashes = crashes; - packet.state = state; - packet.muted = muted; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); -#endif -} - -/** - * @brief Pack a watchdog_process_status message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param watchdog_id Watchdog ID - * @param process_id Process ID - * @param state Is running / finished / suspended / crashed - * @param muted Is muted - * @param pid PID - * @param crashes Number of crashes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_watchdog_process_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t watchdog_id,uint16_t process_id,uint8_t state,uint8_t muted,int32_t pid,uint16_t crashes) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN]; - _mav_put_int32_t(buf, 0, pid); - _mav_put_uint16_t(buf, 4, watchdog_id); - _mav_put_uint16_t(buf, 6, process_id); - _mav_put_uint16_t(buf, 8, crashes); - _mav_put_uint8_t(buf, 10, state); - _mav_put_uint8_t(buf, 11, muted); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); -#else - mavlink_watchdog_process_status_t packet; - packet.pid = pid; - packet.watchdog_id = watchdog_id; - packet.process_id = process_id; - packet.crashes = crashes; - packet.state = state; - packet.muted = muted; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); -#endif -} - -/** - * @brief Encode a watchdog_process_status struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param watchdog_process_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_watchdog_process_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_watchdog_process_status_t* watchdog_process_status) -{ - return mavlink_msg_watchdog_process_status_pack(system_id, component_id, msg, watchdog_process_status->watchdog_id, watchdog_process_status->process_id, watchdog_process_status->state, watchdog_process_status->muted, watchdog_process_status->pid, watchdog_process_status->crashes); -} - -/** - * @brief Encode a watchdog_process_status struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param watchdog_process_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_watchdog_process_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_process_status_t* watchdog_process_status) -{ - return mavlink_msg_watchdog_process_status_pack_chan(system_id, component_id, chan, msg, watchdog_process_status->watchdog_id, watchdog_process_status->process_id, watchdog_process_status->state, watchdog_process_status->muted, watchdog_process_status->pid, watchdog_process_status->crashes); -} - -/** - * @brief Send a watchdog_process_status message - * @param chan MAVLink channel to send the message - * - * @param watchdog_id Watchdog ID - * @param process_id Process ID - * @param state Is running / finished / suspended / crashed - * @param muted Is muted - * @param pid PID - * @param crashes Number of crashes - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_watchdog_process_status_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, uint8_t state, uint8_t muted, int32_t pid, uint16_t crashes) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN]; - _mav_put_int32_t(buf, 0, pid); - _mav_put_uint16_t(buf, 4, watchdog_id); - _mav_put_uint16_t(buf, 6, process_id); - _mav_put_uint16_t(buf, 8, crashes); - _mav_put_uint8_t(buf, 10, state); - _mav_put_uint8_t(buf, 11, muted); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); -#endif -#else - mavlink_watchdog_process_status_t packet; - packet.pid = pid; - packet.watchdog_id = watchdog_id; - packet.process_id = process_id; - packet.crashes = crashes; - packet.state = state; - packet.muted = muted; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); -#endif -#endif -} - -#endif - -// MESSAGE WATCHDOG_PROCESS_STATUS UNPACKING - - -/** - * @brief Get field watchdog_id from watchdog_process_status message - * - * @return Watchdog ID - */ -static inline uint16_t mavlink_msg_watchdog_process_status_get_watchdog_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field process_id from watchdog_process_status message - * - * @return Process ID - */ -static inline uint16_t mavlink_msg_watchdog_process_status_get_process_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Get field state from watchdog_process_status message - * - * @return Is running / finished / suspended / crashed - */ -static inline uint8_t mavlink_msg_watchdog_process_status_get_state(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 10); -} - -/** - * @brief Get field muted from watchdog_process_status message - * - * @return Is muted - */ -static inline uint8_t mavlink_msg_watchdog_process_status_get_muted(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 11); -} - -/** - * @brief Get field pid from watchdog_process_status message - * - * @return PID - */ -static inline int32_t mavlink_msg_watchdog_process_status_get_pid(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field crashes from watchdog_process_status message - * - * @return Number of crashes - */ -static inline uint16_t mavlink_msg_watchdog_process_status_get_crashes(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} - -/** - * @brief Decode a watchdog_process_status message into a struct - * - * @param msg The message to decode - * @param watchdog_process_status C-struct to decode the message contents into - */ -static inline void mavlink_msg_watchdog_process_status_decode(const mavlink_message_t* msg, mavlink_watchdog_process_status_t* watchdog_process_status) -{ -#if MAVLINK_NEED_BYTE_SWAP - watchdog_process_status->pid = mavlink_msg_watchdog_process_status_get_pid(msg); - watchdog_process_status->watchdog_id = mavlink_msg_watchdog_process_status_get_watchdog_id(msg); - watchdog_process_status->process_id = mavlink_msg_watchdog_process_status_get_process_id(msg); - watchdog_process_status->crashes = mavlink_msg_watchdog_process_status_get_crashes(msg); - watchdog_process_status->state = mavlink_msg_watchdog_process_status_get_state(msg); - watchdog_process_status->muted = mavlink_msg_watchdog_process_status_get_muted(msg); -#else - memcpy(watchdog_process_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h b/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h deleted file mode 100644 index 428619e..0000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h +++ /dev/null @@ -1,127 +0,0 @@ -/** @file - * @brief MAVLink comm protocol generated from pixhawk.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef PIXHAWK_H -#define PIXHAWK_H - -#ifdef __cplusplus -extern "C" { -#endif - -// MESSAGE LENGTHS AND CRCS - -#ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 0, 0, 0, 0, 0, 13, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 11, 52, 1, 92, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 26, 16, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 0, 0, 0, 0, 0, 0, 106, 43, 55, 0, 0, 53, 0, 0, 0, 0, 21, 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, 36, 30, 18, 18, 51, 9, 0} -#endif - -#ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 0, 0, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 108, 86, 95, 224, 0, 0, 0, 0, 0, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 249, 182, 0, 0, 0, 0, 0, 0, 0, 153, 16, 29, 162, 0, 0, 0, 0, 0, 0, 90, 95, 36, 0, 0, 88, 0, 0, 0, 0, 254, 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, 204, 49, 170, 44, 83, 46, 0} -#endif - -#ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"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_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_CAM_SHUTTER, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGERED, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGER_CONTROL, MAVLINK_MESSAGE_INFO_IMAGE_AVAILABLE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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_SET_POSITION_CONTROL_OFFSET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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_POSITION_CONTROL_SETPOINT, MAVLINK_MESSAGE_INFO_MARKER, MAVLINK_MESSAGE_INFO_RAW_AUX, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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_WATCHDOG_HEARTBEAT, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_INFO, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_STATUS, MAVLINK_MESSAGE_INFO_WATCHDOG_COMMAND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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_PATTERN_DETECTED, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST_CONNECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BRIEF_FEATURE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} -#endif - -#include "../protocol.h" - -#define MAVLINK_ENABLED_PIXHAWK - -// ENUM DEFINITIONS - - -/** @brief Content Types for data transmission handshake */ -#ifndef HAVE_ENUM_DATA_TYPES -#define HAVE_ENUM_DATA_TYPES -enum DATA_TYPES -{ - DATA_TYPE_JPEG_IMAGE=1, /* | */ - DATA_TYPE_RAW_IMAGE=2, /* | */ - DATA_TYPE_KINECT=3, /* | */ - DATA_TYPES_ENUM_END=4, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_MAV_CMD -#define HAVE_ENUM_MAV_CMD -enum MAV_CMD -{ - MAV_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ - MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ - MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ - MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ - MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ - MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ - MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ - MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - MAV_CMD_ENUM_END=501, /* | */ - MAV_CMD_DO_START_SEARCH=10001, /* Starts a search |1 to arm, 0 to disarm| */ - MAV_CMD_DO_FINISH_SEARCH=10002, /* Starts a search |1 to arm, 0 to disarm| */ - MAV_CMD_NAV_SWEEP=10003, /* Starts a search |1 to arm, 0 to disarm| */ -}; -#endif - -#include "../common/common.h" - -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -// MESSAGE DEFINITIONS -#include "./mavlink_msg_set_cam_shutter.h" -#include "./mavlink_msg_image_triggered.h" -#include "./mavlink_msg_image_trigger_control.h" -#include "./mavlink_msg_image_available.h" -#include "./mavlink_msg_set_position_control_offset.h" -#include "./mavlink_msg_position_control_setpoint.h" -#include "./mavlink_msg_marker.h" -#include "./mavlink_msg_raw_aux.h" -#include "./mavlink_msg_watchdog_heartbeat.h" -#include "./mavlink_msg_watchdog_process_info.h" -#include "./mavlink_msg_watchdog_process_status.h" -#include "./mavlink_msg_watchdog_command.h" -#include "./mavlink_msg_pattern_detected.h" -#include "./mavlink_msg_point_of_interest.h" -#include "./mavlink_msg_point_of_interest_connection.h" -#include "./mavlink_msg_brief_feature.h" -#include "./mavlink_msg_attitude_control.h" - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // PIXHAWK_H diff --git a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.pb.h b/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.pb.h deleted file mode 100644 index 7556606..0000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.pb.h +++ /dev/null @@ -1,3663 +0,0 @@ -// Generated by the protocol buffer compiler. DO NOT EDIT! -// source: pixhawk.proto - -#ifndef PROTOBUF_pixhawk_2eproto__INCLUDED -#define PROTOBUF_pixhawk_2eproto__INCLUDED - -#include - -#include - -#if GOOGLE_PROTOBUF_VERSION < 2004000 -#error This file was generated by a newer version of protoc which is -#error incompatible with your Protocol Buffer headers. Please update -#error your headers. -#endif -#if 2004001 < GOOGLE_PROTOBUF_MIN_PROTOC_VERSION -#error This file was generated by an older version of protoc which is -#error incompatible with your Protocol Buffer headers. Please -#error regenerate this file with a newer version of protoc. -#endif - -#include -#include -#include -#include -// @@protoc_insertion_point(includes) - -namespace px { - -// Internal implementation detail -- do not call these. -void protobuf_AddDesc_pixhawk_2eproto(); -void protobuf_AssignDesc_pixhawk_2eproto(); -void protobuf_ShutdownFile_pixhawk_2eproto(); - -class HeaderInfo; -class GLOverlay; -class Obstacle; -class ObstacleList; -class ObstacleMap; -class Path; -class PointCloudXYZI; -class PointCloudXYZI_PointXYZI; -class PointCloudXYZRGB; -class PointCloudXYZRGB_PointXYZRGB; -class RGBDImage; -class Waypoint; - -enum GLOverlay_CoordinateFrameType { - GLOverlay_CoordinateFrameType_GLOBAL = 0, - GLOverlay_CoordinateFrameType_LOCAL = 1 -}; -bool GLOverlay_CoordinateFrameType_IsValid(int value); -const GLOverlay_CoordinateFrameType GLOverlay_CoordinateFrameType_CoordinateFrameType_MIN = GLOverlay_CoordinateFrameType_GLOBAL; -const GLOverlay_CoordinateFrameType GLOverlay_CoordinateFrameType_CoordinateFrameType_MAX = GLOverlay_CoordinateFrameType_LOCAL; -const int GLOverlay_CoordinateFrameType_CoordinateFrameType_ARRAYSIZE = GLOverlay_CoordinateFrameType_CoordinateFrameType_MAX + 1; - -const ::google::protobuf::EnumDescriptor* GLOverlay_CoordinateFrameType_descriptor(); -inline const ::std::string& GLOverlay_CoordinateFrameType_Name(GLOverlay_CoordinateFrameType value) { - return ::google::protobuf::internal::NameOfEnum( - GLOverlay_CoordinateFrameType_descriptor(), value); -} -inline bool GLOverlay_CoordinateFrameType_Parse( - const ::std::string& name, GLOverlay_CoordinateFrameType* value) { - return ::google::protobuf::internal::ParseNamedEnum( - GLOverlay_CoordinateFrameType_descriptor(), name, value); -} -enum GLOverlay_Mode { - GLOverlay_Mode_POINTS = 0, - GLOverlay_Mode_LINES = 1, - GLOverlay_Mode_LINE_STRIP = 2, - GLOverlay_Mode_LINE_LOOP = 3, - GLOverlay_Mode_TRIANGLES = 4, - GLOverlay_Mode_TRIANGLE_STRIP = 5, - GLOverlay_Mode_TRIANGLE_FAN = 6, - GLOverlay_Mode_QUADS = 7, - GLOverlay_Mode_QUAD_STRIP = 8, - GLOverlay_Mode_POLYGON = 9, - GLOverlay_Mode_SOLID_CIRCLE = 10, - GLOverlay_Mode_WIRE_CIRCLE = 11, - GLOverlay_Mode_SOLID_CUBE = 12, - GLOverlay_Mode_WIRE_CUBE = 13 -}; -bool GLOverlay_Mode_IsValid(int value); -const GLOverlay_Mode GLOverlay_Mode_Mode_MIN = GLOverlay_Mode_POINTS; -const GLOverlay_Mode GLOverlay_Mode_Mode_MAX = GLOverlay_Mode_WIRE_CUBE; -const int GLOverlay_Mode_Mode_ARRAYSIZE = GLOverlay_Mode_Mode_MAX + 1; - -const ::google::protobuf::EnumDescriptor* GLOverlay_Mode_descriptor(); -inline const ::std::string& GLOverlay_Mode_Name(GLOverlay_Mode value) { - return ::google::protobuf::internal::NameOfEnum( - GLOverlay_Mode_descriptor(), value); -} -inline bool GLOverlay_Mode_Parse( - const ::std::string& name, GLOverlay_Mode* value) { - return ::google::protobuf::internal::ParseNamedEnum( - GLOverlay_Mode_descriptor(), name, value); -} -enum GLOverlay_Identifier { - GLOverlay_Identifier_END = 14, - GLOverlay_Identifier_VERTEX2F = 15, - GLOverlay_Identifier_VERTEX3F = 16, - GLOverlay_Identifier_ROTATEF = 17, - GLOverlay_Identifier_TRANSLATEF = 18, - GLOverlay_Identifier_SCALEF = 19, - GLOverlay_Identifier_PUSH_MATRIX = 20, - GLOverlay_Identifier_POP_MATRIX = 21, - GLOverlay_Identifier_COLOR3F = 22, - GLOverlay_Identifier_COLOR4F = 23, - GLOverlay_Identifier_POINTSIZE = 24, - GLOverlay_Identifier_LINEWIDTH = 25 -}; -bool GLOverlay_Identifier_IsValid(int value); -const GLOverlay_Identifier GLOverlay_Identifier_Identifier_MIN = GLOverlay_Identifier_END; -const GLOverlay_Identifier GLOverlay_Identifier_Identifier_MAX = GLOverlay_Identifier_LINEWIDTH; -const int GLOverlay_Identifier_Identifier_ARRAYSIZE = GLOverlay_Identifier_Identifier_MAX + 1; - -const ::google::protobuf::EnumDescriptor* GLOverlay_Identifier_descriptor(); -inline const ::std::string& GLOverlay_Identifier_Name(GLOverlay_Identifier value) { - return ::google::protobuf::internal::NameOfEnum( - GLOverlay_Identifier_descriptor(), value); -} -inline bool GLOverlay_Identifier_Parse( - const ::std::string& name, GLOverlay_Identifier* value) { - return ::google::protobuf::internal::ParseNamedEnum( - GLOverlay_Identifier_descriptor(), name, value); -} -// =================================================================== - -class HeaderInfo : public ::google::protobuf::Message { - public: - HeaderInfo(); - virtual ~HeaderInfo(); - - HeaderInfo(const HeaderInfo& from); - - inline HeaderInfo& operator=(const HeaderInfo& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const HeaderInfo& default_instance(); - - void Swap(HeaderInfo* other); - - // implements Message ---------------------------------------------- - - HeaderInfo* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const HeaderInfo& from); - void MergeFrom(const HeaderInfo& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required int32 source_sysid = 1; - inline bool has_source_sysid() const; - inline void clear_source_sysid(); - static const int kSourceSysidFieldNumber = 1; - inline ::google::protobuf::int32 source_sysid() const; - inline void set_source_sysid(::google::protobuf::int32 value); - - // required int32 source_compid = 2; - inline bool has_source_compid() const; - inline void clear_source_compid(); - static const int kSourceCompidFieldNumber = 2; - inline ::google::protobuf::int32 source_compid() const; - inline void set_source_compid(::google::protobuf::int32 value); - - // required double timestamp = 3; - inline bool has_timestamp() const; - inline void clear_timestamp(); - static const int kTimestampFieldNumber = 3; - inline double timestamp() const; - inline void set_timestamp(double value); - - // @@protoc_insertion_point(class_scope:px.HeaderInfo) - private: - inline void set_has_source_sysid(); - inline void clear_has_source_sysid(); - inline void set_has_source_compid(); - inline void clear_has_source_compid(); - inline void set_has_timestamp(); - inline void clear_has_timestamp(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::google::protobuf::int32 source_sysid_; - ::google::protobuf::int32 source_compid_; - double timestamp_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(3 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static HeaderInfo* default_instance_; -}; -// ------------------------------------------------------------------- - -class GLOverlay : public ::google::protobuf::Message { - public: - GLOverlay(); - virtual ~GLOverlay(); - - GLOverlay(const GLOverlay& from); - - inline GLOverlay& operator=(const GLOverlay& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const GLOverlay& default_instance(); - - void Swap(GLOverlay* other); - - // implements Message ---------------------------------------------- - - GLOverlay* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const GLOverlay& from); - void MergeFrom(const GLOverlay& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - typedef GLOverlay_CoordinateFrameType CoordinateFrameType; - static const CoordinateFrameType GLOBAL = GLOverlay_CoordinateFrameType_GLOBAL; - static const CoordinateFrameType LOCAL = GLOverlay_CoordinateFrameType_LOCAL; - static inline bool CoordinateFrameType_IsValid(int value) { - return GLOverlay_CoordinateFrameType_IsValid(value); - } - static const CoordinateFrameType CoordinateFrameType_MIN = - GLOverlay_CoordinateFrameType_CoordinateFrameType_MIN; - static const CoordinateFrameType CoordinateFrameType_MAX = - GLOverlay_CoordinateFrameType_CoordinateFrameType_MAX; - static const int CoordinateFrameType_ARRAYSIZE = - GLOverlay_CoordinateFrameType_CoordinateFrameType_ARRAYSIZE; - static inline const ::google::protobuf::EnumDescriptor* - CoordinateFrameType_descriptor() { - return GLOverlay_CoordinateFrameType_descriptor(); - } - static inline const ::std::string& CoordinateFrameType_Name(CoordinateFrameType value) { - return GLOverlay_CoordinateFrameType_Name(value); - } - static inline bool CoordinateFrameType_Parse(const ::std::string& name, - CoordinateFrameType* value) { - return GLOverlay_CoordinateFrameType_Parse(name, value); - } - - typedef GLOverlay_Mode Mode; - static const Mode POINTS = GLOverlay_Mode_POINTS; - static const Mode LINES = GLOverlay_Mode_LINES; - static const Mode LINE_STRIP = GLOverlay_Mode_LINE_STRIP; - static const Mode LINE_LOOP = GLOverlay_Mode_LINE_LOOP; - static const Mode TRIANGLES = GLOverlay_Mode_TRIANGLES; - static const Mode TRIANGLE_STRIP = GLOverlay_Mode_TRIANGLE_STRIP; - static const Mode TRIANGLE_FAN = GLOverlay_Mode_TRIANGLE_FAN; - static const Mode QUADS = GLOverlay_Mode_QUADS; - static const Mode QUAD_STRIP = GLOverlay_Mode_QUAD_STRIP; - static const Mode POLYGON = GLOverlay_Mode_POLYGON; - static const Mode SOLID_CIRCLE = GLOverlay_Mode_SOLID_CIRCLE; - static const Mode WIRE_CIRCLE = GLOverlay_Mode_WIRE_CIRCLE; - static const Mode SOLID_CUBE = GLOverlay_Mode_SOLID_CUBE; - static const Mode WIRE_CUBE = GLOverlay_Mode_WIRE_CUBE; - static inline bool Mode_IsValid(int value) { - return GLOverlay_Mode_IsValid(value); - } - static const Mode Mode_MIN = - GLOverlay_Mode_Mode_MIN; - static const Mode Mode_MAX = - GLOverlay_Mode_Mode_MAX; - static const int Mode_ARRAYSIZE = - GLOverlay_Mode_Mode_ARRAYSIZE; - static inline const ::google::protobuf::EnumDescriptor* - Mode_descriptor() { - return GLOverlay_Mode_descriptor(); - } - static inline const ::std::string& Mode_Name(Mode value) { - return GLOverlay_Mode_Name(value); - } - static inline bool Mode_Parse(const ::std::string& name, - Mode* value) { - return GLOverlay_Mode_Parse(name, value); - } - - typedef GLOverlay_Identifier Identifier; - static const Identifier END = GLOverlay_Identifier_END; - static const Identifier VERTEX2F = GLOverlay_Identifier_VERTEX2F; - static const Identifier VERTEX3F = GLOverlay_Identifier_VERTEX3F; - static const Identifier ROTATEF = GLOverlay_Identifier_ROTATEF; - static const Identifier TRANSLATEF = GLOverlay_Identifier_TRANSLATEF; - static const Identifier SCALEF = GLOverlay_Identifier_SCALEF; - static const Identifier PUSH_MATRIX = GLOverlay_Identifier_PUSH_MATRIX; - static const Identifier POP_MATRIX = GLOverlay_Identifier_POP_MATRIX; - static const Identifier COLOR3F = GLOverlay_Identifier_COLOR3F; - static const Identifier COLOR4F = GLOverlay_Identifier_COLOR4F; - static const Identifier POINTSIZE = GLOverlay_Identifier_POINTSIZE; - static const Identifier LINEWIDTH = GLOverlay_Identifier_LINEWIDTH; - static inline bool Identifier_IsValid(int value) { - return GLOverlay_Identifier_IsValid(value); - } - static const Identifier Identifier_MIN = - GLOverlay_Identifier_Identifier_MIN; - static const Identifier Identifier_MAX = - GLOverlay_Identifier_Identifier_MAX; - static const int Identifier_ARRAYSIZE = - GLOverlay_Identifier_Identifier_ARRAYSIZE; - static inline const ::google::protobuf::EnumDescriptor* - Identifier_descriptor() { - return GLOverlay_Identifier_descriptor(); - } - static inline const ::std::string& Identifier_Name(Identifier value) { - return GLOverlay_Identifier_Name(value); - } - static inline bool Identifier_Parse(const ::std::string& name, - Identifier* value) { - return GLOverlay_Identifier_Parse(name, value); - } - - // accessors ------------------------------------------------------- - - // required .px.HeaderInfo header = 1; - inline bool has_header() const; - inline void clear_header(); - static const int kHeaderFieldNumber = 1; - inline const ::px::HeaderInfo& header() const; - inline ::px::HeaderInfo* mutable_header(); - inline ::px::HeaderInfo* release_header(); - - // optional string name = 2; - inline bool has_name() const; - inline void clear_name(); - static const int kNameFieldNumber = 2; - inline const ::std::string& name() const; - inline void set_name(const ::std::string& value); - inline void set_name(const char* value); - inline void set_name(const char* value, size_t size); - inline ::std::string* mutable_name(); - inline ::std::string* release_name(); - - // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3; - inline bool has_coordinateframetype() const; - inline void clear_coordinateframetype(); - static const int kCoordinateFrameTypeFieldNumber = 3; - inline ::px::GLOverlay_CoordinateFrameType coordinateframetype() const; - inline void set_coordinateframetype(::px::GLOverlay_CoordinateFrameType value); - - // optional double origin_x = 4; - inline bool has_origin_x() const; - inline void clear_origin_x(); - static const int kOriginXFieldNumber = 4; - inline double origin_x() const; - inline void set_origin_x(double value); - - // optional double origin_y = 5; - inline bool has_origin_y() const; - inline void clear_origin_y(); - static const int kOriginYFieldNumber = 5; - inline double origin_y() const; - inline void set_origin_y(double value); - - // optional double origin_z = 6; - inline bool has_origin_z() const; - inline void clear_origin_z(); - static const int kOriginZFieldNumber = 6; - inline double origin_z() const; - inline void set_origin_z(double value); - - // optional bytes data = 7; - inline bool has_data() const; - inline void clear_data(); - static const int kDataFieldNumber = 7; - inline const ::std::string& data() const; - inline void set_data(const ::std::string& value); - inline void set_data(const char* value); - inline void set_data(const void* value, size_t size); - inline ::std::string* mutable_data(); - inline ::std::string* release_data(); - - // @@protoc_insertion_point(class_scope:px.GLOverlay) - private: - inline void set_has_header(); - inline void clear_has_header(); - inline void set_has_name(); - inline void clear_has_name(); - inline void set_has_coordinateframetype(); - inline void clear_has_coordinateframetype(); - inline void set_has_origin_x(); - inline void clear_has_origin_x(); - inline void set_has_origin_y(); - inline void clear_has_origin_y(); - inline void set_has_origin_z(); - inline void clear_has_origin_z(); - inline void set_has_data(); - inline void clear_has_data(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::px::HeaderInfo* header_; - ::std::string* name_; - double origin_x_; - double origin_y_; - double origin_z_; - ::std::string* data_; - int coordinateframetype_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(7 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static GLOverlay* default_instance_; -}; -// ------------------------------------------------------------------- - -class Obstacle : public ::google::protobuf::Message { - public: - Obstacle(); - virtual ~Obstacle(); - - Obstacle(const Obstacle& from); - - inline Obstacle& operator=(const Obstacle& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const Obstacle& default_instance(); - - void Swap(Obstacle* other); - - // implements Message ---------------------------------------------- - - Obstacle* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const Obstacle& from); - void MergeFrom(const Obstacle& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // optional float x = 1; - inline bool has_x() const; - inline void clear_x(); - static const int kXFieldNumber = 1; - inline float x() const; - inline void set_x(float value); - - // optional float y = 2; - inline bool has_y() const; - inline void clear_y(); - static const int kYFieldNumber = 2; - inline float y() const; - inline void set_y(float value); - - // optional float z = 3; - inline bool has_z() const; - inline void clear_z(); - static const int kZFieldNumber = 3; - inline float z() const; - inline void set_z(float value); - - // optional float length = 4; - inline bool has_length() const; - inline void clear_length(); - static const int kLengthFieldNumber = 4; - inline float length() const; - inline void set_length(float value); - - // optional float width = 5; - inline bool has_width() const; - inline void clear_width(); - static const int kWidthFieldNumber = 5; - inline float width() const; - inline void set_width(float value); - - // optional float height = 6; - inline bool has_height() const; - inline void clear_height(); - static const int kHeightFieldNumber = 6; - inline float height() const; - inline void set_height(float value); - - // @@protoc_insertion_point(class_scope:px.Obstacle) - private: - inline void set_has_x(); - inline void clear_has_x(); - inline void set_has_y(); - inline void clear_has_y(); - inline void set_has_z(); - inline void clear_has_z(); - inline void set_has_length(); - inline void clear_has_length(); - inline void set_has_width(); - inline void clear_has_width(); - inline void set_has_height(); - inline void clear_has_height(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - float x_; - float y_; - float z_; - float length_; - float width_; - float height_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(6 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static Obstacle* default_instance_; -}; -// ------------------------------------------------------------------- - -class ObstacleList : public ::google::protobuf::Message { - public: - ObstacleList(); - virtual ~ObstacleList(); - - ObstacleList(const ObstacleList& from); - - inline ObstacleList& operator=(const ObstacleList& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const ObstacleList& default_instance(); - - void Swap(ObstacleList* other); - - // implements Message ---------------------------------------------- - - ObstacleList* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const ObstacleList& from); - void MergeFrom(const ObstacleList& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required .px.HeaderInfo header = 1; - inline bool has_header() const; - inline void clear_header(); - static const int kHeaderFieldNumber = 1; - inline const ::px::HeaderInfo& header() const; - inline ::px::HeaderInfo* mutable_header(); - inline ::px::HeaderInfo* release_header(); - - // repeated .px.Obstacle obstacles = 2; - inline int obstacles_size() const; - inline void clear_obstacles(); - static const int kObstaclesFieldNumber = 2; - inline const ::px::Obstacle& obstacles(int index) const; - inline ::px::Obstacle* mutable_obstacles(int index); - inline ::px::Obstacle* add_obstacles(); - inline const ::google::protobuf::RepeatedPtrField< ::px::Obstacle >& - obstacles() const; - inline ::google::protobuf::RepeatedPtrField< ::px::Obstacle >* - mutable_obstacles(); - - // @@protoc_insertion_point(class_scope:px.ObstacleList) - private: - inline void set_has_header(); - inline void clear_has_header(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::px::HeaderInfo* header_; - ::google::protobuf::RepeatedPtrField< ::px::Obstacle > obstacles_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static ObstacleList* default_instance_; -}; -// ------------------------------------------------------------------- - -class ObstacleMap : public ::google::protobuf::Message { - public: - ObstacleMap(); - virtual ~ObstacleMap(); - - ObstacleMap(const ObstacleMap& from); - - inline ObstacleMap& operator=(const ObstacleMap& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const ObstacleMap& default_instance(); - - void Swap(ObstacleMap* other); - - // implements Message ---------------------------------------------- - - ObstacleMap* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const ObstacleMap& from); - void MergeFrom(const ObstacleMap& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required .px.HeaderInfo header = 1; - inline bool has_header() const; - inline void clear_header(); - static const int kHeaderFieldNumber = 1; - inline const ::px::HeaderInfo& header() const; - inline ::px::HeaderInfo* mutable_header(); - inline ::px::HeaderInfo* release_header(); - - // required int32 type = 2; - inline bool has_type() const; - inline void clear_type(); - static const int kTypeFieldNumber = 2; - inline ::google::protobuf::int32 type() const; - inline void set_type(::google::protobuf::int32 value); - - // optional float resolution = 3; - inline bool has_resolution() const; - inline void clear_resolution(); - static const int kResolutionFieldNumber = 3; - inline float resolution() const; - inline void set_resolution(float value); - - // optional int32 rows = 4; - inline bool has_rows() const; - inline void clear_rows(); - static const int kRowsFieldNumber = 4; - inline ::google::protobuf::int32 rows() const; - inline void set_rows(::google::protobuf::int32 value); - - // optional int32 cols = 5; - inline bool has_cols() const; - inline void clear_cols(); - static const int kColsFieldNumber = 5; - inline ::google::protobuf::int32 cols() const; - inline void set_cols(::google::protobuf::int32 value); - - // optional int32 mapR0 = 6; - inline bool has_mapr0() const; - inline void clear_mapr0(); - static const int kMapR0FieldNumber = 6; - inline ::google::protobuf::int32 mapr0() const; - inline void set_mapr0(::google::protobuf::int32 value); - - // optional int32 mapC0 = 7; - inline bool has_mapc0() const; - inline void clear_mapc0(); - static const int kMapC0FieldNumber = 7; - inline ::google::protobuf::int32 mapc0() const; - inline void set_mapc0(::google::protobuf::int32 value); - - // optional int32 arrayR0 = 8; - inline bool has_arrayr0() const; - inline void clear_arrayr0(); - static const int kArrayR0FieldNumber = 8; - inline ::google::protobuf::int32 arrayr0() const; - inline void set_arrayr0(::google::protobuf::int32 value); - - // optional int32 arrayC0 = 9; - inline bool has_arrayc0() const; - inline void clear_arrayc0(); - static const int kArrayC0FieldNumber = 9; - inline ::google::protobuf::int32 arrayc0() const; - inline void set_arrayc0(::google::protobuf::int32 value); - - // optional bytes data = 10; - inline bool has_data() const; - inline void clear_data(); - static const int kDataFieldNumber = 10; - inline const ::std::string& data() const; - inline void set_data(const ::std::string& value); - inline void set_data(const char* value); - inline void set_data(const void* value, size_t size); - inline ::std::string* mutable_data(); - inline ::std::string* release_data(); - - // @@protoc_insertion_point(class_scope:px.ObstacleMap) - private: - inline void set_has_header(); - inline void clear_has_header(); - inline void set_has_type(); - inline void clear_has_type(); - inline void set_has_resolution(); - inline void clear_has_resolution(); - inline void set_has_rows(); - inline void clear_has_rows(); - inline void set_has_cols(); - inline void clear_has_cols(); - inline void set_has_mapr0(); - inline void clear_has_mapr0(); - inline void set_has_mapc0(); - inline void clear_has_mapc0(); - inline void set_has_arrayr0(); - inline void clear_has_arrayr0(); - inline void set_has_arrayc0(); - inline void clear_has_arrayc0(); - inline void set_has_data(); - inline void clear_has_data(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::px::HeaderInfo* header_; - ::google::protobuf::int32 type_; - float resolution_; - ::google::protobuf::int32 rows_; - ::google::protobuf::int32 cols_; - ::google::protobuf::int32 mapr0_; - ::google::protobuf::int32 mapc0_; - ::google::protobuf::int32 arrayr0_; - ::google::protobuf::int32 arrayc0_; - ::std::string* data_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(10 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static ObstacleMap* default_instance_; -}; -// ------------------------------------------------------------------- - -class Path : public ::google::protobuf::Message { - public: - Path(); - virtual ~Path(); - - Path(const Path& from); - - inline Path& operator=(const Path& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const Path& default_instance(); - - void Swap(Path* other); - - // implements Message ---------------------------------------------- - - Path* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const Path& from); - void MergeFrom(const Path& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required .px.HeaderInfo header = 1; - inline bool has_header() const; - inline void clear_header(); - static const int kHeaderFieldNumber = 1; - inline const ::px::HeaderInfo& header() const; - inline ::px::HeaderInfo* mutable_header(); - inline ::px::HeaderInfo* release_header(); - - // repeated .px.Waypoint waypoints = 2; - inline int waypoints_size() const; - inline void clear_waypoints(); - static const int kWaypointsFieldNumber = 2; - inline const ::px::Waypoint& waypoints(int index) const; - inline ::px::Waypoint* mutable_waypoints(int index); - inline ::px::Waypoint* add_waypoints(); - inline const ::google::protobuf::RepeatedPtrField< ::px::Waypoint >& - waypoints() const; - inline ::google::protobuf::RepeatedPtrField< ::px::Waypoint >* - mutable_waypoints(); - - // @@protoc_insertion_point(class_scope:px.Path) - private: - inline void set_has_header(); - inline void clear_has_header(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::px::HeaderInfo* header_; - ::google::protobuf::RepeatedPtrField< ::px::Waypoint > waypoints_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static Path* default_instance_; -}; -// ------------------------------------------------------------------- - -class PointCloudXYZI_PointXYZI : public ::google::protobuf::Message { - public: - PointCloudXYZI_PointXYZI(); - virtual ~PointCloudXYZI_PointXYZI(); - - PointCloudXYZI_PointXYZI(const PointCloudXYZI_PointXYZI& from); - - inline PointCloudXYZI_PointXYZI& operator=(const PointCloudXYZI_PointXYZI& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const PointCloudXYZI_PointXYZI& default_instance(); - - void Swap(PointCloudXYZI_PointXYZI* other); - - // implements Message ---------------------------------------------- - - PointCloudXYZI_PointXYZI* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const PointCloudXYZI_PointXYZI& from); - void MergeFrom(const PointCloudXYZI_PointXYZI& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required float x = 1; - inline bool has_x() const; - inline void clear_x(); - static const int kXFieldNumber = 1; - inline float x() const; - inline void set_x(float value); - - // required float y = 2; - inline bool has_y() const; - inline void clear_y(); - static const int kYFieldNumber = 2; - inline float y() const; - inline void set_y(float value); - - // required float z = 3; - inline bool has_z() const; - inline void clear_z(); - static const int kZFieldNumber = 3; - inline float z() const; - inline void set_z(float value); - - // required float intensity = 4; - inline bool has_intensity() const; - inline void clear_intensity(); - static const int kIntensityFieldNumber = 4; - inline float intensity() const; - inline void set_intensity(float value); - - // @@protoc_insertion_point(class_scope:px.PointCloudXYZI.PointXYZI) - private: - inline void set_has_x(); - inline void clear_has_x(); - inline void set_has_y(); - inline void clear_has_y(); - inline void set_has_z(); - inline void clear_has_z(); - inline void set_has_intensity(); - inline void clear_has_intensity(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - float x_; - float y_; - float z_; - float intensity_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(4 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static PointCloudXYZI_PointXYZI* default_instance_; -}; -// ------------------------------------------------------------------- - -class PointCloudXYZI : public ::google::protobuf::Message { - public: - PointCloudXYZI(); - virtual ~PointCloudXYZI(); - - PointCloudXYZI(const PointCloudXYZI& from); - - inline PointCloudXYZI& operator=(const PointCloudXYZI& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const PointCloudXYZI& default_instance(); - - void Swap(PointCloudXYZI* other); - - // implements Message ---------------------------------------------- - - PointCloudXYZI* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const PointCloudXYZI& from); - void MergeFrom(const PointCloudXYZI& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - typedef PointCloudXYZI_PointXYZI PointXYZI; - - // accessors ------------------------------------------------------- - - // required .px.HeaderInfo header = 1; - inline bool has_header() const; - inline void clear_header(); - static const int kHeaderFieldNumber = 1; - inline const ::px::HeaderInfo& header() const; - inline ::px::HeaderInfo* mutable_header(); - inline ::px::HeaderInfo* release_header(); - - // repeated .px.PointCloudXYZI.PointXYZI points = 2; - inline int points_size() const; - inline void clear_points(); - static const int kPointsFieldNumber = 2; - inline const ::px::PointCloudXYZI_PointXYZI& points(int index) const; - inline ::px::PointCloudXYZI_PointXYZI* mutable_points(int index); - inline ::px::PointCloudXYZI_PointXYZI* add_points(); - inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >& - points() const; - inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >* - mutable_points(); - - // @@protoc_insertion_point(class_scope:px.PointCloudXYZI) - private: - inline void set_has_header(); - inline void clear_has_header(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::px::HeaderInfo* header_; - ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI > points_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static PointCloudXYZI* default_instance_; -}; -// ------------------------------------------------------------------- - -class PointCloudXYZRGB_PointXYZRGB : public ::google::protobuf::Message { - public: - PointCloudXYZRGB_PointXYZRGB(); - virtual ~PointCloudXYZRGB_PointXYZRGB(); - - PointCloudXYZRGB_PointXYZRGB(const PointCloudXYZRGB_PointXYZRGB& from); - - inline PointCloudXYZRGB_PointXYZRGB& operator=(const PointCloudXYZRGB_PointXYZRGB& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const PointCloudXYZRGB_PointXYZRGB& default_instance(); - - void Swap(PointCloudXYZRGB_PointXYZRGB* other); - - // implements Message ---------------------------------------------- - - PointCloudXYZRGB_PointXYZRGB* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const PointCloudXYZRGB_PointXYZRGB& from); - void MergeFrom(const PointCloudXYZRGB_PointXYZRGB& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required float x = 1; - inline bool has_x() const; - inline void clear_x(); - static const int kXFieldNumber = 1; - inline float x() const; - inline void set_x(float value); - - // required float y = 2; - inline bool has_y() const; - inline void clear_y(); - static const int kYFieldNumber = 2; - inline float y() const; - inline void set_y(float value); - - // required float z = 3; - inline bool has_z() const; - inline void clear_z(); - static const int kZFieldNumber = 3; - inline float z() const; - inline void set_z(float value); - - // required float rgb = 4; - inline bool has_rgb() const; - inline void clear_rgb(); - static const int kRgbFieldNumber = 4; - inline float rgb() const; - inline void set_rgb(float value); - - // @@protoc_insertion_point(class_scope:px.PointCloudXYZRGB.PointXYZRGB) - private: - inline void set_has_x(); - inline void clear_has_x(); - inline void set_has_y(); - inline void clear_has_y(); - inline void set_has_z(); - inline void clear_has_z(); - inline void set_has_rgb(); - inline void clear_has_rgb(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - float x_; - float y_; - float z_; - float rgb_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(4 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static PointCloudXYZRGB_PointXYZRGB* default_instance_; -}; -// ------------------------------------------------------------------- - -class PointCloudXYZRGB : public ::google::protobuf::Message { - public: - PointCloudXYZRGB(); - virtual ~PointCloudXYZRGB(); - - PointCloudXYZRGB(const PointCloudXYZRGB& from); - - inline PointCloudXYZRGB& operator=(const PointCloudXYZRGB& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const PointCloudXYZRGB& default_instance(); - - void Swap(PointCloudXYZRGB* other); - - // implements Message ---------------------------------------------- - - PointCloudXYZRGB* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const PointCloudXYZRGB& from); - void MergeFrom(const PointCloudXYZRGB& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - typedef PointCloudXYZRGB_PointXYZRGB PointXYZRGB; - - // accessors ------------------------------------------------------- - - // required .px.HeaderInfo header = 1; - inline bool has_header() const; - inline void clear_header(); - static const int kHeaderFieldNumber = 1; - inline const ::px::HeaderInfo& header() const; - inline ::px::HeaderInfo* mutable_header(); - inline ::px::HeaderInfo* release_header(); - - // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2; - inline int points_size() const; - inline void clear_points(); - static const int kPointsFieldNumber = 2; - inline const ::px::PointCloudXYZRGB_PointXYZRGB& points(int index) const; - inline ::px::PointCloudXYZRGB_PointXYZRGB* mutable_points(int index); - inline ::px::PointCloudXYZRGB_PointXYZRGB* add_points(); - inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >& - points() const; - inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >* - mutable_points(); - - // @@protoc_insertion_point(class_scope:px.PointCloudXYZRGB) - private: - inline void set_has_header(); - inline void clear_has_header(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::px::HeaderInfo* header_; - ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB > points_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static PointCloudXYZRGB* default_instance_; -}; -// ------------------------------------------------------------------- - -class RGBDImage : public ::google::protobuf::Message { - public: - RGBDImage(); - virtual ~RGBDImage(); - - RGBDImage(const RGBDImage& from); - - inline RGBDImage& operator=(const RGBDImage& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const RGBDImage& default_instance(); - - void Swap(RGBDImage* other); - - // implements Message ---------------------------------------------- - - RGBDImage* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const RGBDImage& from); - void MergeFrom(const RGBDImage& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required .px.HeaderInfo header = 1; - inline bool has_header() const; - inline void clear_header(); - static const int kHeaderFieldNumber = 1; - inline const ::px::HeaderInfo& header() const; - inline ::px::HeaderInfo* mutable_header(); - inline ::px::HeaderInfo* release_header(); - - // required uint32 cols = 2; - inline bool has_cols() const; - inline void clear_cols(); - static const int kColsFieldNumber = 2; - inline ::google::protobuf::uint32 cols() const; - inline void set_cols(::google::protobuf::uint32 value); - - // required uint32 rows = 3; - inline bool has_rows() const; - inline void clear_rows(); - static const int kRowsFieldNumber = 3; - inline ::google::protobuf::uint32 rows() const; - inline void set_rows(::google::protobuf::uint32 value); - - // required uint32 step1 = 4; - inline bool has_step1() const; - inline void clear_step1(); - static const int kStep1FieldNumber = 4; - inline ::google::protobuf::uint32 step1() const; - inline void set_step1(::google::protobuf::uint32 value); - - // required uint32 type1 = 5; - inline bool has_type1() const; - inline void clear_type1(); - static const int kType1FieldNumber = 5; - inline ::google::protobuf::uint32 type1() const; - inline void set_type1(::google::protobuf::uint32 value); - - // required bytes imageData1 = 6; - inline bool has_imagedata1() const; - inline void clear_imagedata1(); - static const int kImageData1FieldNumber = 6; - inline const ::std::string& imagedata1() const; - inline void set_imagedata1(const ::std::string& value); - inline void set_imagedata1(const char* value); - inline void set_imagedata1(const void* value, size_t size); - inline ::std::string* mutable_imagedata1(); - inline ::std::string* release_imagedata1(); - - // required uint32 step2 = 7; - inline bool has_step2() const; - inline void clear_step2(); - static const int kStep2FieldNumber = 7; - inline ::google::protobuf::uint32 step2() const; - inline void set_step2(::google::protobuf::uint32 value); - - // required uint32 type2 = 8; - inline bool has_type2() const; - inline void clear_type2(); - static const int kType2FieldNumber = 8; - inline ::google::protobuf::uint32 type2() const; - inline void set_type2(::google::protobuf::uint32 value); - - // required bytes imageData2 = 9; - inline bool has_imagedata2() const; - inline void clear_imagedata2(); - static const int kImageData2FieldNumber = 9; - inline const ::std::string& imagedata2() const; - inline void set_imagedata2(const ::std::string& value); - inline void set_imagedata2(const char* value); - inline void set_imagedata2(const void* value, size_t size); - inline ::std::string* mutable_imagedata2(); - inline ::std::string* release_imagedata2(); - - // optional uint32 camera_config = 10; - inline bool has_camera_config() const; - inline void clear_camera_config(); - static const int kCameraConfigFieldNumber = 10; - inline ::google::protobuf::uint32 camera_config() const; - inline void set_camera_config(::google::protobuf::uint32 value); - - // optional uint32 camera_type = 11; - inline bool has_camera_type() const; - inline void clear_camera_type(); - static const int kCameraTypeFieldNumber = 11; - inline ::google::protobuf::uint32 camera_type() const; - inline void set_camera_type(::google::protobuf::uint32 value); - - // optional float roll = 12; - inline bool has_roll() const; - inline void clear_roll(); - static const int kRollFieldNumber = 12; - inline float roll() const; - inline void set_roll(float value); - - // optional float pitch = 13; - inline bool has_pitch() const; - inline void clear_pitch(); - static const int kPitchFieldNumber = 13; - inline float pitch() const; - inline void set_pitch(float value); - - // optional float yaw = 14; - inline bool has_yaw() const; - inline void clear_yaw(); - static const int kYawFieldNumber = 14; - inline float yaw() const; - inline void set_yaw(float value); - - // optional float lon = 15; - inline bool has_lon() const; - inline void clear_lon(); - static const int kLonFieldNumber = 15; - inline float lon() const; - inline void set_lon(float value); - - // optional float lat = 16; - inline bool has_lat() const; - inline void clear_lat(); - static const int kLatFieldNumber = 16; - inline float lat() const; - inline void set_lat(float value); - - // optional float alt = 17; - inline bool has_alt() const; - inline void clear_alt(); - static const int kAltFieldNumber = 17; - inline float alt() const; - inline void set_alt(float value); - - // optional float ground_x = 18; - inline bool has_ground_x() const; - inline void clear_ground_x(); - static const int kGroundXFieldNumber = 18; - inline float ground_x() const; - inline void set_ground_x(float value); - - // optional float ground_y = 19; - inline bool has_ground_y() const; - inline void clear_ground_y(); - static const int kGroundYFieldNumber = 19; - inline float ground_y() const; - inline void set_ground_y(float value); - - // optional float ground_z = 20; - inline bool has_ground_z() const; - inline void clear_ground_z(); - static const int kGroundZFieldNumber = 20; - inline float ground_z() const; - inline void set_ground_z(float value); - - // repeated float camera_matrix = 21; - inline int camera_matrix_size() const; - inline void clear_camera_matrix(); - static const int kCameraMatrixFieldNumber = 21; - inline float camera_matrix(int index) const; - inline void set_camera_matrix(int index, float value); - inline void add_camera_matrix(float value); - inline const ::google::protobuf::RepeatedField< float >& - camera_matrix() const; - inline ::google::protobuf::RepeatedField< float >* - mutable_camera_matrix(); - - // @@protoc_insertion_point(class_scope:px.RGBDImage) - private: - inline void set_has_header(); - inline void clear_has_header(); - inline void set_has_cols(); - inline void clear_has_cols(); - inline void set_has_rows(); - inline void clear_has_rows(); - inline void set_has_step1(); - inline void clear_has_step1(); - inline void set_has_type1(); - inline void clear_has_type1(); - inline void set_has_imagedata1(); - inline void clear_has_imagedata1(); - inline void set_has_step2(); - inline void clear_has_step2(); - inline void set_has_type2(); - inline void clear_has_type2(); - inline void set_has_imagedata2(); - inline void clear_has_imagedata2(); - inline void set_has_camera_config(); - inline void clear_has_camera_config(); - inline void set_has_camera_type(); - inline void clear_has_camera_type(); - inline void set_has_roll(); - inline void clear_has_roll(); - inline void set_has_pitch(); - inline void clear_has_pitch(); - inline void set_has_yaw(); - inline void clear_has_yaw(); - inline void set_has_lon(); - inline void clear_has_lon(); - inline void set_has_lat(); - inline void clear_has_lat(); - inline void set_has_alt(); - inline void clear_has_alt(); - inline void set_has_ground_x(); - inline void clear_has_ground_x(); - inline void set_has_ground_y(); - inline void clear_has_ground_y(); - inline void set_has_ground_z(); - inline void clear_has_ground_z(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::px::HeaderInfo* header_; - ::google::protobuf::uint32 cols_; - ::google::protobuf::uint32 rows_; - ::google::protobuf::uint32 step1_; - ::google::protobuf::uint32 type1_; - ::std::string* imagedata1_; - ::google::protobuf::uint32 step2_; - ::google::protobuf::uint32 type2_; - ::std::string* imagedata2_; - ::google::protobuf::uint32 camera_config_; - ::google::protobuf::uint32 camera_type_; - float roll_; - float pitch_; - float yaw_; - float lon_; - float lat_; - float alt_; - float ground_x_; - float ground_y_; - ::google::protobuf::RepeatedField< float > camera_matrix_; - float ground_z_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(21 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static RGBDImage* default_instance_; -}; -// ------------------------------------------------------------------- - -class Waypoint : public ::google::protobuf::Message { - public: - Waypoint(); - virtual ~Waypoint(); - - Waypoint(const Waypoint& from); - - inline Waypoint& operator=(const Waypoint& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const Waypoint& default_instance(); - - void Swap(Waypoint* other); - - // implements Message ---------------------------------------------- - - Waypoint* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const Waypoint& from); - void MergeFrom(const Waypoint& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required double x = 1; - inline bool has_x() const; - inline void clear_x(); - static const int kXFieldNumber = 1; - inline double x() const; - inline void set_x(double value); - - // required double y = 2; - inline bool has_y() const; - inline void clear_y(); - static const int kYFieldNumber = 2; - inline double y() const; - inline void set_y(double value); - - // optional double z = 3; - inline bool has_z() const; - inline void clear_z(); - static const int kZFieldNumber = 3; - inline double z() const; - inline void set_z(double value); - - // optional double roll = 4; - inline bool has_roll() const; - inline void clear_roll(); - static const int kRollFieldNumber = 4; - inline double roll() const; - inline void set_roll(double value); - - // optional double pitch = 5; - inline bool has_pitch() const; - inline void clear_pitch(); - static const int kPitchFieldNumber = 5; - inline double pitch() const; - inline void set_pitch(double value); - - // optional double yaw = 6; - inline bool has_yaw() const; - inline void clear_yaw(); - static const int kYawFieldNumber = 6; - inline double yaw() const; - inline void set_yaw(double value); - - // @@protoc_insertion_point(class_scope:px.Waypoint) - private: - inline void set_has_x(); - inline void clear_has_x(); - inline void set_has_y(); - inline void clear_has_y(); - inline void set_has_z(); - inline void clear_has_z(); - inline void set_has_roll(); - inline void clear_has_roll(); - inline void set_has_pitch(); - inline void clear_has_pitch(); - inline void set_has_yaw(); - inline void clear_has_yaw(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - double x_; - double y_; - double z_; - double roll_; - double pitch_; - double yaw_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(6 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static Waypoint* default_instance_; -}; -// =================================================================== - - -// =================================================================== - -// HeaderInfo - -// required int32 source_sysid = 1; -inline bool HeaderInfo::has_source_sysid() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void HeaderInfo::set_has_source_sysid() { - _has_bits_[0] |= 0x00000001u; -} -inline void HeaderInfo::clear_has_source_sysid() { - _has_bits_[0] &= ~0x00000001u; -} -inline void HeaderInfo::clear_source_sysid() { - source_sysid_ = 0; - clear_has_source_sysid(); -} -inline ::google::protobuf::int32 HeaderInfo::source_sysid() const { - return source_sysid_; -} -inline void HeaderInfo::set_source_sysid(::google::protobuf::int32 value) { - set_has_source_sysid(); - source_sysid_ = value; -} - -// required int32 source_compid = 2; -inline bool HeaderInfo::has_source_compid() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void HeaderInfo::set_has_source_compid() { - _has_bits_[0] |= 0x00000002u; -} -inline void HeaderInfo::clear_has_source_compid() { - _has_bits_[0] &= ~0x00000002u; -} -inline void HeaderInfo::clear_source_compid() { - source_compid_ = 0; - clear_has_source_compid(); -} -inline ::google::protobuf::int32 HeaderInfo::source_compid() const { - return source_compid_; -} -inline void HeaderInfo::set_source_compid(::google::protobuf::int32 value) { - set_has_source_compid(); - source_compid_ = value; -} - -// required double timestamp = 3; -inline bool HeaderInfo::has_timestamp() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void HeaderInfo::set_has_timestamp() { - _has_bits_[0] |= 0x00000004u; -} -inline void HeaderInfo::clear_has_timestamp() { - _has_bits_[0] &= ~0x00000004u; -} -inline void HeaderInfo::clear_timestamp() { - timestamp_ = 0; - clear_has_timestamp(); -} -inline double HeaderInfo::timestamp() const { - return timestamp_; -} -inline void HeaderInfo::set_timestamp(double value) { - set_has_timestamp(); - timestamp_ = value; -} - -// ------------------------------------------------------------------- - -// GLOverlay - -// required .px.HeaderInfo header = 1; -inline bool GLOverlay::has_header() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void GLOverlay::set_has_header() { - _has_bits_[0] |= 0x00000001u; -} -inline void GLOverlay::clear_has_header() { - _has_bits_[0] &= ~0x00000001u; -} -inline void GLOverlay::clear_header() { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - clear_has_header(); -} -inline const ::px::HeaderInfo& GLOverlay::header() const { - return header_ != NULL ? *header_ : *default_instance_->header_; -} -inline ::px::HeaderInfo* GLOverlay::mutable_header() { - set_has_header(); - if (header_ == NULL) header_ = new ::px::HeaderInfo; - return header_; -} -inline ::px::HeaderInfo* GLOverlay::release_header() { - clear_has_header(); - ::px::HeaderInfo* temp = header_; - header_ = NULL; - return temp; -} - -// optional string name = 2; -inline bool GLOverlay::has_name() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void GLOverlay::set_has_name() { - _has_bits_[0] |= 0x00000002u; -} -inline void GLOverlay::clear_has_name() { - _has_bits_[0] &= ~0x00000002u; -} -inline void GLOverlay::clear_name() { - if (name_ != &::google::protobuf::internal::kEmptyString) { - name_->clear(); - } - clear_has_name(); -} -inline const ::std::string& GLOverlay::name() const { - return *name_; -} -inline void GLOverlay::set_name(const ::std::string& value) { - set_has_name(); - if (name_ == &::google::protobuf::internal::kEmptyString) { - name_ = new ::std::string; - } - name_->assign(value); -} -inline void GLOverlay::set_name(const char* value) { - set_has_name(); - if (name_ == &::google::protobuf::internal::kEmptyString) { - name_ = new ::std::string; - } - name_->assign(value); -} -inline void GLOverlay::set_name(const char* value, size_t size) { - set_has_name(); - if (name_ == &::google::protobuf::internal::kEmptyString) { - name_ = new ::std::string; - } - name_->assign(reinterpret_cast(value), size); -} -inline ::std::string* GLOverlay::mutable_name() { - set_has_name(); - if (name_ == &::google::protobuf::internal::kEmptyString) { - name_ = new ::std::string; - } - return name_; -} -inline ::std::string* GLOverlay::release_name() { - clear_has_name(); - if (name_ == &::google::protobuf::internal::kEmptyString) { - return NULL; - } else { - ::std::string* temp = name_; - name_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - return temp; - } -} - -// optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3; -inline bool GLOverlay::has_coordinateframetype() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void GLOverlay::set_has_coordinateframetype() { - _has_bits_[0] |= 0x00000004u; -} -inline void GLOverlay::clear_has_coordinateframetype() { - _has_bits_[0] &= ~0x00000004u; -} -inline void GLOverlay::clear_coordinateframetype() { - coordinateframetype_ = 0; - clear_has_coordinateframetype(); -} -inline ::px::GLOverlay_CoordinateFrameType GLOverlay::coordinateframetype() const { - return static_cast< ::px::GLOverlay_CoordinateFrameType >(coordinateframetype_); -} -inline void GLOverlay::set_coordinateframetype(::px::GLOverlay_CoordinateFrameType value) { - GOOGLE_DCHECK(::px::GLOverlay_CoordinateFrameType_IsValid(value)); - set_has_coordinateframetype(); - coordinateframetype_ = value; -} - -// optional double origin_x = 4; -inline bool GLOverlay::has_origin_x() const { - return (_has_bits_[0] & 0x00000008u) != 0; -} -inline void GLOverlay::set_has_origin_x() { - _has_bits_[0] |= 0x00000008u; -} -inline void GLOverlay::clear_has_origin_x() { - _has_bits_[0] &= ~0x00000008u; -} -inline void GLOverlay::clear_origin_x() { - origin_x_ = 0; - clear_has_origin_x(); -} -inline double GLOverlay::origin_x() const { - return origin_x_; -} -inline void GLOverlay::set_origin_x(double value) { - set_has_origin_x(); - origin_x_ = value; -} - -// optional double origin_y = 5; -inline bool GLOverlay::has_origin_y() const { - return (_has_bits_[0] & 0x00000010u) != 0; -} -inline void GLOverlay::set_has_origin_y() { - _has_bits_[0] |= 0x00000010u; -} -inline void GLOverlay::clear_has_origin_y() { - _has_bits_[0] &= ~0x00000010u; -} -inline void GLOverlay::clear_origin_y() { - origin_y_ = 0; - clear_has_origin_y(); -} -inline double GLOverlay::origin_y() const { - return origin_y_; -} -inline void GLOverlay::set_origin_y(double value) { - set_has_origin_y(); - origin_y_ = value; -} - -// optional double origin_z = 6; -inline bool GLOverlay::has_origin_z() const { - return (_has_bits_[0] & 0x00000020u) != 0; -} -inline void GLOverlay::set_has_origin_z() { - _has_bits_[0] |= 0x00000020u; -} -inline void GLOverlay::clear_has_origin_z() { - _has_bits_[0] &= ~0x00000020u; -} -inline void GLOverlay::clear_origin_z() { - origin_z_ = 0; - clear_has_origin_z(); -} -inline double GLOverlay::origin_z() const { - return origin_z_; -} -inline void GLOverlay::set_origin_z(double value) { - set_has_origin_z(); - origin_z_ = value; -} - -// optional bytes data = 7; -inline bool GLOverlay::has_data() const { - return (_has_bits_[0] & 0x00000040u) != 0; -} -inline void GLOverlay::set_has_data() { - _has_bits_[0] |= 0x00000040u; -} -inline void GLOverlay::clear_has_data() { - _has_bits_[0] &= ~0x00000040u; -} -inline void GLOverlay::clear_data() { - if (data_ != &::google::protobuf::internal::kEmptyString) { - data_->clear(); - } - clear_has_data(); -} -inline const ::std::string& GLOverlay::data() const { - return *data_; -} -inline void GLOverlay::set_data(const ::std::string& value) { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - data_->assign(value); -} -inline void GLOverlay::set_data(const char* value) { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - data_->assign(value); -} -inline void GLOverlay::set_data(const void* value, size_t size) { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - data_->assign(reinterpret_cast(value), size); -} -inline ::std::string* GLOverlay::mutable_data() { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - return data_; -} -inline ::std::string* GLOverlay::release_data() { - clear_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - return NULL; - } else { - ::std::string* temp = data_; - data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - return temp; - } -} - -// ------------------------------------------------------------------- - -// Obstacle - -// optional float x = 1; -inline bool Obstacle::has_x() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void Obstacle::set_has_x() { - _has_bits_[0] |= 0x00000001u; -} -inline void Obstacle::clear_has_x() { - _has_bits_[0] &= ~0x00000001u; -} -inline void Obstacle::clear_x() { - x_ = 0; - clear_has_x(); -} -inline float Obstacle::x() const { - return x_; -} -inline void Obstacle::set_x(float value) { - set_has_x(); - x_ = value; -} - -// optional float y = 2; -inline bool Obstacle::has_y() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void Obstacle::set_has_y() { - _has_bits_[0] |= 0x00000002u; -} -inline void Obstacle::clear_has_y() { - _has_bits_[0] &= ~0x00000002u; -} -inline void Obstacle::clear_y() { - y_ = 0; - clear_has_y(); -} -inline float Obstacle::y() const { - return y_; -} -inline void Obstacle::set_y(float value) { - set_has_y(); - y_ = value; -} - -// optional float z = 3; -inline bool Obstacle::has_z() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void Obstacle::set_has_z() { - _has_bits_[0] |= 0x00000004u; -} -inline void Obstacle::clear_has_z() { - _has_bits_[0] &= ~0x00000004u; -} -inline void Obstacle::clear_z() { - z_ = 0; - clear_has_z(); -} -inline float Obstacle::z() const { - return z_; -} -inline void Obstacle::set_z(float value) { - set_has_z(); - z_ = value; -} - -// optional float length = 4; -inline bool Obstacle::has_length() const { - return (_has_bits_[0] & 0x00000008u) != 0; -} -inline void Obstacle::set_has_length() { - _has_bits_[0] |= 0x00000008u; -} -inline void Obstacle::clear_has_length() { - _has_bits_[0] &= ~0x00000008u; -} -inline void Obstacle::clear_length() { - length_ = 0; - clear_has_length(); -} -inline float Obstacle::length() const { - return length_; -} -inline void Obstacle::set_length(float value) { - set_has_length(); - length_ = value; -} - -// optional float width = 5; -inline bool Obstacle::has_width() const { - return (_has_bits_[0] & 0x00000010u) != 0; -} -inline void Obstacle::set_has_width() { - _has_bits_[0] |= 0x00000010u; -} -inline void Obstacle::clear_has_width() { - _has_bits_[0] &= ~0x00000010u; -} -inline void Obstacle::clear_width() { - width_ = 0; - clear_has_width(); -} -inline float Obstacle::width() const { - return width_; -} -inline void Obstacle::set_width(float value) { - set_has_width(); - width_ = value; -} - -// optional float height = 6; -inline bool Obstacle::has_height() const { - return (_has_bits_[0] & 0x00000020u) != 0; -} -inline void Obstacle::set_has_height() { - _has_bits_[0] |= 0x00000020u; -} -inline void Obstacle::clear_has_height() { - _has_bits_[0] &= ~0x00000020u; -} -inline void Obstacle::clear_height() { - height_ = 0; - clear_has_height(); -} -inline float Obstacle::height() const { - return height_; -} -inline void Obstacle::set_height(float value) { - set_has_height(); - height_ = value; -} - -// ------------------------------------------------------------------- - -// ObstacleList - -// required .px.HeaderInfo header = 1; -inline bool ObstacleList::has_header() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void ObstacleList::set_has_header() { - _has_bits_[0] |= 0x00000001u; -} -inline void ObstacleList::clear_has_header() { - _has_bits_[0] &= ~0x00000001u; -} -inline void ObstacleList::clear_header() { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - clear_has_header(); -} -inline const ::px::HeaderInfo& ObstacleList::header() const { - return header_ != NULL ? *header_ : *default_instance_->header_; -} -inline ::px::HeaderInfo* ObstacleList::mutable_header() { - set_has_header(); - if (header_ == NULL) header_ = new ::px::HeaderInfo; - return header_; -} -inline ::px::HeaderInfo* ObstacleList::release_header() { - clear_has_header(); - ::px::HeaderInfo* temp = header_; - header_ = NULL; - return temp; -} - -// repeated .px.Obstacle obstacles = 2; -inline int ObstacleList::obstacles_size() const { - return obstacles_.size(); -} -inline void ObstacleList::clear_obstacles() { - obstacles_.Clear(); -} -inline const ::px::Obstacle& ObstacleList::obstacles(int index) const { - return obstacles_.Get(index); -} -inline ::px::Obstacle* ObstacleList::mutable_obstacles(int index) { - return obstacles_.Mutable(index); -} -inline ::px::Obstacle* ObstacleList::add_obstacles() { - return obstacles_.Add(); -} -inline const ::google::protobuf::RepeatedPtrField< ::px::Obstacle >& -ObstacleList::obstacles() const { - return obstacles_; -} -inline ::google::protobuf::RepeatedPtrField< ::px::Obstacle >* -ObstacleList::mutable_obstacles() { - return &obstacles_; -} - -// ------------------------------------------------------------------- - -// ObstacleMap - -// required .px.HeaderInfo header = 1; -inline bool ObstacleMap::has_header() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void ObstacleMap::set_has_header() { - _has_bits_[0] |= 0x00000001u; -} -inline void ObstacleMap::clear_has_header() { - _has_bits_[0] &= ~0x00000001u; -} -inline void ObstacleMap::clear_header() { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - clear_has_header(); -} -inline const ::px::HeaderInfo& ObstacleMap::header() const { - return header_ != NULL ? *header_ : *default_instance_->header_; -} -inline ::px::HeaderInfo* ObstacleMap::mutable_header() { - set_has_header(); - if (header_ == NULL) header_ = new ::px::HeaderInfo; - return header_; -} -inline ::px::HeaderInfo* ObstacleMap::release_header() { - clear_has_header(); - ::px::HeaderInfo* temp = header_; - header_ = NULL; - return temp; -} - -// required int32 type = 2; -inline bool ObstacleMap::has_type() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void ObstacleMap::set_has_type() { - _has_bits_[0] |= 0x00000002u; -} -inline void ObstacleMap::clear_has_type() { - _has_bits_[0] &= ~0x00000002u; -} -inline void ObstacleMap::clear_type() { - type_ = 0; - clear_has_type(); -} -inline ::google::protobuf::int32 ObstacleMap::type() const { - return type_; -} -inline void ObstacleMap::set_type(::google::protobuf::int32 value) { - set_has_type(); - type_ = value; -} - -// optional float resolution = 3; -inline bool ObstacleMap::has_resolution() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void ObstacleMap::set_has_resolution() { - _has_bits_[0] |= 0x00000004u; -} -inline void ObstacleMap::clear_has_resolution() { - _has_bits_[0] &= ~0x00000004u; -} -inline void ObstacleMap::clear_resolution() { - resolution_ = 0; - clear_has_resolution(); -} -inline float ObstacleMap::resolution() const { - return resolution_; -} -inline void ObstacleMap::set_resolution(float value) { - set_has_resolution(); - resolution_ = value; -} - -// optional int32 rows = 4; -inline bool ObstacleMap::has_rows() const { - return (_has_bits_[0] & 0x00000008u) != 0; -} -inline void ObstacleMap::set_has_rows() { - _has_bits_[0] |= 0x00000008u; -} -inline void ObstacleMap::clear_has_rows() { - _has_bits_[0] &= ~0x00000008u; -} -inline void ObstacleMap::clear_rows() { - rows_ = 0; - clear_has_rows(); -} -inline ::google::protobuf::int32 ObstacleMap::rows() const { - return rows_; -} -inline void ObstacleMap::set_rows(::google::protobuf::int32 value) { - set_has_rows(); - rows_ = value; -} - -// optional int32 cols = 5; -inline bool ObstacleMap::has_cols() const { - return (_has_bits_[0] & 0x00000010u) != 0; -} -inline void ObstacleMap::set_has_cols() { - _has_bits_[0] |= 0x00000010u; -} -inline void ObstacleMap::clear_has_cols() { - _has_bits_[0] &= ~0x00000010u; -} -inline void ObstacleMap::clear_cols() { - cols_ = 0; - clear_has_cols(); -} -inline ::google::protobuf::int32 ObstacleMap::cols() const { - return cols_; -} -inline void ObstacleMap::set_cols(::google::protobuf::int32 value) { - set_has_cols(); - cols_ = value; -} - -// optional int32 mapR0 = 6; -inline bool ObstacleMap::has_mapr0() const { - return (_has_bits_[0] & 0x00000020u) != 0; -} -inline void ObstacleMap::set_has_mapr0() { - _has_bits_[0] |= 0x00000020u; -} -inline void ObstacleMap::clear_has_mapr0() { - _has_bits_[0] &= ~0x00000020u; -} -inline void ObstacleMap::clear_mapr0() { - mapr0_ = 0; - clear_has_mapr0(); -} -inline ::google::protobuf::int32 ObstacleMap::mapr0() const { - return mapr0_; -} -inline void ObstacleMap::set_mapr0(::google::protobuf::int32 value) { - set_has_mapr0(); - mapr0_ = value; -} - -// optional int32 mapC0 = 7; -inline bool ObstacleMap::has_mapc0() const { - return (_has_bits_[0] & 0x00000040u) != 0; -} -inline void ObstacleMap::set_has_mapc0() { - _has_bits_[0] |= 0x00000040u; -} -inline void ObstacleMap::clear_has_mapc0() { - _has_bits_[0] &= ~0x00000040u; -} -inline void ObstacleMap::clear_mapc0() { - mapc0_ = 0; - clear_has_mapc0(); -} -inline ::google::protobuf::int32 ObstacleMap::mapc0() const { - return mapc0_; -} -inline void ObstacleMap::set_mapc0(::google::protobuf::int32 value) { - set_has_mapc0(); - mapc0_ = value; -} - -// optional int32 arrayR0 = 8; -inline bool ObstacleMap::has_arrayr0() const { - return (_has_bits_[0] & 0x00000080u) != 0; -} -inline void ObstacleMap::set_has_arrayr0() { - _has_bits_[0] |= 0x00000080u; -} -inline void ObstacleMap::clear_has_arrayr0() { - _has_bits_[0] &= ~0x00000080u; -} -inline void ObstacleMap::clear_arrayr0() { - arrayr0_ = 0; - clear_has_arrayr0(); -} -inline ::google::protobuf::int32 ObstacleMap::arrayr0() const { - return arrayr0_; -} -inline void ObstacleMap::set_arrayr0(::google::protobuf::int32 value) { - set_has_arrayr0(); - arrayr0_ = value; -} - -// optional int32 arrayC0 = 9; -inline bool ObstacleMap::has_arrayc0() const { - return (_has_bits_[0] & 0x00000100u) != 0; -} -inline void ObstacleMap::set_has_arrayc0() { - _has_bits_[0] |= 0x00000100u; -} -inline void ObstacleMap::clear_has_arrayc0() { - _has_bits_[0] &= ~0x00000100u; -} -inline void ObstacleMap::clear_arrayc0() { - arrayc0_ = 0; - clear_has_arrayc0(); -} -inline ::google::protobuf::int32 ObstacleMap::arrayc0() const { - return arrayc0_; -} -inline void ObstacleMap::set_arrayc0(::google::protobuf::int32 value) { - set_has_arrayc0(); - arrayc0_ = value; -} - -// optional bytes data = 10; -inline bool ObstacleMap::has_data() const { - return (_has_bits_[0] & 0x00000200u) != 0; -} -inline void ObstacleMap::set_has_data() { - _has_bits_[0] |= 0x00000200u; -} -inline void ObstacleMap::clear_has_data() { - _has_bits_[0] &= ~0x00000200u; -} -inline void ObstacleMap::clear_data() { - if (data_ != &::google::protobuf::internal::kEmptyString) { - data_->clear(); - } - clear_has_data(); -} -inline const ::std::string& ObstacleMap::data() const { - return *data_; -} -inline void ObstacleMap::set_data(const ::std::string& value) { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - data_->assign(value); -} -inline void ObstacleMap::set_data(const char* value) { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - data_->assign(value); -} -inline void ObstacleMap::set_data(const void* value, size_t size) { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - data_->assign(reinterpret_cast(value), size); -} -inline ::std::string* ObstacleMap::mutable_data() { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - return data_; -} -inline ::std::string* ObstacleMap::release_data() { - clear_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - return NULL; - } else { - ::std::string* temp = data_; - data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - return temp; - } -} - -// ------------------------------------------------------------------- - -// Path - -// required .px.HeaderInfo header = 1; -inline bool Path::has_header() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void Path::set_has_header() { - _has_bits_[0] |= 0x00000001u; -} -inline void Path::clear_has_header() { - _has_bits_[0] &= ~0x00000001u; -} -inline void Path::clear_header() { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - clear_has_header(); -} -inline const ::px::HeaderInfo& Path::header() const { - return header_ != NULL ? *header_ : *default_instance_->header_; -} -inline ::px::HeaderInfo* Path::mutable_header() { - set_has_header(); - if (header_ == NULL) header_ = new ::px::HeaderInfo; - return header_; -} -inline ::px::HeaderInfo* Path::release_header() { - clear_has_header(); - ::px::HeaderInfo* temp = header_; - header_ = NULL; - return temp; -} - -// repeated .px.Waypoint waypoints = 2; -inline int Path::waypoints_size() const { - return waypoints_.size(); -} -inline void Path::clear_waypoints() { - waypoints_.Clear(); -} -inline const ::px::Waypoint& Path::waypoints(int index) const { - return waypoints_.Get(index); -} -inline ::px::Waypoint* Path::mutable_waypoints(int index) { - return waypoints_.Mutable(index); -} -inline ::px::Waypoint* Path::add_waypoints() { - return waypoints_.Add(); -} -inline const ::google::protobuf::RepeatedPtrField< ::px::Waypoint >& -Path::waypoints() const { - return waypoints_; -} -inline ::google::protobuf::RepeatedPtrField< ::px::Waypoint >* -Path::mutable_waypoints() { - return &waypoints_; -} - -// ------------------------------------------------------------------- - -// PointCloudXYZI_PointXYZI - -// required float x = 1; -inline bool PointCloudXYZI_PointXYZI::has_x() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void PointCloudXYZI_PointXYZI::set_has_x() { - _has_bits_[0] |= 0x00000001u; -} -inline void PointCloudXYZI_PointXYZI::clear_has_x() { - _has_bits_[0] &= ~0x00000001u; -} -inline void PointCloudXYZI_PointXYZI::clear_x() { - x_ = 0; - clear_has_x(); -} -inline float PointCloudXYZI_PointXYZI::x() const { - return x_; -} -inline void PointCloudXYZI_PointXYZI::set_x(float value) { - set_has_x(); - x_ = value; -} - -// required float y = 2; -inline bool PointCloudXYZI_PointXYZI::has_y() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void PointCloudXYZI_PointXYZI::set_has_y() { - _has_bits_[0] |= 0x00000002u; -} -inline void PointCloudXYZI_PointXYZI::clear_has_y() { - _has_bits_[0] &= ~0x00000002u; -} -inline void PointCloudXYZI_PointXYZI::clear_y() { - y_ = 0; - clear_has_y(); -} -inline float PointCloudXYZI_PointXYZI::y() const { - return y_; -} -inline void PointCloudXYZI_PointXYZI::set_y(float value) { - set_has_y(); - y_ = value; -} - -// required float z = 3; -inline bool PointCloudXYZI_PointXYZI::has_z() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void PointCloudXYZI_PointXYZI::set_has_z() { - _has_bits_[0] |= 0x00000004u; -} -inline void PointCloudXYZI_PointXYZI::clear_has_z() { - _has_bits_[0] &= ~0x00000004u; -} -inline void PointCloudXYZI_PointXYZI::clear_z() { - z_ = 0; - clear_has_z(); -} -inline float PointCloudXYZI_PointXYZI::z() const { - return z_; -} -inline void PointCloudXYZI_PointXYZI::set_z(float value) { - set_has_z(); - z_ = value; -} - -// required float intensity = 4; -inline bool PointCloudXYZI_PointXYZI::has_intensity() const { - return (_has_bits_[0] & 0x00000008u) != 0; -} -inline void PointCloudXYZI_PointXYZI::set_has_intensity() { - _has_bits_[0] |= 0x00000008u; -} -inline void PointCloudXYZI_PointXYZI::clear_has_intensity() { - _has_bits_[0] &= ~0x00000008u; -} -inline void PointCloudXYZI_PointXYZI::clear_intensity() { - intensity_ = 0; - clear_has_intensity(); -} -inline float PointCloudXYZI_PointXYZI::intensity() const { - return intensity_; -} -inline void PointCloudXYZI_PointXYZI::set_intensity(float value) { - set_has_intensity(); - intensity_ = value; -} - -// ------------------------------------------------------------------- - -// PointCloudXYZI - -// required .px.HeaderInfo header = 1; -inline bool PointCloudXYZI::has_header() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void PointCloudXYZI::set_has_header() { - _has_bits_[0] |= 0x00000001u; -} -inline void PointCloudXYZI::clear_has_header() { - _has_bits_[0] &= ~0x00000001u; -} -inline void PointCloudXYZI::clear_header() { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - clear_has_header(); -} -inline const ::px::HeaderInfo& PointCloudXYZI::header() const { - return header_ != NULL ? *header_ : *default_instance_->header_; -} -inline ::px::HeaderInfo* PointCloudXYZI::mutable_header() { - set_has_header(); - if (header_ == NULL) header_ = new ::px::HeaderInfo; - return header_; -} -inline ::px::HeaderInfo* PointCloudXYZI::release_header() { - clear_has_header(); - ::px::HeaderInfo* temp = header_; - header_ = NULL; - return temp; -} - -// repeated .px.PointCloudXYZI.PointXYZI points = 2; -inline int PointCloudXYZI::points_size() const { - return points_.size(); -} -inline void PointCloudXYZI::clear_points() { - points_.Clear(); -} -inline const ::px::PointCloudXYZI_PointXYZI& PointCloudXYZI::points(int index) const { - return points_.Get(index); -} -inline ::px::PointCloudXYZI_PointXYZI* PointCloudXYZI::mutable_points(int index) { - return points_.Mutable(index); -} -inline ::px::PointCloudXYZI_PointXYZI* PointCloudXYZI::add_points() { - return points_.Add(); -} -inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >& -PointCloudXYZI::points() const { - return points_; -} -inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >* -PointCloudXYZI::mutable_points() { - return &points_; -} - -// ------------------------------------------------------------------- - -// PointCloudXYZRGB_PointXYZRGB - -// required float x = 1; -inline bool PointCloudXYZRGB_PointXYZRGB::has_x() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_has_x() { - _has_bits_[0] |= 0x00000001u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_has_x() { - _has_bits_[0] &= ~0x00000001u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_x() { - x_ = 0; - clear_has_x(); -} -inline float PointCloudXYZRGB_PointXYZRGB::x() const { - return x_; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_x(float value) { - set_has_x(); - x_ = value; -} - -// required float y = 2; -inline bool PointCloudXYZRGB_PointXYZRGB::has_y() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_has_y() { - _has_bits_[0] |= 0x00000002u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_has_y() { - _has_bits_[0] &= ~0x00000002u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_y() { - y_ = 0; - clear_has_y(); -} -inline float PointCloudXYZRGB_PointXYZRGB::y() const { - return y_; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_y(float value) { - set_has_y(); - y_ = value; -} - -// required float z = 3; -inline bool PointCloudXYZRGB_PointXYZRGB::has_z() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_has_z() { - _has_bits_[0] |= 0x00000004u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_has_z() { - _has_bits_[0] &= ~0x00000004u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_z() { - z_ = 0; - clear_has_z(); -} -inline float PointCloudXYZRGB_PointXYZRGB::z() const { - return z_; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_z(float value) { - set_has_z(); - z_ = value; -} - -// required float rgb = 4; -inline bool PointCloudXYZRGB_PointXYZRGB::has_rgb() const { - return (_has_bits_[0] & 0x00000008u) != 0; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_has_rgb() { - _has_bits_[0] |= 0x00000008u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_has_rgb() { - _has_bits_[0] &= ~0x00000008u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_rgb() { - rgb_ = 0; - clear_has_rgb(); -} -inline float PointCloudXYZRGB_PointXYZRGB::rgb() const { - return rgb_; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_rgb(float value) { - set_has_rgb(); - rgb_ = value; -} - -// ------------------------------------------------------------------- - -// PointCloudXYZRGB - -// required .px.HeaderInfo header = 1; -inline bool PointCloudXYZRGB::has_header() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void PointCloudXYZRGB::set_has_header() { - _has_bits_[0] |= 0x00000001u; -} -inline void PointCloudXYZRGB::clear_has_header() { - _has_bits_[0] &= ~0x00000001u; -} -inline void PointCloudXYZRGB::clear_header() { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - clear_has_header(); -} -inline const ::px::HeaderInfo& PointCloudXYZRGB::header() const { - return header_ != NULL ? *header_ : *default_instance_->header_; -} -inline ::px::HeaderInfo* PointCloudXYZRGB::mutable_header() { - set_has_header(); - if (header_ == NULL) header_ = new ::px::HeaderInfo; - return header_; -} -inline ::px::HeaderInfo* PointCloudXYZRGB::release_header() { - clear_has_header(); - ::px::HeaderInfo* temp = header_; - header_ = NULL; - return temp; -} - -// repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2; -inline int PointCloudXYZRGB::points_size() const { - return points_.size(); -} -inline void PointCloudXYZRGB::clear_points() { - points_.Clear(); -} -inline const ::px::PointCloudXYZRGB_PointXYZRGB& PointCloudXYZRGB::points(int index) const { - return points_.Get(index); -} -inline ::px::PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB::mutable_points(int index) { - return points_.Mutable(index); -} -inline ::px::PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB::add_points() { - return points_.Add(); -} -inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >& -PointCloudXYZRGB::points() const { - return points_; -} -inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >* -PointCloudXYZRGB::mutable_points() { - return &points_; -} - -// ------------------------------------------------------------------- - -// RGBDImage - -// required .px.HeaderInfo header = 1; -inline bool RGBDImage::has_header() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void RGBDImage::set_has_header() { - _has_bits_[0] |= 0x00000001u; -} -inline void RGBDImage::clear_has_header() { - _has_bits_[0] &= ~0x00000001u; -} -inline void RGBDImage::clear_header() { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - clear_has_header(); -} -inline const ::px::HeaderInfo& RGBDImage::header() const { - return header_ != NULL ? *header_ : *default_instance_->header_; -} -inline ::px::HeaderInfo* RGBDImage::mutable_header() { - set_has_header(); - if (header_ == NULL) header_ = new ::px::HeaderInfo; - return header_; -} -inline ::px::HeaderInfo* RGBDImage::release_header() { - clear_has_header(); - ::px::HeaderInfo* temp = header_; - header_ = NULL; - return temp; -} - -// required uint32 cols = 2; -inline bool RGBDImage::has_cols() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void RGBDImage::set_has_cols() { - _has_bits_[0] |= 0x00000002u; -} -inline void RGBDImage::clear_has_cols() { - _has_bits_[0] &= ~0x00000002u; -} -inline void RGBDImage::clear_cols() { - cols_ = 0u; - clear_has_cols(); -} -inline ::google::protobuf::uint32 RGBDImage::cols() const { - return cols_; -} -inline void RGBDImage::set_cols(::google::protobuf::uint32 value) { - set_has_cols(); - cols_ = value; -} - -// required uint32 rows = 3; -inline bool RGBDImage::has_rows() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void RGBDImage::set_has_rows() { - _has_bits_[0] |= 0x00000004u; -} -inline void RGBDImage::clear_has_rows() { - _has_bits_[0] &= ~0x00000004u; -} -inline void RGBDImage::clear_rows() { - rows_ = 0u; - clear_has_rows(); -} -inline ::google::protobuf::uint32 RGBDImage::rows() const { - return rows_; -} -inline void RGBDImage::set_rows(::google::protobuf::uint32 value) { - set_has_rows(); - rows_ = value; -} - -// required uint32 step1 = 4; -inline bool RGBDImage::has_step1() const { - return (_has_bits_[0] & 0x00000008u) != 0; -} -inline void RGBDImage::set_has_step1() { - _has_bits_[0] |= 0x00000008u; -} -inline void RGBDImage::clear_has_step1() { - _has_bits_[0] &= ~0x00000008u; -} -inline void RGBDImage::clear_step1() { - step1_ = 0u; - clear_has_step1(); -} -inline ::google::protobuf::uint32 RGBDImage::step1() const { - return step1_; -} -inline void RGBDImage::set_step1(::google::protobuf::uint32 value) { - set_has_step1(); - step1_ = value; -} - -// required uint32 type1 = 5; -inline bool RGBDImage::has_type1() const { - return (_has_bits_[0] & 0x00000010u) != 0; -} -inline void RGBDImage::set_has_type1() { - _has_bits_[0] |= 0x00000010u; -} -inline void RGBDImage::clear_has_type1() { - _has_bits_[0] &= ~0x00000010u; -} -inline void RGBDImage::clear_type1() { - type1_ = 0u; - clear_has_type1(); -} -inline ::google::protobuf::uint32 RGBDImage::type1() const { - return type1_; -} -inline void RGBDImage::set_type1(::google::protobuf::uint32 value) { - set_has_type1(); - type1_ = value; -} - -// required bytes imageData1 = 6; -inline bool RGBDImage::has_imagedata1() const { - return (_has_bits_[0] & 0x00000020u) != 0; -} -inline void RGBDImage::set_has_imagedata1() { - _has_bits_[0] |= 0x00000020u; -} -inline void RGBDImage::clear_has_imagedata1() { - _has_bits_[0] &= ~0x00000020u; -} -inline void RGBDImage::clear_imagedata1() { - if (imagedata1_ != &::google::protobuf::internal::kEmptyString) { - imagedata1_->clear(); - } - clear_has_imagedata1(); -} -inline const ::std::string& RGBDImage::imagedata1() const { - return *imagedata1_; -} -inline void RGBDImage::set_imagedata1(const ::std::string& value) { - set_has_imagedata1(); - if (imagedata1_ == &::google::protobuf::internal::kEmptyString) { - imagedata1_ = new ::std::string; - } - imagedata1_->assign(value); -} -inline void RGBDImage::set_imagedata1(const char* value) { - set_has_imagedata1(); - if (imagedata1_ == &::google::protobuf::internal::kEmptyString) { - imagedata1_ = new ::std::string; - } - imagedata1_->assign(value); -} -inline void RGBDImage::set_imagedata1(const void* value, size_t size) { - set_has_imagedata1(); - if (imagedata1_ == &::google::protobuf::internal::kEmptyString) { - imagedata1_ = new ::std::string; - } - imagedata1_->assign(reinterpret_cast(value), size); -} -inline ::std::string* RGBDImage::mutable_imagedata1() { - set_has_imagedata1(); - if (imagedata1_ == &::google::protobuf::internal::kEmptyString) { - imagedata1_ = new ::std::string; - } - return imagedata1_; -} -inline ::std::string* RGBDImage::release_imagedata1() { - clear_has_imagedata1(); - if (imagedata1_ == &::google::protobuf::internal::kEmptyString) { - return NULL; - } else { - ::std::string* temp = imagedata1_; - imagedata1_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - return temp; - } -} - -// required uint32 step2 = 7; -inline bool RGBDImage::has_step2() const { - return (_has_bits_[0] & 0x00000040u) != 0; -} -inline void RGBDImage::set_has_step2() { - _has_bits_[0] |= 0x00000040u; -} -inline void RGBDImage::clear_has_step2() { - _has_bits_[0] &= ~0x00000040u; -} -inline void RGBDImage::clear_step2() { - step2_ = 0u; - clear_has_step2(); -} -inline ::google::protobuf::uint32 RGBDImage::step2() const { - return step2_; -} -inline void RGBDImage::set_step2(::google::protobuf::uint32 value) { - set_has_step2(); - step2_ = value; -} - -// required uint32 type2 = 8; -inline bool RGBDImage::has_type2() const { - return (_has_bits_[0] & 0x00000080u) != 0; -} -inline void RGBDImage::set_has_type2() { - _has_bits_[0] |= 0x00000080u; -} -inline void RGBDImage::clear_has_type2() { - _has_bits_[0] &= ~0x00000080u; -} -inline void RGBDImage::clear_type2() { - type2_ = 0u; - clear_has_type2(); -} -inline ::google::protobuf::uint32 RGBDImage::type2() const { - return type2_; -} -inline void RGBDImage::set_type2(::google::protobuf::uint32 value) { - set_has_type2(); - type2_ = value; -} - -// required bytes imageData2 = 9; -inline bool RGBDImage::has_imagedata2() const { - return (_has_bits_[0] & 0x00000100u) != 0; -} -inline void RGBDImage::set_has_imagedata2() { - _has_bits_[0] |= 0x00000100u; -} -inline void RGBDImage::clear_has_imagedata2() { - _has_bits_[0] &= ~0x00000100u; -} -inline void RGBDImage::clear_imagedata2() { - if (imagedata2_ != &::google::protobuf::internal::kEmptyString) { - imagedata2_->clear(); - } - clear_has_imagedata2(); -} -inline const ::std::string& RGBDImage::imagedata2() const { - return *imagedata2_; -} -inline void RGBDImage::set_imagedata2(const ::std::string& value) { - set_has_imagedata2(); - if (imagedata2_ == &::google::protobuf::internal::kEmptyString) { - imagedata2_ = new ::std::string; - } - imagedata2_->assign(value); -} -inline void RGBDImage::set_imagedata2(const char* value) { - set_has_imagedata2(); - if (imagedata2_ == &::google::protobuf::internal::kEmptyString) { - imagedata2_ = new ::std::string; - } - imagedata2_->assign(value); -} -inline void RGBDImage::set_imagedata2(const void* value, size_t size) { - set_has_imagedata2(); - if (imagedata2_ == &::google::protobuf::internal::kEmptyString) { - imagedata2_ = new ::std::string; - } - imagedata2_->assign(reinterpret_cast(value), size); -} -inline ::std::string* RGBDImage::mutable_imagedata2() { - set_has_imagedata2(); - if (imagedata2_ == &::google::protobuf::internal::kEmptyString) { - imagedata2_ = new ::std::string; - } - return imagedata2_; -} -inline ::std::string* RGBDImage::release_imagedata2() { - clear_has_imagedata2(); - if (imagedata2_ == &::google::protobuf::internal::kEmptyString) { - return NULL; - } else { - ::std::string* temp = imagedata2_; - imagedata2_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - return temp; - } -} - -// optional uint32 camera_config = 10; -inline bool RGBDImage::has_camera_config() const { - return (_has_bits_[0] & 0x00000200u) != 0; -} -inline void RGBDImage::set_has_camera_config() { - _has_bits_[0] |= 0x00000200u; -} -inline void RGBDImage::clear_has_camera_config() { - _has_bits_[0] &= ~0x00000200u; -} -inline void RGBDImage::clear_camera_config() { - camera_config_ = 0u; - clear_has_camera_config(); -} -inline ::google::protobuf::uint32 RGBDImage::camera_config() const { - return camera_config_; -} -inline void RGBDImage::set_camera_config(::google::protobuf::uint32 value) { - set_has_camera_config(); - camera_config_ = value; -} - -// optional uint32 camera_type = 11; -inline bool RGBDImage::has_camera_type() const { - return (_has_bits_[0] & 0x00000400u) != 0; -} -inline void RGBDImage::set_has_camera_type() { - _has_bits_[0] |= 0x00000400u; -} -inline void RGBDImage::clear_has_camera_type() { - _has_bits_[0] &= ~0x00000400u; -} -inline void RGBDImage::clear_camera_type() { - camera_type_ = 0u; - clear_has_camera_type(); -} -inline ::google::protobuf::uint32 RGBDImage::camera_type() const { - return camera_type_; -} -inline void RGBDImage::set_camera_type(::google::protobuf::uint32 value) { - set_has_camera_type(); - camera_type_ = value; -} - -// optional float roll = 12; -inline bool RGBDImage::has_roll() const { - return (_has_bits_[0] & 0x00000800u) != 0; -} -inline void RGBDImage::set_has_roll() { - _has_bits_[0] |= 0x00000800u; -} -inline void RGBDImage::clear_has_roll() { - _has_bits_[0] &= ~0x00000800u; -} -inline void RGBDImage::clear_roll() { - roll_ = 0; - clear_has_roll(); -} -inline float RGBDImage::roll() const { - return roll_; -} -inline void RGBDImage::set_roll(float value) { - set_has_roll(); - roll_ = value; -} - -// optional float pitch = 13; -inline bool RGBDImage::has_pitch() const { - return (_has_bits_[0] & 0x00001000u) != 0; -} -inline void RGBDImage::set_has_pitch() { - _has_bits_[0] |= 0x00001000u; -} -inline void RGBDImage::clear_has_pitch() { - _has_bits_[0] &= ~0x00001000u; -} -inline void RGBDImage::clear_pitch() { - pitch_ = 0; - clear_has_pitch(); -} -inline float RGBDImage::pitch() const { - return pitch_; -} -inline void RGBDImage::set_pitch(float value) { - set_has_pitch(); - pitch_ = value; -} - -// optional float yaw = 14; -inline bool RGBDImage::has_yaw() const { - return (_has_bits_[0] & 0x00002000u) != 0; -} -inline void RGBDImage::set_has_yaw() { - _has_bits_[0] |= 0x00002000u; -} -inline void RGBDImage::clear_has_yaw() { - _has_bits_[0] &= ~0x00002000u; -} -inline void RGBDImage::clear_yaw() { - yaw_ = 0; - clear_has_yaw(); -} -inline float RGBDImage::yaw() const { - return yaw_; -} -inline void RGBDImage::set_yaw(float value) { - set_has_yaw(); - yaw_ = value; -} - -// optional float lon = 15; -inline bool RGBDImage::has_lon() const { - return (_has_bits_[0] & 0x00004000u) != 0; -} -inline void RGBDImage::set_has_lon() { - _has_bits_[0] |= 0x00004000u; -} -inline void RGBDImage::clear_has_lon() { - _has_bits_[0] &= ~0x00004000u; -} -inline void RGBDImage::clear_lon() { - lon_ = 0; - clear_has_lon(); -} -inline float RGBDImage::lon() const { - return lon_; -} -inline void RGBDImage::set_lon(float value) { - set_has_lon(); - lon_ = value; -} - -// optional float lat = 16; -inline bool RGBDImage::has_lat() const { - return (_has_bits_[0] & 0x00008000u) != 0; -} -inline void RGBDImage::set_has_lat() { - _has_bits_[0] |= 0x00008000u; -} -inline void RGBDImage::clear_has_lat() { - _has_bits_[0] &= ~0x00008000u; -} -inline void RGBDImage::clear_lat() { - lat_ = 0; - clear_has_lat(); -} -inline float RGBDImage::lat() const { - return lat_; -} -inline void RGBDImage::set_lat(float value) { - set_has_lat(); - lat_ = value; -} - -// optional float alt = 17; -inline bool RGBDImage::has_alt() const { - return (_has_bits_[0] & 0x00010000u) != 0; -} -inline void RGBDImage::set_has_alt() { - _has_bits_[0] |= 0x00010000u; -} -inline void RGBDImage::clear_has_alt() { - _has_bits_[0] &= ~0x00010000u; -} -inline void RGBDImage::clear_alt() { - alt_ = 0; - clear_has_alt(); -} -inline float RGBDImage::alt() const { - return alt_; -} -inline void RGBDImage::set_alt(float value) { - set_has_alt(); - alt_ = value; -} - -// optional float ground_x = 18; -inline bool RGBDImage::has_ground_x() const { - return (_has_bits_[0] & 0x00020000u) != 0; -} -inline void RGBDImage::set_has_ground_x() { - _has_bits_[0] |= 0x00020000u; -} -inline void RGBDImage::clear_has_ground_x() { - _has_bits_[0] &= ~0x00020000u; -} -inline void RGBDImage::clear_ground_x() { - ground_x_ = 0; - clear_has_ground_x(); -} -inline float RGBDImage::ground_x() const { - return ground_x_; -} -inline void RGBDImage::set_ground_x(float value) { - set_has_ground_x(); - ground_x_ = value; -} - -// optional float ground_y = 19; -inline bool RGBDImage::has_ground_y() const { - return (_has_bits_[0] & 0x00040000u) != 0; -} -inline void RGBDImage::set_has_ground_y() { - _has_bits_[0] |= 0x00040000u; -} -inline void RGBDImage::clear_has_ground_y() { - _has_bits_[0] &= ~0x00040000u; -} -inline void RGBDImage::clear_ground_y() { - ground_y_ = 0; - clear_has_ground_y(); -} -inline float RGBDImage::ground_y() const { - return ground_y_; -} -inline void RGBDImage::set_ground_y(float value) { - set_has_ground_y(); - ground_y_ = value; -} - -// optional float ground_z = 20; -inline bool RGBDImage::has_ground_z() const { - return (_has_bits_[0] & 0x00080000u) != 0; -} -inline void RGBDImage::set_has_ground_z() { - _has_bits_[0] |= 0x00080000u; -} -inline void RGBDImage::clear_has_ground_z() { - _has_bits_[0] &= ~0x00080000u; -} -inline void RGBDImage::clear_ground_z() { - ground_z_ = 0; - clear_has_ground_z(); -} -inline float RGBDImage::ground_z() const { - return ground_z_; -} -inline void RGBDImage::set_ground_z(float value) { - set_has_ground_z(); - ground_z_ = value; -} - -// repeated float camera_matrix = 21; -inline int RGBDImage::camera_matrix_size() const { - return camera_matrix_.size(); -} -inline void RGBDImage::clear_camera_matrix() { - camera_matrix_.Clear(); -} -inline float RGBDImage::camera_matrix(int index) const { - return camera_matrix_.Get(index); -} -inline void RGBDImage::set_camera_matrix(int index, float value) { - camera_matrix_.Set(index, value); -} -inline void RGBDImage::add_camera_matrix(float value) { - camera_matrix_.Add(value); -} -inline const ::google::protobuf::RepeatedField< float >& -RGBDImage::camera_matrix() const { - return camera_matrix_; -} -inline ::google::protobuf::RepeatedField< float >* -RGBDImage::mutable_camera_matrix() { - return &camera_matrix_; -} - -// ------------------------------------------------------------------- - -// Waypoint - -// required double x = 1; -inline bool Waypoint::has_x() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void Waypoint::set_has_x() { - _has_bits_[0] |= 0x00000001u; -} -inline void Waypoint::clear_has_x() { - _has_bits_[0] &= ~0x00000001u; -} -inline void Waypoint::clear_x() { - x_ = 0; - clear_has_x(); -} -inline double Waypoint::x() const { - return x_; -} -inline void Waypoint::set_x(double value) { - set_has_x(); - x_ = value; -} - -// required double y = 2; -inline bool Waypoint::has_y() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void Waypoint::set_has_y() { - _has_bits_[0] |= 0x00000002u; -} -inline void Waypoint::clear_has_y() { - _has_bits_[0] &= ~0x00000002u; -} -inline void Waypoint::clear_y() { - y_ = 0; - clear_has_y(); -} -inline double Waypoint::y() const { - return y_; -} -inline void Waypoint::set_y(double value) { - set_has_y(); - y_ = value; -} - -// optional double z = 3; -inline bool Waypoint::has_z() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void Waypoint::set_has_z() { - _has_bits_[0] |= 0x00000004u; -} -inline void Waypoint::clear_has_z() { - _has_bits_[0] &= ~0x00000004u; -} -inline void Waypoint::clear_z() { - z_ = 0; - clear_has_z(); -} -inline double Waypoint::z() const { - return z_; -} -inline void Waypoint::set_z(double value) { - set_has_z(); - z_ = value; -} - -// optional double roll = 4; -inline bool Waypoint::has_roll() const { - return (_has_bits_[0] & 0x00000008u) != 0; -} -inline void Waypoint::set_has_roll() { - _has_bits_[0] |= 0x00000008u; -} -inline void Waypoint::clear_has_roll() { - _has_bits_[0] &= ~0x00000008u; -} -inline void Waypoint::clear_roll() { - roll_ = 0; - clear_has_roll(); -} -inline double Waypoint::roll() const { - return roll_; -} -inline void Waypoint::set_roll(double value) { - set_has_roll(); - roll_ = value; -} - -// optional double pitch = 5; -inline bool Waypoint::has_pitch() const { - return (_has_bits_[0] & 0x00000010u) != 0; -} -inline void Waypoint::set_has_pitch() { - _has_bits_[0] |= 0x00000010u; -} -inline void Waypoint::clear_has_pitch() { - _has_bits_[0] &= ~0x00000010u; -} -inline void Waypoint::clear_pitch() { - pitch_ = 0; - clear_has_pitch(); -} -inline double Waypoint::pitch() const { - return pitch_; -} -inline void Waypoint::set_pitch(double value) { - set_has_pitch(); - pitch_ = value; -} - -// optional double yaw = 6; -inline bool Waypoint::has_yaw() const { - return (_has_bits_[0] & 0x00000020u) != 0; -} -inline void Waypoint::set_has_yaw() { - _has_bits_[0] |= 0x00000020u; -} -inline void Waypoint::clear_has_yaw() { - _has_bits_[0] &= ~0x00000020u; -} -inline void Waypoint::clear_yaw() { - yaw_ = 0; - clear_has_yaw(); -} -inline double Waypoint::yaw() const { - return yaw_; -} -inline void Waypoint::set_yaw(double value) { - set_has_yaw(); - yaw_ = value; -} - - -// @@protoc_insertion_point(namespace_scope) - -} // namespace px - -#ifndef SWIG -namespace google { -namespace protobuf { - -template <> -inline const EnumDescriptor* GetEnumDescriptor< ::px::GLOverlay_CoordinateFrameType>() { - return ::px::GLOverlay_CoordinateFrameType_descriptor(); -} -template <> -inline const EnumDescriptor* GetEnumDescriptor< ::px::GLOverlay_Mode>() { - return ::px::GLOverlay_Mode_descriptor(); -} -template <> -inline const EnumDescriptor* GetEnumDescriptor< ::px::GLOverlay_Identifier>() { - return ::px::GLOverlay_Identifier_descriptor(); -} - -} // namespace google -} // namespace protobuf -#endif // SWIG - -// @@protoc_insertion_point(global_scope) - -#endif // PROTOBUF_pixhawk_2eproto__INCLUDED diff --git a/mavlink/include/mavlink/v1.0/pixhawk/testsuite.h b/mavlink/include/mavlink/v1.0/pixhawk/testsuite.h deleted file mode 100644 index caa4d3d..0000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/testsuite.h +++ /dev/null @@ -1,996 +0,0 @@ -/** @file - * @brief MAVLink comm protocol testsuite generated from pixhawk.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef PIXHAWK_TESTSUITE_H -#define PIXHAWK_TESTSUITE_H - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef MAVLINK_TEST_ALL -#define MAVLINK_TEST_ALL -static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); -static void mavlink_test_pixhawk(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_common(system_id, component_id, last_msg); - mavlink_test_pixhawk(system_id, component_id, last_msg); -} -#endif - -#include "../common/testsuite.h" - - -static void mavlink_test_set_cam_shutter(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_set_cam_shutter_t packet_in = { - 17.0, - }17443, - }17547, - }29, - }96, - }163, - }; - mavlink_set_cam_shutter_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.gain = packet_in.gain; - packet1.interval = packet_in.interval; - packet1.exposure = packet_in.exposure; - packet1.cam_no = packet_in.cam_no; - packet1.cam_mode = packet_in.cam_mode; - packet1.trigger_pin = packet_in.trigger_pin; - - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_set_cam_shutter_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_set_cam_shutter_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_set_cam_shutter_pack(system_id, component_id, &msg , packet1.cam_no , packet1.cam_mode , packet1.trigger_pin , packet1.interval , packet1.exposure , packet1.gain ); - mavlink_msg_set_cam_shutter_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_set_cam_shutter_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.cam_no , packet1.cam_mode , packet1.trigger_pin , packet1.interval , packet1.exposure , packet1.gain ); - mavlink_msg_set_cam_shutter_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; imsgid = MAVLINK_MSG_ID_CMD_AIRSPEED_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN); -#endif -} - -/** - * @brief Pack a cmd_airspeed_ack message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param spCmd - - - * @param ack - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_cmd_airspeed_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float spCmd,uint8_t ack) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN]; - _mav_put_float(buf, 0, spCmd); - _mav_put_uint8_t(buf, 4, ack); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN); -#else - mavlink_cmd_airspeed_ack_t packet; - packet.spCmd = spCmd; - packet.ack = ack; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_CMD_AIRSPEED_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN); -#endif -} - -/** - * @brief Encode a cmd_airspeed_ack struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param cmd_airspeed_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_cmd_airspeed_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cmd_airspeed_ack_t* cmd_airspeed_ack) -{ - return mavlink_msg_cmd_airspeed_ack_pack(system_id, component_id, msg, cmd_airspeed_ack->spCmd, cmd_airspeed_ack->ack); -} - -/** - * @brief Encode a cmd_airspeed_ack struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param cmd_airspeed_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_cmd_airspeed_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_cmd_airspeed_ack_t* cmd_airspeed_ack) -{ - return mavlink_msg_cmd_airspeed_ack_pack_chan(system_id, component_id, chan, msg, cmd_airspeed_ack->spCmd, cmd_airspeed_ack->ack); -} - -/** - * @brief Send a cmd_airspeed_ack message - * @param chan MAVLink channel to send the message - * - * @param spCmd - - - * @param ack - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_cmd_airspeed_ack_send(mavlink_channel_t chan, float spCmd, uint8_t ack) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN]; - _mav_put_float(buf, 0, spCmd); - _mav_put_uint8_t(buf, 4, ack); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN); -#endif -#else - mavlink_cmd_airspeed_ack_t packet; - packet.spCmd = spCmd; - packet.ack = ack; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, (const char *)&packet, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, (const char *)&packet, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN); -#endif -#endif -} - -#endif - -// MESSAGE CMD_AIRSPEED_ACK UNPACKING - - -/** - * @brief Get field spCmd from cmd_airspeed_ack message - * - * @return - - - */ -static inline float mavlink_msg_cmd_airspeed_ack_get_spCmd(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field ack from cmd_airspeed_ack message - * - * @return - - - */ -static inline uint8_t mavlink_msg_cmd_airspeed_ack_get_ack(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Decode a cmd_airspeed_ack message into a struct - * - * @param msg The message to decode - * @param cmd_airspeed_ack C-struct to decode the message contents into - */ -static inline void mavlink_msg_cmd_airspeed_ack_decode(const mavlink_message_t* msg, mavlink_cmd_airspeed_ack_t* cmd_airspeed_ack) -{ -#if MAVLINK_NEED_BYTE_SWAP - cmd_airspeed_ack->spCmd = mavlink_msg_cmd_airspeed_ack_get_spCmd(msg); - cmd_airspeed_ack->ack = mavlink_msg_cmd_airspeed_ack_get_ack(msg); -#else - memcpy(cmd_airspeed_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h deleted file mode 100644 index b6b567f..0000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h +++ /dev/null @@ -1,219 +0,0 @@ -// MESSAGE CMD_AIRSPEED_CHNG PACKING - -#define MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG 192 - -typedef struct __mavlink_cmd_airspeed_chng_t -{ - float spCmd; ///< - - - uint8_t target; ///< - - -} mavlink_cmd_airspeed_chng_t; - -#define MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN 5 -#define MAVLINK_MSG_ID_192_LEN 5 - -#define MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_CRC 209 -#define MAVLINK_MSG_ID_192_CRC 209 - - - -#define MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_CHNG { \ - "CMD_AIRSPEED_CHNG", \ - 2, \ - { { "spCmd", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_cmd_airspeed_chng_t, spCmd) }, \ - { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_cmd_airspeed_chng_t, target) }, \ - } \ -} - - -/** - * @brief Pack a cmd_airspeed_chng message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target - - - * @param spCmd - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_cmd_airspeed_chng_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target, float spCmd) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN]; - _mav_put_float(buf, 0, spCmd); - _mav_put_uint8_t(buf, 4, target); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); -#else - mavlink_cmd_airspeed_chng_t packet; - packet.spCmd = spCmd; - packet.target = target; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); -#endif -} - -/** - * @brief Pack a cmd_airspeed_chng message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target - - - * @param spCmd - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_cmd_airspeed_chng_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target,float spCmd) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN]; - _mav_put_float(buf, 0, spCmd); - _mav_put_uint8_t(buf, 4, target); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); -#else - mavlink_cmd_airspeed_chng_t packet; - packet.spCmd = spCmd; - packet.target = target; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); -#endif -} - -/** - * @brief Encode a cmd_airspeed_chng struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param cmd_airspeed_chng C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_cmd_airspeed_chng_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cmd_airspeed_chng_t* cmd_airspeed_chng) -{ - return mavlink_msg_cmd_airspeed_chng_pack(system_id, component_id, msg, cmd_airspeed_chng->target, cmd_airspeed_chng->spCmd); -} - -/** - * @brief Encode a cmd_airspeed_chng struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param cmd_airspeed_chng C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_cmd_airspeed_chng_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_cmd_airspeed_chng_t* cmd_airspeed_chng) -{ - return mavlink_msg_cmd_airspeed_chng_pack_chan(system_id, component_id, chan, msg, cmd_airspeed_chng->target, cmd_airspeed_chng->spCmd); -} - -/** - * @brief Send a cmd_airspeed_chng message - * @param chan MAVLink channel to send the message - * - * @param target - - - * @param spCmd - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_cmd_airspeed_chng_send(mavlink_channel_t chan, uint8_t target, float spCmd) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN]; - _mav_put_float(buf, 0, spCmd); - _mav_put_uint8_t(buf, 4, target); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); -#endif -#else - mavlink_cmd_airspeed_chng_t packet; - packet.spCmd = spCmd; - packet.target = target; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, (const char *)&packet, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, (const char *)&packet, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); -#endif -#endif -} - -#endif - -// MESSAGE CMD_AIRSPEED_CHNG UNPACKING - - -/** - * @brief Get field target from cmd_airspeed_chng message - * - * @return - - - */ -static inline uint8_t mavlink_msg_cmd_airspeed_chng_get_target(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field spCmd from cmd_airspeed_chng message - * - * @return - - - */ -static inline float mavlink_msg_cmd_airspeed_chng_get_spCmd(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Decode a cmd_airspeed_chng message into a struct - * - * @param msg The message to decode - * @param cmd_airspeed_chng C-struct to decode the message contents into - */ -static inline void mavlink_msg_cmd_airspeed_chng_decode(const mavlink_message_t* msg, mavlink_cmd_airspeed_chng_t* cmd_airspeed_chng) -{ -#if MAVLINK_NEED_BYTE_SWAP - cmd_airspeed_chng->spCmd = mavlink_msg_cmd_airspeed_chng_get_spCmd(msg); - cmd_airspeed_chng->target = mavlink_msg_cmd_airspeed_chng_get_target(msg); -#else - memcpy(cmd_airspeed_chng, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h deleted file mode 100644 index 642f7aa..0000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h +++ /dev/null @@ -1,187 +0,0 @@ -// MESSAGE FILT_ROT_VEL PACKING - -#define MAVLINK_MSG_ID_FILT_ROT_VEL 184 - -typedef struct __mavlink_filt_rot_vel_t -{ - float rotVel[3]; ///< - - -} mavlink_filt_rot_vel_t; - -#define MAVLINK_MSG_ID_FILT_ROT_VEL_LEN 12 -#define MAVLINK_MSG_ID_184_LEN 12 - -#define MAVLINK_MSG_ID_FILT_ROT_VEL_CRC 79 -#define MAVLINK_MSG_ID_184_CRC 79 - -#define MAVLINK_MSG_FILT_ROT_VEL_FIELD_ROTVEL_LEN 3 - -#define MAVLINK_MESSAGE_INFO_FILT_ROT_VEL { \ - "FILT_ROT_VEL", \ - 1, \ - { { "rotVel", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_filt_rot_vel_t, rotVel) }, \ - } \ -} - - -/** - * @brief Pack a filt_rot_vel message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param rotVel - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_filt_rot_vel_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const float *rotVel) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILT_ROT_VEL_LEN]; - - _mav_put_float_array(buf, 0, rotVel, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); -#else - mavlink_filt_rot_vel_t packet; - - mav_array_memcpy(packet.rotVel, rotVel, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FILT_ROT_VEL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN, MAVLINK_MSG_ID_FILT_ROT_VEL_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); -#endif -} - -/** - * @brief Pack a filt_rot_vel message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param rotVel - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_filt_rot_vel_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const float *rotVel) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILT_ROT_VEL_LEN]; - - _mav_put_float_array(buf, 0, rotVel, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); -#else - mavlink_filt_rot_vel_t packet; - - mav_array_memcpy(packet.rotVel, rotVel, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FILT_ROT_VEL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN, MAVLINK_MSG_ID_FILT_ROT_VEL_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); -#endif -} - -/** - * @brief Encode a filt_rot_vel struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param filt_rot_vel C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_filt_rot_vel_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_filt_rot_vel_t* filt_rot_vel) -{ - return mavlink_msg_filt_rot_vel_pack(system_id, component_id, msg, filt_rot_vel->rotVel); -} - -/** - * @brief Encode a filt_rot_vel struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param filt_rot_vel C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_filt_rot_vel_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_filt_rot_vel_t* filt_rot_vel) -{ - return mavlink_msg_filt_rot_vel_pack_chan(system_id, component_id, chan, msg, filt_rot_vel->rotVel); -} - -/** - * @brief Send a filt_rot_vel message - * @param chan MAVLink channel to send the message - * - * @param rotVel - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_filt_rot_vel_send(mavlink_channel_t chan, const float *rotVel) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILT_ROT_VEL_LEN]; - - _mav_put_float_array(buf, 0, rotVel, 3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, buf, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN, MAVLINK_MSG_ID_FILT_ROT_VEL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, buf, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); -#endif -#else - mavlink_filt_rot_vel_t packet; - - mav_array_memcpy(packet.rotVel, rotVel, sizeof(float)*3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, (const char *)&packet, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN, MAVLINK_MSG_ID_FILT_ROT_VEL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, (const char *)&packet, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); -#endif -#endif -} - -#endif - -// MESSAGE FILT_ROT_VEL UNPACKING - - -/** - * @brief Get field rotVel from filt_rot_vel message - * - * @return - - - */ -static inline uint16_t mavlink_msg_filt_rot_vel_get_rotVel(const mavlink_message_t* msg, float *rotVel) -{ - return _MAV_RETURN_float_array(msg, rotVel, 3, 0); -} - -/** - * @brief Decode a filt_rot_vel message into a struct - * - * @param msg The message to decode - * @param filt_rot_vel C-struct to decode the message contents into - */ -static inline void mavlink_msg_filt_rot_vel_decode(const mavlink_message_t* msg, mavlink_filt_rot_vel_t* filt_rot_vel) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_filt_rot_vel_get_rotVel(msg, filt_rot_vel->rotVel); -#else - memcpy(filt_rot_vel, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h deleted file mode 100644 index 0f83698..0000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h +++ /dev/null @@ -1,220 +0,0 @@ -// MESSAGE LLC_OUT PACKING - -#define MAVLINK_MSG_ID_LLC_OUT 186 - -typedef struct __mavlink_llc_out_t -{ - int16_t servoOut[4]; ///< - - - int16_t MotorOut[2]; ///< - - -} mavlink_llc_out_t; - -#define MAVLINK_MSG_ID_LLC_OUT_LEN 12 -#define MAVLINK_MSG_ID_186_LEN 12 - -#define MAVLINK_MSG_ID_LLC_OUT_CRC 5 -#define MAVLINK_MSG_ID_186_CRC 5 - -#define MAVLINK_MSG_LLC_OUT_FIELD_SERVOOUT_LEN 4 -#define MAVLINK_MSG_LLC_OUT_FIELD_MOTOROUT_LEN 2 - -#define MAVLINK_MESSAGE_INFO_LLC_OUT { \ - "LLC_OUT", \ - 2, \ - { { "servoOut", NULL, MAVLINK_TYPE_INT16_T, 4, 0, offsetof(mavlink_llc_out_t, servoOut) }, \ - { "MotorOut", NULL, MAVLINK_TYPE_INT16_T, 2, 8, offsetof(mavlink_llc_out_t, MotorOut) }, \ - } \ -} - - -/** - * @brief Pack a llc_out message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param servoOut - - - * @param MotorOut - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_llc_out_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const int16_t *servoOut, const int16_t *MotorOut) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LLC_OUT_LEN]; - - _mav_put_int16_t_array(buf, 0, servoOut, 4); - _mav_put_int16_t_array(buf, 8, MotorOut, 2); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LLC_OUT_LEN); -#else - mavlink_llc_out_t packet; - - mav_array_memcpy(packet.servoOut, servoOut, sizeof(int16_t)*4); - mav_array_memcpy(packet.MotorOut, MotorOut, sizeof(int16_t)*2); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LLC_OUT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LLC_OUT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LLC_OUT_LEN, MAVLINK_MSG_ID_LLC_OUT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LLC_OUT_LEN); -#endif -} - -/** - * @brief Pack a llc_out message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param servoOut - - - * @param MotorOut - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_llc_out_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const int16_t *servoOut,const int16_t *MotorOut) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LLC_OUT_LEN]; - - _mav_put_int16_t_array(buf, 0, servoOut, 4); - _mav_put_int16_t_array(buf, 8, MotorOut, 2); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LLC_OUT_LEN); -#else - mavlink_llc_out_t packet; - - mav_array_memcpy(packet.servoOut, servoOut, sizeof(int16_t)*4); - mav_array_memcpy(packet.MotorOut, MotorOut, sizeof(int16_t)*2); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LLC_OUT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LLC_OUT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LLC_OUT_LEN, MAVLINK_MSG_ID_LLC_OUT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LLC_OUT_LEN); -#endif -} - -/** - * @brief Encode a llc_out struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param llc_out C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_llc_out_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_llc_out_t* llc_out) -{ - return mavlink_msg_llc_out_pack(system_id, component_id, msg, llc_out->servoOut, llc_out->MotorOut); -} - -/** - * @brief Encode a llc_out struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param llc_out C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_llc_out_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_llc_out_t* llc_out) -{ - return mavlink_msg_llc_out_pack_chan(system_id, component_id, chan, msg, llc_out->servoOut, llc_out->MotorOut); -} - -/** - * @brief Send a llc_out message - * @param chan MAVLink channel to send the message - * - * @param servoOut - - - * @param MotorOut - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_llc_out_send(mavlink_channel_t chan, const int16_t *servoOut, const int16_t *MotorOut) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LLC_OUT_LEN]; - - _mav_put_int16_t_array(buf, 0, servoOut, 4); - _mav_put_int16_t_array(buf, 8, MotorOut, 2); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, buf, MAVLINK_MSG_ID_LLC_OUT_LEN, MAVLINK_MSG_ID_LLC_OUT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, buf, MAVLINK_MSG_ID_LLC_OUT_LEN); -#endif -#else - mavlink_llc_out_t packet; - - mav_array_memcpy(packet.servoOut, servoOut, sizeof(int16_t)*4); - mav_array_memcpy(packet.MotorOut, MotorOut, sizeof(int16_t)*2); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, (const char *)&packet, MAVLINK_MSG_ID_LLC_OUT_LEN, MAVLINK_MSG_ID_LLC_OUT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, (const char *)&packet, MAVLINK_MSG_ID_LLC_OUT_LEN); -#endif -#endif -} - -#endif - -// MESSAGE LLC_OUT UNPACKING - - -/** - * @brief Get field servoOut from llc_out message - * - * @return - - - */ -static inline uint16_t mavlink_msg_llc_out_get_servoOut(const mavlink_message_t* msg, int16_t *servoOut) -{ - return _MAV_RETURN_int16_t_array(msg, servoOut, 4, 0); -} - -/** - * @brief Get field MotorOut from llc_out message - * - * @return - - - */ -static inline uint16_t mavlink_msg_llc_out_get_MotorOut(const mavlink_message_t* msg, int16_t *MotorOut) -{ - return _MAV_RETURN_int16_t_array(msg, MotorOut, 2, 8); -} - -/** - * @brief Decode a llc_out message into a struct - * - * @param msg The message to decode - * @param llc_out C-struct to decode the message contents into - */ -static inline void mavlink_msg_llc_out_decode(const mavlink_message_t* msg, mavlink_llc_out_t* llc_out) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_llc_out_get_servoOut(msg, llc_out->servoOut); - mavlink_msg_llc_out_get_MotorOut(msg, llc_out->MotorOut); -#else - memcpy(llc_out, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LLC_OUT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h deleted file mode 100644 index 5d93823..0000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h +++ /dev/null @@ -1,187 +0,0 @@ -// MESSAGE OBS_AIR_TEMP PACKING - -#define MAVLINK_MSG_ID_OBS_AIR_TEMP 183 - -typedef struct __mavlink_obs_air_temp_t -{ - float airT; ///< - - -} mavlink_obs_air_temp_t; - -#define MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN 4 -#define MAVLINK_MSG_ID_183_LEN 4 - -#define MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC 248 -#define MAVLINK_MSG_ID_183_CRC 248 - - - -#define MAVLINK_MESSAGE_INFO_OBS_AIR_TEMP { \ - "OBS_AIR_TEMP", \ - 1, \ - { { "airT", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_obs_air_temp_t, airT) }, \ - } \ -} - - -/** - * @brief Pack a obs_air_temp message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param airT - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_air_temp_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float airT) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN]; - _mav_put_float(buf, 0, airT); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); -#else - mavlink_obs_air_temp_t packet; - packet.airT = airT; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_AIR_TEMP; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); -#endif -} - -/** - * @brief Pack a obs_air_temp message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param airT - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_air_temp_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float airT) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN]; - _mav_put_float(buf, 0, airT); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); -#else - mavlink_obs_air_temp_t packet; - packet.airT = airT; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_AIR_TEMP; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); -#endif -} - -/** - * @brief Encode a obs_air_temp struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param obs_air_temp C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_air_temp_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_air_temp_t* obs_air_temp) -{ - return mavlink_msg_obs_air_temp_pack(system_id, component_id, msg, obs_air_temp->airT); -} - -/** - * @brief Encode a obs_air_temp struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param obs_air_temp C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_air_temp_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_air_temp_t* obs_air_temp) -{ - return mavlink_msg_obs_air_temp_pack_chan(system_id, component_id, chan, msg, obs_air_temp->airT); -} - -/** - * @brief Send a obs_air_temp message - * @param chan MAVLink channel to send the message - * - * @param airT - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_air_temp_send(mavlink_channel_t chan, float airT) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN]; - _mav_put_float(buf, 0, airT); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); -#endif -#else - mavlink_obs_air_temp_t packet; - packet.airT = airT; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, (const char *)&packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, (const char *)&packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); -#endif -#endif -} - -#endif - -// MESSAGE OBS_AIR_TEMP UNPACKING - - -/** - * @brief Get field airT from obs_air_temp message - * - * @return - - - */ -static inline float mavlink_msg_obs_air_temp_get_airT(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Decode a obs_air_temp message into a struct - * - * @param msg The message to decode - * @param obs_air_temp C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_air_temp_decode(const mavlink_message_t* msg, mavlink_obs_air_temp_t* obs_air_temp) -{ -#if MAVLINK_NEED_BYTE_SWAP - obs_air_temp->airT = mavlink_msg_obs_air_temp_get_airT(msg); -#else - memcpy(obs_air_temp, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h deleted file mode 100644 index 35d813c..0000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h +++ /dev/null @@ -1,251 +0,0 @@ -// MESSAGE OBS_AIR_VELOCITY PACKING - -#define MAVLINK_MSG_ID_OBS_AIR_VELOCITY 178 - -typedef struct __mavlink_obs_air_velocity_t -{ - float magnitude; ///< - - - float aoa; ///< - - - float slip; ///< - - -} mavlink_obs_air_velocity_t; - -#define MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN 12 -#define MAVLINK_MSG_ID_178_LEN 12 - -#define MAVLINK_MSG_ID_OBS_AIR_VELOCITY_CRC 32 -#define MAVLINK_MSG_ID_178_CRC 32 - - - -#define MAVLINK_MESSAGE_INFO_OBS_AIR_VELOCITY { \ - "OBS_AIR_VELOCITY", \ - 3, \ - { { "magnitude", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_obs_air_velocity_t, magnitude) }, \ - { "aoa", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_obs_air_velocity_t, aoa) }, \ - { "slip", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_obs_air_velocity_t, slip) }, \ - } \ -} - - -/** - * @brief Pack a obs_air_velocity message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param magnitude - - - * @param aoa - - - * @param slip - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_air_velocity_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float magnitude, float aoa, float slip) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN]; - _mav_put_float(buf, 0, magnitude); - _mav_put_float(buf, 4, aoa); - _mav_put_float(buf, 8, slip); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); -#else - mavlink_obs_air_velocity_t packet; - packet.magnitude = magnitude; - packet.aoa = aoa; - packet.slip = slip; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_AIR_VELOCITY; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); -#endif -} - -/** - * @brief Pack a obs_air_velocity message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param magnitude - - - * @param aoa - - - * @param slip - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_air_velocity_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float magnitude,float aoa,float slip) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN]; - _mav_put_float(buf, 0, magnitude); - _mav_put_float(buf, 4, aoa); - _mav_put_float(buf, 8, slip); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); -#else - mavlink_obs_air_velocity_t packet; - packet.magnitude = magnitude; - packet.aoa = aoa; - packet.slip = slip; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_AIR_VELOCITY; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); -#endif -} - -/** - * @brief Encode a obs_air_velocity struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param obs_air_velocity C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_air_velocity_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_air_velocity_t* obs_air_velocity) -{ - return mavlink_msg_obs_air_velocity_pack(system_id, component_id, msg, obs_air_velocity->magnitude, obs_air_velocity->aoa, obs_air_velocity->slip); -} - -/** - * @brief Encode a obs_air_velocity struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param obs_air_velocity C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_air_velocity_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_air_velocity_t* obs_air_velocity) -{ - return mavlink_msg_obs_air_velocity_pack_chan(system_id, component_id, chan, msg, obs_air_velocity->magnitude, obs_air_velocity->aoa, obs_air_velocity->slip); -} - -/** - * @brief Send a obs_air_velocity message - * @param chan MAVLink channel to send the message - * - * @param magnitude - - - * @param aoa - - - * @param slip - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_air_velocity_send(mavlink_channel_t chan, float magnitude, float aoa, float slip) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN]; - _mav_put_float(buf, 0, magnitude); - _mav_put_float(buf, 4, aoa); - _mav_put_float(buf, 8, slip); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, buf, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, buf, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); -#endif -#else - mavlink_obs_air_velocity_t packet; - packet.magnitude = magnitude; - packet.aoa = aoa; - packet.slip = slip; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, (const char *)&packet, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, (const char *)&packet, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); -#endif -#endif -} - -#endif - -// MESSAGE OBS_AIR_VELOCITY UNPACKING - - -/** - * @brief Get field magnitude from obs_air_velocity message - * - * @return - - - */ -static inline float mavlink_msg_obs_air_velocity_get_magnitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field aoa from obs_air_velocity message - * - * @return - - - */ -static inline float mavlink_msg_obs_air_velocity_get_aoa(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field slip from obs_air_velocity message - * - * @return - - - */ -static inline float mavlink_msg_obs_air_velocity_get_slip(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Decode a obs_air_velocity message into a struct - * - * @param msg The message to decode - * @param obs_air_velocity C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_air_velocity_decode(const mavlink_message_t* msg, mavlink_obs_air_velocity_t* obs_air_velocity) -{ -#if MAVLINK_NEED_BYTE_SWAP - obs_air_velocity->magnitude = mavlink_msg_obs_air_velocity_get_magnitude(msg); - obs_air_velocity->aoa = mavlink_msg_obs_air_velocity_get_aoa(msg); - obs_air_velocity->slip = mavlink_msg_obs_air_velocity_get_slip(msg); -#else - memcpy(obs_air_velocity, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h deleted file mode 100644 index 9c80cd6..0000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h +++ /dev/null @@ -1,187 +0,0 @@ -// MESSAGE OBS_ATTITUDE PACKING - -#define MAVLINK_MSG_ID_OBS_ATTITUDE 174 - -typedef struct __mavlink_obs_attitude_t -{ - double quat[4]; ///< - - -} mavlink_obs_attitude_t; - -#define MAVLINK_MSG_ID_OBS_ATTITUDE_LEN 32 -#define MAVLINK_MSG_ID_174_LEN 32 - -#define MAVLINK_MSG_ID_OBS_ATTITUDE_CRC 146 -#define MAVLINK_MSG_ID_174_CRC 146 - -#define MAVLINK_MSG_OBS_ATTITUDE_FIELD_QUAT_LEN 4 - -#define MAVLINK_MESSAGE_INFO_OBS_ATTITUDE { \ - "OBS_ATTITUDE", \ - 1, \ - { { "quat", NULL, MAVLINK_TYPE_DOUBLE, 4, 0, offsetof(mavlink_obs_attitude_t, quat) }, \ - } \ -} - - -/** - * @brief Pack a obs_attitude message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param quat - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_attitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const double *quat) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_ATTITUDE_LEN]; - - _mav_put_double_array(buf, 0, quat, 4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); -#else - mavlink_obs_attitude_t packet; - - mav_array_memcpy(packet.quat, quat, sizeof(double)*4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_ATTITUDE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN, MAVLINK_MSG_ID_OBS_ATTITUDE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); -#endif -} - -/** - * @brief Pack a obs_attitude message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param quat - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_attitude_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const double *quat) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_ATTITUDE_LEN]; - - _mav_put_double_array(buf, 0, quat, 4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); -#else - mavlink_obs_attitude_t packet; - - mav_array_memcpy(packet.quat, quat, sizeof(double)*4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_ATTITUDE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN, MAVLINK_MSG_ID_OBS_ATTITUDE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); -#endif -} - -/** - * @brief Encode a obs_attitude struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param obs_attitude C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_attitude_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_attitude_t* obs_attitude) -{ - return mavlink_msg_obs_attitude_pack(system_id, component_id, msg, obs_attitude->quat); -} - -/** - * @brief Encode a obs_attitude struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param obs_attitude C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_attitude_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_attitude_t* obs_attitude) -{ - return mavlink_msg_obs_attitude_pack_chan(system_id, component_id, chan, msg, obs_attitude->quat); -} - -/** - * @brief Send a obs_attitude message - * @param chan MAVLink channel to send the message - * - * @param quat - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_attitude_send(mavlink_channel_t chan, const double *quat) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_ATTITUDE_LEN]; - - _mav_put_double_array(buf, 0, quat, 4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, buf, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN, MAVLINK_MSG_ID_OBS_ATTITUDE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, buf, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); -#endif -#else - mavlink_obs_attitude_t packet; - - mav_array_memcpy(packet.quat, quat, sizeof(double)*4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, (const char *)&packet, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN, MAVLINK_MSG_ID_OBS_ATTITUDE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, (const char *)&packet, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); -#endif -#endif -} - -#endif - -// MESSAGE OBS_ATTITUDE UNPACKING - - -/** - * @brief Get field quat from obs_attitude message - * - * @return - - - */ -static inline uint16_t mavlink_msg_obs_attitude_get_quat(const mavlink_message_t* msg, double *quat) -{ - return _MAV_RETURN_double_array(msg, quat, 4, 0); -} - -/** - * @brief Decode a obs_attitude message into a struct - * - * @param msg The message to decode - * @param obs_attitude C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_attitude_decode(const mavlink_message_t* msg, mavlink_obs_attitude_t* obs_attitude) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_obs_attitude_get_quat(msg, obs_attitude->quat); -#else - memcpy(obs_attitude, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h deleted file mode 100644 index 24dd43b..0000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h +++ /dev/null @@ -1,220 +0,0 @@ -// MESSAGE OBS_BIAS PACKING - -#define MAVLINK_MSG_ID_OBS_BIAS 180 - -typedef struct __mavlink_obs_bias_t -{ - float accBias[3]; ///< - - - float gyroBias[3]; ///< - - -} mavlink_obs_bias_t; - -#define MAVLINK_MSG_ID_OBS_BIAS_LEN 24 -#define MAVLINK_MSG_ID_180_LEN 24 - -#define MAVLINK_MSG_ID_OBS_BIAS_CRC 159 -#define MAVLINK_MSG_ID_180_CRC 159 - -#define MAVLINK_MSG_OBS_BIAS_FIELD_ACCBIAS_LEN 3 -#define MAVLINK_MSG_OBS_BIAS_FIELD_GYROBIAS_LEN 3 - -#define MAVLINK_MESSAGE_INFO_OBS_BIAS { \ - "OBS_BIAS", \ - 2, \ - { { "accBias", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_obs_bias_t, accBias) }, \ - { "gyroBias", NULL, MAVLINK_TYPE_FLOAT, 3, 12, offsetof(mavlink_obs_bias_t, gyroBias) }, \ - } \ -} - - -/** - * @brief Pack a obs_bias message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param accBias - - - * @param gyroBias - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_bias_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const float *accBias, const float *gyroBias) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_BIAS_LEN]; - - _mav_put_float_array(buf, 0, accBias, 3); - _mav_put_float_array(buf, 12, gyroBias, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_BIAS_LEN); -#else - mavlink_obs_bias_t packet; - - mav_array_memcpy(packet.accBias, accBias, sizeof(float)*3); - mav_array_memcpy(packet.gyroBias, gyroBias, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_BIAS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_BIAS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_BIAS_LEN, MAVLINK_MSG_ID_OBS_BIAS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_BIAS_LEN); -#endif -} - -/** - * @brief Pack a obs_bias message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param accBias - - - * @param gyroBias - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_bias_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const float *accBias,const float *gyroBias) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_BIAS_LEN]; - - _mav_put_float_array(buf, 0, accBias, 3); - _mav_put_float_array(buf, 12, gyroBias, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_BIAS_LEN); -#else - mavlink_obs_bias_t packet; - - mav_array_memcpy(packet.accBias, accBias, sizeof(float)*3); - mav_array_memcpy(packet.gyroBias, gyroBias, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_BIAS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_BIAS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_BIAS_LEN, MAVLINK_MSG_ID_OBS_BIAS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_BIAS_LEN); -#endif -} - -/** - * @brief Encode a obs_bias struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param obs_bias C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_bias_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_bias_t* obs_bias) -{ - return mavlink_msg_obs_bias_pack(system_id, component_id, msg, obs_bias->accBias, obs_bias->gyroBias); -} - -/** - * @brief Encode a obs_bias struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param obs_bias C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_bias_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_bias_t* obs_bias) -{ - return mavlink_msg_obs_bias_pack_chan(system_id, component_id, chan, msg, obs_bias->accBias, obs_bias->gyroBias); -} - -/** - * @brief Send a obs_bias message - * @param chan MAVLink channel to send the message - * - * @param accBias - - - * @param gyroBias - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_bias_send(mavlink_channel_t chan, const float *accBias, const float *gyroBias) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_BIAS_LEN]; - - _mav_put_float_array(buf, 0, accBias, 3); - _mav_put_float_array(buf, 12, gyroBias, 3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, buf, MAVLINK_MSG_ID_OBS_BIAS_LEN, MAVLINK_MSG_ID_OBS_BIAS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, buf, MAVLINK_MSG_ID_OBS_BIAS_LEN); -#endif -#else - mavlink_obs_bias_t packet; - - mav_array_memcpy(packet.accBias, accBias, sizeof(float)*3); - mav_array_memcpy(packet.gyroBias, gyroBias, sizeof(float)*3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, (const char *)&packet, MAVLINK_MSG_ID_OBS_BIAS_LEN, MAVLINK_MSG_ID_OBS_BIAS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, (const char *)&packet, MAVLINK_MSG_ID_OBS_BIAS_LEN); -#endif -#endif -} - -#endif - -// MESSAGE OBS_BIAS UNPACKING - - -/** - * @brief Get field accBias from obs_bias message - * - * @return - - - */ -static inline uint16_t mavlink_msg_obs_bias_get_accBias(const mavlink_message_t* msg, float *accBias) -{ - return _MAV_RETURN_float_array(msg, accBias, 3, 0); -} - -/** - * @brief Get field gyroBias from obs_bias message - * - * @return - - - */ -static inline uint16_t mavlink_msg_obs_bias_get_gyroBias(const mavlink_message_t* msg, float *gyroBias) -{ - return _MAV_RETURN_float_array(msg, gyroBias, 3, 12); -} - -/** - * @brief Decode a obs_bias message into a struct - * - * @param msg The message to decode - * @param obs_bias C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_bias_decode(const mavlink_message_t* msg, mavlink_obs_bias_t* obs_bias) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_obs_bias_get_accBias(msg, obs_bias->accBias); - mavlink_msg_obs_bias_get_gyroBias(msg, obs_bias->gyroBias); -#else - memcpy(obs_bias, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_BIAS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h deleted file mode 100644 index cfc2fe7..0000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h +++ /dev/null @@ -1,251 +0,0 @@ -// MESSAGE OBS_POSITION PACKING - -#define MAVLINK_MSG_ID_OBS_POSITION 170 - -typedef struct __mavlink_obs_position_t -{ - int32_t lon; ///< - - - int32_t lat; ///< - - - int32_t alt; ///< - - -} mavlink_obs_position_t; - -#define MAVLINK_MSG_ID_OBS_POSITION_LEN 12 -#define MAVLINK_MSG_ID_170_LEN 12 - -#define MAVLINK_MSG_ID_OBS_POSITION_CRC 15 -#define MAVLINK_MSG_ID_170_CRC 15 - - - -#define MAVLINK_MESSAGE_INFO_OBS_POSITION { \ - "OBS_POSITION", \ - 3, \ - { { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_obs_position_t, lon) }, \ - { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_obs_position_t, lat) }, \ - { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_obs_position_t, alt) }, \ - } \ -} - - -/** - * @brief Pack a obs_position message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param lon - - - * @param lat - - - * @param alt - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - int32_t lon, int32_t lat, int32_t alt) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_POSITION_LEN]; - _mav_put_int32_t(buf, 0, lon); - _mav_put_int32_t(buf, 4, lat); - _mav_put_int32_t(buf, 8, alt); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_POSITION_LEN); -#else - mavlink_obs_position_t packet; - packet.lon = lon; - packet.lat = lat; - packet.alt = alt; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_POSITION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_POSITION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_POSITION_LEN, MAVLINK_MSG_ID_OBS_POSITION_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_POSITION_LEN); -#endif -} - -/** - * @brief Pack a obs_position message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param lon - - - * @param lat - - - * @param alt - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - int32_t lon,int32_t lat,int32_t alt) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_POSITION_LEN]; - _mav_put_int32_t(buf, 0, lon); - _mav_put_int32_t(buf, 4, lat); - _mav_put_int32_t(buf, 8, alt); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_POSITION_LEN); -#else - mavlink_obs_position_t packet; - packet.lon = lon; - packet.lat = lat; - packet.alt = alt; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_POSITION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_POSITION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_POSITION_LEN, MAVLINK_MSG_ID_OBS_POSITION_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_POSITION_LEN); -#endif -} - -/** - * @brief Encode a obs_position struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param obs_position C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_position_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_position_t* obs_position) -{ - return mavlink_msg_obs_position_pack(system_id, component_id, msg, obs_position->lon, obs_position->lat, obs_position->alt); -} - -/** - * @brief Encode a obs_position struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param obs_position C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_position_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_position_t* obs_position) -{ - return mavlink_msg_obs_position_pack_chan(system_id, component_id, chan, msg, obs_position->lon, obs_position->lat, obs_position->alt); -} - -/** - * @brief Send a obs_position message - * @param chan MAVLink channel to send the message - * - * @param lon - - - * @param lat - - - * @param alt - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_position_send(mavlink_channel_t chan, int32_t lon, int32_t lat, int32_t alt) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_POSITION_LEN]; - _mav_put_int32_t(buf, 0, lon); - _mav_put_int32_t(buf, 4, lat); - _mav_put_int32_t(buf, 8, alt); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, buf, MAVLINK_MSG_ID_OBS_POSITION_LEN, MAVLINK_MSG_ID_OBS_POSITION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, buf, MAVLINK_MSG_ID_OBS_POSITION_LEN); -#endif -#else - mavlink_obs_position_t packet; - packet.lon = lon; - packet.lat = lat; - packet.alt = alt; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, (const char *)&packet, MAVLINK_MSG_ID_OBS_POSITION_LEN, MAVLINK_MSG_ID_OBS_POSITION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, (const char *)&packet, MAVLINK_MSG_ID_OBS_POSITION_LEN); -#endif -#endif -} - -#endif - -// MESSAGE OBS_POSITION UNPACKING - - -/** - * @brief Get field lon from obs_position message - * - * @return - - - */ -static inline int32_t mavlink_msg_obs_position_get_lon(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field lat from obs_position message - * - * @return - - - */ -static inline int32_t mavlink_msg_obs_position_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field alt from obs_position message - * - * @return - - - */ -static inline int32_t mavlink_msg_obs_position_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Decode a obs_position message into a struct - * - * @param msg The message to decode - * @param obs_position C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_position_decode(const mavlink_message_t* msg, mavlink_obs_position_t* obs_position) -{ -#if MAVLINK_NEED_BYTE_SWAP - obs_position->lon = mavlink_msg_obs_position_get_lon(msg); - obs_position->lat = mavlink_msg_obs_position_get_lat(msg); - obs_position->alt = mavlink_msg_obs_position_get_alt(msg); -#else - memcpy(obs_position, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_POSITION_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h deleted file mode 100644 index 24e272b..0000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h +++ /dev/null @@ -1,187 +0,0 @@ -// MESSAGE OBS_QFF PACKING - -#define MAVLINK_MSG_ID_OBS_QFF 182 - -typedef struct __mavlink_obs_qff_t -{ - float qff; ///< - - -} mavlink_obs_qff_t; - -#define MAVLINK_MSG_ID_OBS_QFF_LEN 4 -#define MAVLINK_MSG_ID_182_LEN 4 - -#define MAVLINK_MSG_ID_OBS_QFF_CRC 24 -#define MAVLINK_MSG_ID_182_CRC 24 - - - -#define MAVLINK_MESSAGE_INFO_OBS_QFF { \ - "OBS_QFF", \ - 1, \ - { { "qff", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_obs_qff_t, qff) }, \ - } \ -} - - -/** - * @brief Pack a obs_qff message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param qff - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_qff_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float qff) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_QFF_LEN]; - _mav_put_float(buf, 0, qff); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_QFF_LEN); -#else - mavlink_obs_qff_t packet; - packet.qff = qff; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_QFF_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_QFF; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_QFF_LEN); -#endif -} - -/** - * @brief Pack a obs_qff message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param qff - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_qff_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float qff) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_QFF_LEN]; - _mav_put_float(buf, 0, qff); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_QFF_LEN); -#else - mavlink_obs_qff_t packet; - packet.qff = qff; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_QFF_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_QFF; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_QFF_LEN); -#endif -} - -/** - * @brief Encode a obs_qff struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param obs_qff C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_qff_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_qff_t* obs_qff) -{ - return mavlink_msg_obs_qff_pack(system_id, component_id, msg, obs_qff->qff); -} - -/** - * @brief Encode a obs_qff struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param obs_qff C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_qff_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_qff_t* obs_qff) -{ - return mavlink_msg_obs_qff_pack_chan(system_id, component_id, chan, msg, obs_qff->qff); -} - -/** - * @brief Send a obs_qff message - * @param chan MAVLink channel to send the message - * - * @param qff - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_qff_send(mavlink_channel_t chan, float qff) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_QFF_LEN]; - _mav_put_float(buf, 0, qff); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, buf, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, buf, MAVLINK_MSG_ID_OBS_QFF_LEN); -#endif -#else - mavlink_obs_qff_t packet; - packet.qff = qff; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, (const char *)&packet, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, (const char *)&packet, MAVLINK_MSG_ID_OBS_QFF_LEN); -#endif -#endif -} - -#endif - -// MESSAGE OBS_QFF UNPACKING - - -/** - * @brief Get field qff from obs_qff message - * - * @return - - - */ -static inline float mavlink_msg_obs_qff_get_qff(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Decode a obs_qff message into a struct - * - * @param msg The message to decode - * @param obs_qff C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_qff_decode(const mavlink_message_t* msg, mavlink_obs_qff_t* obs_qff) -{ -#if MAVLINK_NEED_BYTE_SWAP - obs_qff->qff = mavlink_msg_obs_qff_get_qff(msg); -#else - memcpy(obs_qff, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_QFF_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h deleted file mode 100644 index 6e37766..0000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h +++ /dev/null @@ -1,187 +0,0 @@ -// MESSAGE OBS_VELOCITY PACKING - -#define MAVLINK_MSG_ID_OBS_VELOCITY 172 - -typedef struct __mavlink_obs_velocity_t -{ - float vel[3]; ///< - - -} mavlink_obs_velocity_t; - -#define MAVLINK_MSG_ID_OBS_VELOCITY_LEN 12 -#define MAVLINK_MSG_ID_172_LEN 12 - -#define MAVLINK_MSG_ID_OBS_VELOCITY_CRC 108 -#define MAVLINK_MSG_ID_172_CRC 108 - -#define MAVLINK_MSG_OBS_VELOCITY_FIELD_VEL_LEN 3 - -#define MAVLINK_MESSAGE_INFO_OBS_VELOCITY { \ - "OBS_VELOCITY", \ - 1, \ - { { "vel", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_obs_velocity_t, vel) }, \ - } \ -} - - -/** - * @brief Pack a obs_velocity message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param vel - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_velocity_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const float *vel) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_VELOCITY_LEN]; - - _mav_put_float_array(buf, 0, vel, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); -#else - mavlink_obs_velocity_t packet; - - mav_array_memcpy(packet.vel, vel, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_VELOCITY; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); -#endif -} - -/** - * @brief Pack a obs_velocity message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param vel - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_velocity_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const float *vel) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_VELOCITY_LEN]; - - _mav_put_float_array(buf, 0, vel, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); -#else - mavlink_obs_velocity_t packet; - - mav_array_memcpy(packet.vel, vel, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_VELOCITY; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); -#endif -} - -/** - * @brief Encode a obs_velocity struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param obs_velocity C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_velocity_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_velocity_t* obs_velocity) -{ - return mavlink_msg_obs_velocity_pack(system_id, component_id, msg, obs_velocity->vel); -} - -/** - * @brief Encode a obs_velocity struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param obs_velocity C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_velocity_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_velocity_t* obs_velocity) -{ - return mavlink_msg_obs_velocity_pack_chan(system_id, component_id, chan, msg, obs_velocity->vel); -} - -/** - * @brief Send a obs_velocity message - * @param chan MAVLink channel to send the message - * - * @param vel - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_velocity_send(mavlink_channel_t chan, const float *vel) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_VELOCITY_LEN]; - - _mav_put_float_array(buf, 0, vel, 3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); -#endif -#else - mavlink_obs_velocity_t packet; - - mav_array_memcpy(packet.vel, vel, sizeof(float)*3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, (const char *)&packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, (const char *)&packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); -#endif -#endif -} - -#endif - -// MESSAGE OBS_VELOCITY UNPACKING - - -/** - * @brief Get field vel from obs_velocity message - * - * @return - - - */ -static inline uint16_t mavlink_msg_obs_velocity_get_vel(const mavlink_message_t* msg, float *vel) -{ - return _MAV_RETURN_float_array(msg, vel, 3, 0); -} - -/** - * @brief Decode a obs_velocity message into a struct - * - * @param msg The message to decode - * @param obs_velocity C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_velocity_decode(const mavlink_message_t* msg, mavlink_obs_velocity_t* obs_velocity) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_obs_velocity_get_vel(msg, obs_velocity->vel); -#else - memcpy(obs_velocity, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_VELOCITY_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h deleted file mode 100644 index 55f7cb2..0000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h +++ /dev/null @@ -1,187 +0,0 @@ -// MESSAGE OBS_WIND PACKING - -#define MAVLINK_MSG_ID_OBS_WIND 176 - -typedef struct __mavlink_obs_wind_t -{ - float wind[3]; ///< - - -} mavlink_obs_wind_t; - -#define MAVLINK_MSG_ID_OBS_WIND_LEN 12 -#define MAVLINK_MSG_ID_176_LEN 12 - -#define MAVLINK_MSG_ID_OBS_WIND_CRC 16 -#define MAVLINK_MSG_ID_176_CRC 16 - -#define MAVLINK_MSG_OBS_WIND_FIELD_WIND_LEN 3 - -#define MAVLINK_MESSAGE_INFO_OBS_WIND { \ - "OBS_WIND", \ - 1, \ - { { "wind", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_obs_wind_t, wind) }, \ - } \ -} - - -/** - * @brief Pack a obs_wind message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param wind - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_wind_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const float *wind) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_WIND_LEN]; - - _mav_put_float_array(buf, 0, wind, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_WIND_LEN); -#else - mavlink_obs_wind_t packet; - - mav_array_memcpy(packet.wind, wind, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_WIND_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_WIND; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_WIND_LEN); -#endif -} - -/** - * @brief Pack a obs_wind message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param wind - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_wind_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const float *wind) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_WIND_LEN]; - - _mav_put_float_array(buf, 0, wind, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_WIND_LEN); -#else - mavlink_obs_wind_t packet; - - mav_array_memcpy(packet.wind, wind, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_WIND_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_WIND; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_WIND_LEN); -#endif -} - -/** - * @brief Encode a obs_wind struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param obs_wind C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_wind_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_wind_t* obs_wind) -{ - return mavlink_msg_obs_wind_pack(system_id, component_id, msg, obs_wind->wind); -} - -/** - * @brief Encode a obs_wind struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param obs_wind C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_wind_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_wind_t* obs_wind) -{ - return mavlink_msg_obs_wind_pack_chan(system_id, component_id, chan, msg, obs_wind->wind); -} - -/** - * @brief Send a obs_wind message - * @param chan MAVLink channel to send the message - * - * @param wind - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_wind_send(mavlink_channel_t chan, const float *wind) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_WIND_LEN]; - - _mav_put_float_array(buf, 0, wind, 3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, buf, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, buf, MAVLINK_MSG_ID_OBS_WIND_LEN); -#endif -#else - mavlink_obs_wind_t packet; - - mav_array_memcpy(packet.wind, wind, sizeof(float)*3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, (const char *)&packet, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, (const char *)&packet, MAVLINK_MSG_ID_OBS_WIND_LEN); -#endif -#endif -} - -#endif - -// MESSAGE OBS_WIND UNPACKING - - -/** - * @brief Get field wind from obs_wind message - * - * @return - - - */ -static inline uint16_t mavlink_msg_obs_wind_get_wind(const mavlink_message_t* msg, float *wind) -{ - return _MAV_RETURN_float_array(msg, wind, 3, 0); -} - -/** - * @brief Decode a obs_wind message into a struct - * - * @param msg The message to decode - * @param obs_wind C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_wind_decode(const mavlink_message_t* msg, mavlink_obs_wind_t* obs_wind) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_obs_wind_get_wind(msg, obs_wind->wind); -#else - memcpy(obs_wind, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_WIND_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h deleted file mode 100644 index e0963ec..0000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h +++ /dev/null @@ -1,245 +0,0 @@ -// MESSAGE PM_ELEC PACKING - -#define MAVLINK_MSG_ID_PM_ELEC 188 - -typedef struct __mavlink_pm_elec_t -{ - float PwCons; ///< - - - float BatStat; ///< - - - float PwGen[3]; ///< - - -} mavlink_pm_elec_t; - -#define MAVLINK_MSG_ID_PM_ELEC_LEN 20 -#define MAVLINK_MSG_ID_188_LEN 20 - -#define MAVLINK_MSG_ID_PM_ELEC_CRC 170 -#define MAVLINK_MSG_ID_188_CRC 170 - -#define MAVLINK_MSG_PM_ELEC_FIELD_PWGEN_LEN 3 - -#define MAVLINK_MESSAGE_INFO_PM_ELEC { \ - "PM_ELEC", \ - 3, \ - { { "PwCons", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_pm_elec_t, PwCons) }, \ - { "BatStat", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_pm_elec_t, BatStat) }, \ - { "PwGen", NULL, MAVLINK_TYPE_FLOAT, 3, 8, offsetof(mavlink_pm_elec_t, PwGen) }, \ - } \ -} - - -/** - * @brief Pack a pm_elec message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param PwCons - - - * @param BatStat - - - * @param PwGen - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_pm_elec_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float PwCons, float BatStat, const float *PwGen) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PM_ELEC_LEN]; - _mav_put_float(buf, 0, PwCons); - _mav_put_float(buf, 4, BatStat); - _mav_put_float_array(buf, 8, PwGen, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PM_ELEC_LEN); -#else - mavlink_pm_elec_t packet; - packet.PwCons = PwCons; - packet.BatStat = BatStat; - mav_array_memcpy(packet.PwGen, PwGen, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PM_ELEC_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PM_ELEC; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PM_ELEC_LEN, MAVLINK_MSG_ID_PM_ELEC_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PM_ELEC_LEN); -#endif -} - -/** - * @brief Pack a pm_elec message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param PwCons - - - * @param BatStat - - - * @param PwGen - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_pm_elec_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float PwCons,float BatStat,const float *PwGen) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PM_ELEC_LEN]; - _mav_put_float(buf, 0, PwCons); - _mav_put_float(buf, 4, BatStat); - _mav_put_float_array(buf, 8, PwGen, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PM_ELEC_LEN); -#else - mavlink_pm_elec_t packet; - packet.PwCons = PwCons; - packet.BatStat = BatStat; - mav_array_memcpy(packet.PwGen, PwGen, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PM_ELEC_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PM_ELEC; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PM_ELEC_LEN, MAVLINK_MSG_ID_PM_ELEC_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PM_ELEC_LEN); -#endif -} - -/** - * @brief Encode a pm_elec struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param pm_elec C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_pm_elec_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_pm_elec_t* pm_elec) -{ - return mavlink_msg_pm_elec_pack(system_id, component_id, msg, pm_elec->PwCons, pm_elec->BatStat, pm_elec->PwGen); -} - -/** - * @brief Encode a pm_elec struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param pm_elec C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_pm_elec_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_pm_elec_t* pm_elec) -{ - return mavlink_msg_pm_elec_pack_chan(system_id, component_id, chan, msg, pm_elec->PwCons, pm_elec->BatStat, pm_elec->PwGen); -} - -/** - * @brief Send a pm_elec message - * @param chan MAVLink channel to send the message - * - * @param PwCons - - - * @param BatStat - - - * @param PwGen - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_pm_elec_send(mavlink_channel_t chan, float PwCons, float BatStat, const float *PwGen) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PM_ELEC_LEN]; - _mav_put_float(buf, 0, PwCons); - _mav_put_float(buf, 4, BatStat); - _mav_put_float_array(buf, 8, PwGen, 3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, buf, MAVLINK_MSG_ID_PM_ELEC_LEN, MAVLINK_MSG_ID_PM_ELEC_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, buf, MAVLINK_MSG_ID_PM_ELEC_LEN); -#endif -#else - mavlink_pm_elec_t packet; - packet.PwCons = PwCons; - packet.BatStat = BatStat; - mav_array_memcpy(packet.PwGen, PwGen, sizeof(float)*3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, (const char *)&packet, MAVLINK_MSG_ID_PM_ELEC_LEN, MAVLINK_MSG_ID_PM_ELEC_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, (const char *)&packet, MAVLINK_MSG_ID_PM_ELEC_LEN); -#endif -#endif -} - -#endif - -// MESSAGE PM_ELEC UNPACKING - - -/** - * @brief Get field PwCons from pm_elec message - * - * @return - - - */ -static inline float mavlink_msg_pm_elec_get_PwCons(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field BatStat from pm_elec message - * - * @return - - - */ -static inline float mavlink_msg_pm_elec_get_BatStat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field PwGen from pm_elec message - * - * @return - - - */ -static inline uint16_t mavlink_msg_pm_elec_get_PwGen(const mavlink_message_t* msg, float *PwGen) -{ - return _MAV_RETURN_float_array(msg, PwGen, 3, 8); -} - -/** - * @brief Decode a pm_elec message into a struct - * - * @param msg The message to decode - * @param pm_elec C-struct to decode the message contents into - */ -static inline void mavlink_msg_pm_elec_decode(const mavlink_message_t* msg, mavlink_pm_elec_t* pm_elec) -{ -#if MAVLINK_NEED_BYTE_SWAP - pm_elec->PwCons = mavlink_msg_pm_elec_get_PwCons(msg); - pm_elec->BatStat = mavlink_msg_pm_elec_get_BatStat(msg); - mavlink_msg_pm_elec_get_PwGen(msg, pm_elec->PwGen); -#else - memcpy(pm_elec, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PM_ELEC_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h deleted file mode 100644 index 94f3e58..0000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h +++ /dev/null @@ -1,283 +0,0 @@ -// MESSAGE SYS_Stat PACKING - -#define MAVLINK_MSG_ID_SYS_Stat 190 - -typedef struct __mavlink_sys_stat_t -{ - uint8_t gps; ///< - - - uint8_t act; ///< - - - uint8_t mod; ///< - - - uint8_t commRssi; ///< - - -} mavlink_sys_stat_t; - -#define MAVLINK_MSG_ID_SYS_Stat_LEN 4 -#define MAVLINK_MSG_ID_190_LEN 4 - -#define MAVLINK_MSG_ID_SYS_Stat_CRC 157 -#define MAVLINK_MSG_ID_190_CRC 157 - - - -#define MAVLINK_MESSAGE_INFO_SYS_Stat { \ - "SYS_Stat", \ - 4, \ - { { "gps", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_sys_stat_t, gps) }, \ - { "act", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_sys_stat_t, act) }, \ - { "mod", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_sys_stat_t, mod) }, \ - { "commRssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_sys_stat_t, commRssi) }, \ - } \ -} - - -/** - * @brief Pack a sys_stat message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param gps - - - * @param act - - - * @param mod - - - * @param commRssi - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_sys_stat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t gps, uint8_t act, uint8_t mod, uint8_t commRssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SYS_Stat_LEN]; - _mav_put_uint8_t(buf, 0, gps); - _mav_put_uint8_t(buf, 1, act); - _mav_put_uint8_t(buf, 2, mod); - _mav_put_uint8_t(buf, 3, commRssi); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYS_Stat_LEN); -#else - mavlink_sys_stat_t packet; - packet.gps = gps; - packet.act = act; - packet.mod = mod; - packet.commRssi = commRssi; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_Stat_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SYS_Stat; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYS_Stat_LEN, MAVLINK_MSG_ID_SYS_Stat_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYS_Stat_LEN); -#endif -} - -/** - * @brief Pack a sys_stat message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param gps - - - * @param act - - - * @param mod - - - * @param commRssi - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_sys_stat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t gps,uint8_t act,uint8_t mod,uint8_t commRssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SYS_Stat_LEN]; - _mav_put_uint8_t(buf, 0, gps); - _mav_put_uint8_t(buf, 1, act); - _mav_put_uint8_t(buf, 2, mod); - _mav_put_uint8_t(buf, 3, commRssi); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYS_Stat_LEN); -#else - mavlink_sys_stat_t packet; - packet.gps = gps; - packet.act = act; - packet.mod = mod; - packet.commRssi = commRssi; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_Stat_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SYS_Stat; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYS_Stat_LEN, MAVLINK_MSG_ID_SYS_Stat_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYS_Stat_LEN); -#endif -} - -/** - * @brief Encode a sys_stat struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param sys_stat C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_sys_stat_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sys_stat_t* sys_stat) -{ - return mavlink_msg_sys_stat_pack(system_id, component_id, msg, sys_stat->gps, sys_stat->act, sys_stat->mod, sys_stat->commRssi); -} - -/** - * @brief Encode a sys_stat struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sys_stat C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_sys_stat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sys_stat_t* sys_stat) -{ - return mavlink_msg_sys_stat_pack_chan(system_id, component_id, chan, msg, sys_stat->gps, sys_stat->act, sys_stat->mod, sys_stat->commRssi); -} - -/** - * @brief Send a sys_stat message - * @param chan MAVLink channel to send the message - * - * @param gps - - - * @param act - - - * @param mod - - - * @param commRssi - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_sys_stat_send(mavlink_channel_t chan, uint8_t gps, uint8_t act, uint8_t mod, uint8_t commRssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SYS_Stat_LEN]; - _mav_put_uint8_t(buf, 0, gps); - _mav_put_uint8_t(buf, 1, act); - _mav_put_uint8_t(buf, 2, mod); - _mav_put_uint8_t(buf, 3, commRssi); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, buf, MAVLINK_MSG_ID_SYS_Stat_LEN, MAVLINK_MSG_ID_SYS_Stat_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, buf, MAVLINK_MSG_ID_SYS_Stat_LEN); -#endif -#else - mavlink_sys_stat_t packet; - packet.gps = gps; - packet.act = act; - packet.mod = mod; - packet.commRssi = commRssi; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, (const char *)&packet, MAVLINK_MSG_ID_SYS_Stat_LEN, MAVLINK_MSG_ID_SYS_Stat_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, (const char *)&packet, MAVLINK_MSG_ID_SYS_Stat_LEN); -#endif -#endif -} - -#endif - -// MESSAGE SYS_Stat UNPACKING - - -/** - * @brief Get field gps from sys_stat message - * - * @return - - - */ -static inline uint8_t mavlink_msg_sys_stat_get_gps(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field act from sys_stat message - * - * @return - - - */ -static inline uint8_t mavlink_msg_sys_stat_get_act(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field mod from sys_stat message - * - * @return - - - */ -static inline uint8_t mavlink_msg_sys_stat_get_mod(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field commRssi from sys_stat message - * - * @return - - - */ -static inline uint8_t mavlink_msg_sys_stat_get_commRssi(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Decode a sys_stat message into a struct - * - * @param msg The message to decode - * @param sys_stat C-struct to decode the message contents into - */ -static inline void mavlink_msg_sys_stat_decode(const mavlink_message_t* msg, mavlink_sys_stat_t* sys_stat) -{ -#if MAVLINK_NEED_BYTE_SWAP - sys_stat->gps = mavlink_msg_sys_stat_get_gps(msg); - sys_stat->act = mavlink_msg_sys_stat_get_act(msg); - sys_stat->mod = mavlink_msg_sys_stat_get_mod(msg); - sys_stat->commRssi = mavlink_msg_sys_stat_get_commRssi(msg); -#else - memcpy(sys_stat, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SYS_Stat_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h b/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h deleted file mode 100644 index 26666b0..0000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h +++ /dev/null @@ -1,77 +0,0 @@ -/** @file - * @brief MAVLink comm protocol generated from sensesoar.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef SENSESOAR_H -#define SENSESOAR_H - -#ifdef __cplusplus -extern "C" { -#endif - -// MESSAGE LENGTHS AND CRCS - -#ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 0, 0, 0, 0, 0, 13, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 12, 0, 12, 0, 32, 0, 12, 0, 12, 0, 24, 0, 4, 4, 12, 0, 12, 0, 20, 0, 4, 0, 5, 0, 5, 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, 36, 30, 18, 18, 51, 9, 0} -#endif - -#ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 0, 0, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 15, 0, 108, 0, 146, 0, 16, 0, 32, 0, 159, 0, 24, 248, 79, 0, 5, 0, 170, 0, 157, 0, 209, 0, 243, 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, 204, 49, 170, 44, 83, 46, 0} -#endif - -#ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"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_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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_OBS_POSITION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_ATTITUDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_WIND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_AIR_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_BIAS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_QFF, MAVLINK_MESSAGE_INFO_OBS_AIR_TEMP, MAVLINK_MESSAGE_INFO_FILT_ROT_VEL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LLC_OUT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PM_ELEC, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SYS_Stat, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_CHNG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} -#endif - -#include "../protocol.h" - -#define MAVLINK_ENABLED_SENSESOAR - -// ENUM DEFINITIONS - - -/** @brief Different flight modes */ -#ifndef HAVE_ENUM_SENSESOAR_MODE -#define HAVE_ENUM_SENSESOAR_MODE -enum SENSESOAR_MODE -{ - SENSESOAR_MODE_GLIDING=1, /* Gliding mode with motors off | */ - SENSESOAR_MODE_AUTONOMOUS=2, /* Autonomous flight | */ - SENSESOAR_MODE_MANUAL=3, /* RC controlled | */ - SENSESOAR_MODE_ENUM_END=4, /* | */ -}; -#endif - -#include "../common/common.h" - -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -// MESSAGE DEFINITIONS -#include "./mavlink_msg_obs_position.h" -#include "./mavlink_msg_obs_velocity.h" -#include "./mavlink_msg_obs_attitude.h" -#include "./mavlink_msg_obs_wind.h" -#include "./mavlink_msg_obs_air_velocity.h" -#include "./mavlink_msg_obs_bias.h" -#include "./mavlink_msg_obs_qff.h" -#include "./mavlink_msg_obs_air_temp.h" -#include "./mavlink_msg_filt_rot_vel.h" -#include "./mavlink_msg_llc_out.h" -#include "./mavlink_msg_pm_elec.h" -#include "./mavlink_msg_sys_stat.h" -#include "./mavlink_msg_cmd_airspeed_chng.h" -#include "./mavlink_msg_cmd_airspeed_ack.h" - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // SENSESOAR_H diff --git a/mavlink/include/mavlink/v1.0/sensesoar/testsuite.h b/mavlink/include/mavlink/v1.0/sensesoar/testsuite.h deleted file mode 100644 index 67ffca7..0000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/testsuite.h +++ /dev/null @@ -1,676 +0,0 @@ -/** @file - * @brief MAVLink comm protocol testsuite generated from sensesoar.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef SENSESOAR_TESTSUITE_H -#define SENSESOAR_TESTSUITE_H - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef MAVLINK_TEST_ALL -#define MAVLINK_TEST_ALL -static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); -static void mavlink_test_sensesoar(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_common(system_id, component_id, last_msg); - mavlink_test_sensesoar(system_id, component_id, last_msg); -} -#endif - -#include "../common/testsuite.h" - - -static void mavlink_test_obs_position(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_obs_position_t packet_in = { - 963497464, - }963497672, - }963497880, - }; - mavlink_obs_position_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.lon = packet_in.lon; - packet1.lat = packet_in.lat; - packet1.alt = packet_in.alt; - - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_obs_position_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_obs_position_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_obs_position_pack(system_id, component_id, &msg , packet1.lon , packet1.lat , packet1.alt ); - mavlink_msg_obs_position_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_obs_position_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.lon , packet1.lat , packet1.alt ); - mavlink_msg_obs_position_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_to_send_buffer(buffer, &msg); - for (i=0; i Date: Wed, 29 Oct 2014 17:08:21 +0100 Subject: [PATCH 06/14] Change to default / common dialect --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index f82ad6c..569a343 100644 --- a/Makefile +++ b/Makefile @@ -10,7 +10,7 @@ JTAGCONFIG ?= olimex-jtag-tiny.cfg #JTAGCONFIG ?= olimex-arm-usb-tiny-h.cfg MAVLINKBASEDIR = mavlink/include/mavlink/v1.0 -MAVLINKDIR = mavlink/include/mavlink/v1.0/pixhawk +MAVLINKDIR = mavlink/include/mavlink/v1.0/common MAVLINKUSERDIR = mavlink/include/mavlink/v1.0/user AS = arm-none-eabi-as From f6ccb8f0c8099a3d4b9f72d27daa9f5b285399a2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Oct 2014 17:08:53 +0100 Subject: [PATCH 07/14] Fix API and repeated declarations of UART interfaces, fix meaningless parameter index comparison --- inc/communication.h | 1 - inc/mavlink_bridge_header.h | 2 +- inc/usart.h | 2 +- src/communication.c | 4 ++-- src/usart.c | 2 +- 5 files changed, 5 insertions(+), 6 deletions(-) diff --git a/inc/communication.h b/inc/communication.h index 80586da..10f09de 100644 --- a/inc/communication.h +++ b/inc/communication.h @@ -48,7 +48,6 @@ void communication_receive_forward(void); void communication_receive_usb(void); void handle_mavlink_message(mavlink_channel_t chan, mavlink_message_t* msg); -void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t * ch, uint16_t length); mavlink_status_t* mavlink_get_channel_status(uint8_t channel); mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel); diff --git a/inc/mavlink_bridge_header.h b/inc/mavlink_bridge_header.h index 7ee4331..c3934e5 100644 --- a/inc/mavlink_bridge_header.h +++ b/inc/mavlink_bridge_header.h @@ -49,7 +49,7 @@ extern mavlink_system_t mavlink_system; /* defined in communication.c */ -extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t * ch, uint16_t length); +extern void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t * ch, uint16_t length); mavlink_status_t* mavlink_get_channel_status(uint8_t chan); mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan); diff --git a/inc/usart.h b/inc/usart.h index 046b1a7..97e32ae 100644 --- a/inc/usart.h +++ b/inc/usart.h @@ -59,7 +59,7 @@ uint8_t usart3_rx_ringbuffer_pop(void); /** * @brief Push one byte to ringbuffer of USART3 */ -uint8_t usart3_tx_ringbuffer_push(uint8_t* ch, uint8_t len); +uint8_t usart3_tx_ringbuffer_push(const uint8_t* ch, uint8_t len); /** * @brief Check character availability USART2 diff --git a/src/communication.c b/src/communication.c index 97d45fe..5e15261 100644 --- a/src/communication.c +++ b/src/communication.c @@ -141,7 +141,7 @@ void handle_mavlink_message(mavlink_channel_t chan, if (set.param_id[0] != -1) { /* Choose parameter based on index */ - if (0 <= set.param_index < ONBOARD_PARAM_COUNT) + if ((set.param_index >= 0) && (set.param_index < ONBOARD_PARAM_COUNT)) { /* Report back value */ mavlink_msg_param_value_send(chan, @@ -421,7 +421,7 @@ void communication_receive_usb(void) * @param chan MAVLink channel to use * @param ch Character to send */ -void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t * ch, uint16_t length) +void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t * ch, uint16_t length) { if (chan == MAVLINK_COMM_0) { diff --git a/src/usart.c b/src/usart.c index 7064737..c6535d3 100644 --- a/src/usart.c +++ b/src/usart.c @@ -87,7 +87,7 @@ uint8_t usart2_tx_ringbuffer_push(uint8_t* ch, uint8_t len) /** * @brief Push one byte to ringbuffer of USART3 */ -uint8_t usart3_tx_ringbuffer_push(uint8_t* ch, uint8_t len) +uint8_t usart3_tx_ringbuffer_push(const uint8_t* ch, uint8_t len) { USART_ITConfig(USART3, USART_IT_TXE, DISABLE); From 1e4baab9df1d3d4cefb10f98ef5459cbd5a60cb9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 30 Oct 2014 15:38:13 +0100 Subject: [PATCH 08/14] Updated MAVLink version --- mavlink/include/mavlink/v1.0/common/common.h | 4 +- .../common/mavlink_msg_hil_optical_flow.h | 342 +++++++++++------- .../common/mavlink_msg_optical_flow_rad.h | 186 +++++++--- .../include/mavlink/v1.0/common/testsuite.h | 34 +- mavlink/include/mavlink/v1.0/common/version.h | 2 +- 5 files changed, 384 insertions(+), 184 deletions(-) diff --git a/mavlink/include/mavlink/v1.0/common/common.h b/mavlink/include/mavlink/v1.0/common/common.h index 39af53a..4bb08a1 100644 --- a/mavlink/include/mavlink/v1.0/common/common.h +++ b/mavlink/include/mavlink/v1.0/common/common.h @@ -16,11 +16,11 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 0, 0, 0, 0, 27, 25, 0, 0, 0, 0, 0, 68, 26, 185, 181, 42, 6, 4, 0, 11, 18, 0, 0, 37, 20, 35, 33, 3, 0, 0, 0, 22, 39, 37, 53, 51, 53, 51, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 30, 64, 84, 9, 254, 0, 0, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 35, 35, 0, 13, 255, 14, 18, 43, 8, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 20, 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, 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, 254, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 0, 0, 0, 0, 27, 25, 0, 0, 0, 0, 0, 68, 26, 185, 181, 42, 6, 4, 0, 11, 18, 0, 0, 37, 20, 35, 33, 3, 0, 0, 0, 22, 39, 37, 53, 51, 53, 51, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 44, 64, 84, 9, 254, 0, 0, 36, 44, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 35, 35, 0, 13, 255, 14, 18, 43, 8, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 20, 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, 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, 254, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 0, 0, 0, 0, 15, 3, 0, 0, 0, 0, 0, 153, 183, 51, 82, 118, 148, 21, 0, 243, 124, 0, 0, 38, 20, 158, 152, 143, 0, 0, 0, 106, 49, 22, 143, 140, 5, 150, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 102, 108, 32, 185, 84, 0, 0, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 25, 226, 0, 29, 223, 85, 6, 229, 203, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 154, 49, 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, 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, 8, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 0, 0, 0, 0, 15, 3, 0, 0, 0, 0, 0, 153, 183, 51, 82, 118, 148, 21, 0, 243, 124, 0, 0, 38, 20, 158, 152, 143, 0, 0, 0, 106, 49, 22, 143, 140, 5, 150, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 138, 108, 32, 185, 84, 0, 0, 124, 237, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 25, 226, 0, 29, 223, 85, 6, 229, 203, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 154, 49, 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, 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, 8, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h index eaadf24..7acb0b8 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h @@ -4,35 +4,43 @@ typedef struct __mavlink_hil_optical_flow_t { - uint64_t time_usec; ///< Timestamp (UNIX) - float flow_comp_m_x; ///< Flow in meters in x-sensor direction, angular-speed compensated - float flow_comp_m_y; ///< Flow in meters in y-sensor direction, angular-speed compensated - float ground_distance; ///< Ground distance in meters. Positive value: distance known. Negative value: Unknown distance - int16_t flow_x; ///< Flow in pixels in x-sensor direction - int16_t flow_y; ///< Flow in pixels in y-sensor direction + uint64_t time_usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot) + uint32_t integration_time_us; ///< Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + float integrated_x; ///< Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + float integrated_y; ///< Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + float integrated_xgyro; ///< RH rotation around X axis (rad) + float integrated_ygyro; ///< RH rotation around Y axis (rad) + float integrated_zgyro; ///< RH rotation around Z axis (rad) + uint32_t time_delta_distance_us; ///< Time in microseconds since the distance was sampled. + float distance; ///< Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. + int16_t temperature; ///< Temperature * 100 in centi-degrees Celsius uint8_t sensor_id; ///< Sensor ID - uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality + uint8_t quality; ///< Optical flow quality / confidence. 0: no valid flow, 255: maximum quality } mavlink_hil_optical_flow_t; -#define MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN 26 -#define MAVLINK_MSG_ID_114_LEN 26 +#define MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN 44 +#define MAVLINK_MSG_ID_114_LEN 44 -#define MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC 119 -#define MAVLINK_MSG_ID_114_CRC 119 +#define MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC 237 +#define MAVLINK_MSG_ID_114_CRC 237 #define MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW { \ "HIL_OPTICAL_FLOW", \ - 8, \ + 12, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_optical_flow_t, time_usec) }, \ - { "flow_comp_m_x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_optical_flow_t, flow_comp_m_x) }, \ - { "flow_comp_m_y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_optical_flow_t, flow_comp_m_y) }, \ - { "ground_distance", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_optical_flow_t, ground_distance) }, \ - { "flow_x", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_hil_optical_flow_t, flow_x) }, \ - { "flow_y", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_hil_optical_flow_t, flow_y) }, \ - { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_hil_optical_flow_t, sensor_id) }, \ - { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_hil_optical_flow_t, quality) }, \ + { "integration_time_us", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_hil_optical_flow_t, integration_time_us) }, \ + { "integrated_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_optical_flow_t, integrated_x) }, \ + { "integrated_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_optical_flow_t, integrated_y) }, \ + { "integrated_xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_hil_optical_flow_t, integrated_xgyro) }, \ + { "integrated_ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_optical_flow_t, integrated_ygyro) }, \ + { "integrated_zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_optical_flow_t, integrated_zgyro) }, \ + { "time_delta_distance_us", NULL, MAVLINK_TYPE_UINT32_T, 0, 32, offsetof(mavlink_hil_optical_flow_t, time_delta_distance_us) }, \ + { "distance", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_hil_optical_flow_t, distance) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_hil_optical_flow_t, temperature) }, \ + { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_hil_optical_flow_t, sensor_id) }, \ + { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_hil_optical_flow_t, quality) }, \ } \ } @@ -43,39 +51,51 @@ typedef struct __mavlink_hil_optical_flow_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_usec Timestamp (UNIX) + * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param sensor_id Sensor ID - * @param flow_x Flow in pixels in x-sensor direction - * @param flow_y Flow in pixels in y-sensor direction - * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated - * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated - * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality - * @param ground_distance Ground distance in meters. Positive value: distance known. Negative value: Unknown distance + * @param integration_time_us Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + * @param integrated_x Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + * @param integrated_y Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + * @param integrated_xgyro RH rotation around X axis (rad) + * @param integrated_ygyro RH rotation around Y axis (rad) + * @param integrated_zgyro RH rotation around Z axis (rad) + * @param temperature Temperature * 100 in centi-degrees Celsius + * @param quality Optical flow quality / confidence. 0: no valid flow, 255: maximum quality + * @param time_delta_distance_us Time in microseconds since the distance was sampled. + * @param distance Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_hil_optical_flow_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) + uint64_t time_usec, uint8_t sensor_id, uint32_t integration_time_us, float integrated_x, float integrated_y, float integrated_xgyro, float integrated_ygyro, float integrated_zgyro, int16_t temperature, uint8_t quality, uint32_t time_delta_distance_us, float distance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN]; _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, flow_comp_m_x); - _mav_put_float(buf, 12, flow_comp_m_y); - _mav_put_float(buf, 16, ground_distance); - _mav_put_int16_t(buf, 20, flow_x); - _mav_put_int16_t(buf, 22, flow_y); - _mav_put_uint8_t(buf, 24, sensor_id); - _mav_put_uint8_t(buf, 25, quality); + _mav_put_uint32_t(buf, 8, integration_time_us); + _mav_put_float(buf, 12, integrated_x); + _mav_put_float(buf, 16, integrated_y); + _mav_put_float(buf, 20, integrated_xgyro); + _mav_put_float(buf, 24, integrated_ygyro); + _mav_put_float(buf, 28, integrated_zgyro); + _mav_put_uint32_t(buf, 32, time_delta_distance_us); + _mav_put_float(buf, 36, distance); + _mav_put_int16_t(buf, 40, temperature); + _mav_put_uint8_t(buf, 42, sensor_id); + _mav_put_uint8_t(buf, 43, quality); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); #else mavlink_hil_optical_flow_t packet; packet.time_usec = time_usec; - packet.flow_comp_m_x = flow_comp_m_x; - packet.flow_comp_m_y = flow_comp_m_y; - packet.ground_distance = ground_distance; - packet.flow_x = flow_x; - packet.flow_y = flow_y; + packet.integration_time_us = integration_time_us; + packet.integrated_x = integrated_x; + packet.integrated_y = integrated_y; + packet.integrated_xgyro = integrated_xgyro; + packet.integrated_ygyro = integrated_ygyro; + packet.integrated_zgyro = integrated_zgyro; + packet.time_delta_distance_us = time_delta_distance_us; + packet.distance = distance; + packet.temperature = temperature; packet.sensor_id = sensor_id; packet.quality = quality; @@ -96,40 +116,52 @@ static inline uint16_t mavlink_msg_hil_optical_flow_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 time_usec Timestamp (UNIX) + * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param sensor_id Sensor ID - * @param flow_x Flow in pixels in x-sensor direction - * @param flow_y Flow in pixels in y-sensor direction - * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated - * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated - * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality - * @param ground_distance Ground distance in meters. Positive value: distance known. Negative value: Unknown distance + * @param integration_time_us Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + * @param integrated_x Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + * @param integrated_y Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + * @param integrated_xgyro RH rotation around X axis (rad) + * @param integrated_ygyro RH rotation around Y axis (rad) + * @param integrated_zgyro RH rotation around Z axis (rad) + * @param temperature Temperature * 100 in centi-degrees Celsius + * @param quality Optical flow quality / confidence. 0: no valid flow, 255: maximum quality + * @param time_delta_distance_us Time in microseconds since the distance was sampled. + * @param distance Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_hil_optical_flow_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,uint8_t sensor_id,int16_t flow_x,int16_t flow_y,float flow_comp_m_x,float flow_comp_m_y,uint8_t quality,float ground_distance) + uint64_t time_usec,uint8_t sensor_id,uint32_t integration_time_us,float integrated_x,float integrated_y,float integrated_xgyro,float integrated_ygyro,float integrated_zgyro,int16_t temperature,uint8_t quality,uint32_t time_delta_distance_us,float distance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN]; _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, flow_comp_m_x); - _mav_put_float(buf, 12, flow_comp_m_y); - _mav_put_float(buf, 16, ground_distance); - _mav_put_int16_t(buf, 20, flow_x); - _mav_put_int16_t(buf, 22, flow_y); - _mav_put_uint8_t(buf, 24, sensor_id); - _mav_put_uint8_t(buf, 25, quality); + _mav_put_uint32_t(buf, 8, integration_time_us); + _mav_put_float(buf, 12, integrated_x); + _mav_put_float(buf, 16, integrated_y); + _mav_put_float(buf, 20, integrated_xgyro); + _mav_put_float(buf, 24, integrated_ygyro); + _mav_put_float(buf, 28, integrated_zgyro); + _mav_put_uint32_t(buf, 32, time_delta_distance_us); + _mav_put_float(buf, 36, distance); + _mav_put_int16_t(buf, 40, temperature); + _mav_put_uint8_t(buf, 42, sensor_id); + _mav_put_uint8_t(buf, 43, quality); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); #else mavlink_hil_optical_flow_t packet; packet.time_usec = time_usec; - packet.flow_comp_m_x = flow_comp_m_x; - packet.flow_comp_m_y = flow_comp_m_y; - packet.ground_distance = ground_distance; - packet.flow_x = flow_x; - packet.flow_y = flow_y; + packet.integration_time_us = integration_time_us; + packet.integrated_x = integrated_x; + packet.integrated_y = integrated_y; + packet.integrated_xgyro = integrated_xgyro; + packet.integrated_ygyro = integrated_ygyro; + packet.integrated_zgyro = integrated_zgyro; + packet.time_delta_distance_us = time_delta_distance_us; + packet.distance = distance; + packet.temperature = temperature; packet.sensor_id = sensor_id; packet.quality = quality; @@ -154,7 +186,7 @@ static inline uint16_t mavlink_msg_hil_optical_flow_pack_chan(uint8_t system_id, */ static inline uint16_t mavlink_msg_hil_optical_flow_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_optical_flow_t* hil_optical_flow) { - return mavlink_msg_hil_optical_flow_pack(system_id, component_id, msg, hil_optical_flow->time_usec, hil_optical_flow->sensor_id, hil_optical_flow->flow_x, hil_optical_flow->flow_y, hil_optical_flow->flow_comp_m_x, hil_optical_flow->flow_comp_m_y, hil_optical_flow->quality, hil_optical_flow->ground_distance); + return mavlink_msg_hil_optical_flow_pack(system_id, component_id, msg, hil_optical_flow->time_usec, hil_optical_flow->sensor_id, hil_optical_flow->integration_time_us, hil_optical_flow->integrated_x, hil_optical_flow->integrated_y, hil_optical_flow->integrated_xgyro, hil_optical_flow->integrated_ygyro, hil_optical_flow->integrated_zgyro, hil_optical_flow->temperature, hil_optical_flow->quality, hil_optical_flow->time_delta_distance_us, hil_optical_flow->distance); } /** @@ -168,36 +200,44 @@ static inline uint16_t mavlink_msg_hil_optical_flow_encode(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_hil_optical_flow_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_optical_flow_t* hil_optical_flow) { - return mavlink_msg_hil_optical_flow_pack_chan(system_id, component_id, chan, msg, hil_optical_flow->time_usec, hil_optical_flow->sensor_id, hil_optical_flow->flow_x, hil_optical_flow->flow_y, hil_optical_flow->flow_comp_m_x, hil_optical_flow->flow_comp_m_y, hil_optical_flow->quality, hil_optical_flow->ground_distance); + return mavlink_msg_hil_optical_flow_pack_chan(system_id, component_id, chan, msg, hil_optical_flow->time_usec, hil_optical_flow->sensor_id, hil_optical_flow->integration_time_us, hil_optical_flow->integrated_x, hil_optical_flow->integrated_y, hil_optical_flow->integrated_xgyro, hil_optical_flow->integrated_ygyro, hil_optical_flow->integrated_zgyro, hil_optical_flow->temperature, hil_optical_flow->quality, hil_optical_flow->time_delta_distance_us, hil_optical_flow->distance); } /** * @brief Send a hil_optical_flow message * @param chan MAVLink channel to send the message * - * @param time_usec Timestamp (UNIX) + * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param sensor_id Sensor ID - * @param flow_x Flow in pixels in x-sensor direction - * @param flow_y Flow in pixels in y-sensor direction - * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated - * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated - * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality - * @param ground_distance Ground distance in meters. Positive value: distance known. Negative value: Unknown distance + * @param integration_time_us Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + * @param integrated_x Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + * @param integrated_y Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + * @param integrated_xgyro RH rotation around X axis (rad) + * @param integrated_ygyro RH rotation around Y axis (rad) + * @param integrated_zgyro RH rotation around Z axis (rad) + * @param temperature Temperature * 100 in centi-degrees Celsius + * @param quality Optical flow quality / confidence. 0: no valid flow, 255: maximum quality + * @param time_delta_distance_us Time in microseconds since the distance was sampled. + * @param distance Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_hil_optical_flow_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) +static inline void mavlink_msg_hil_optical_flow_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, uint32_t integration_time_us, float integrated_x, float integrated_y, float integrated_xgyro, float integrated_ygyro, float integrated_zgyro, int16_t temperature, uint8_t quality, uint32_t time_delta_distance_us, float distance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN]; _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, flow_comp_m_x); - _mav_put_float(buf, 12, flow_comp_m_y); - _mav_put_float(buf, 16, ground_distance); - _mav_put_int16_t(buf, 20, flow_x); - _mav_put_int16_t(buf, 22, flow_y); - _mav_put_uint8_t(buf, 24, sensor_id); - _mav_put_uint8_t(buf, 25, quality); + _mav_put_uint32_t(buf, 8, integration_time_us); + _mav_put_float(buf, 12, integrated_x); + _mav_put_float(buf, 16, integrated_y); + _mav_put_float(buf, 20, integrated_xgyro); + _mav_put_float(buf, 24, integrated_ygyro); + _mav_put_float(buf, 28, integrated_zgyro); + _mav_put_uint32_t(buf, 32, time_delta_distance_us); + _mav_put_float(buf, 36, distance); + _mav_put_int16_t(buf, 40, temperature); + _mav_put_uint8_t(buf, 42, sensor_id); + _mav_put_uint8_t(buf, 43, quality); #if MAVLINK_CRC_EXTRA _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); @@ -207,11 +247,15 @@ static inline void mavlink_msg_hil_optical_flow_send(mavlink_channel_t chan, uin #else mavlink_hil_optical_flow_t packet; packet.time_usec = time_usec; - packet.flow_comp_m_x = flow_comp_m_x; - packet.flow_comp_m_y = flow_comp_m_y; - packet.ground_distance = ground_distance; - packet.flow_x = flow_x; - packet.flow_y = flow_y; + packet.integration_time_us = integration_time_us; + packet.integrated_x = integrated_x; + packet.integrated_y = integrated_y; + packet.integrated_xgyro = integrated_xgyro; + packet.integrated_ygyro = integrated_ygyro; + packet.integrated_zgyro = integrated_zgyro; + packet.time_delta_distance_us = time_delta_distance_us; + packet.distance = distance; + packet.temperature = temperature; packet.sensor_id = sensor_id; packet.quality = quality; @@ -231,18 +275,22 @@ static inline void mavlink_msg_hil_optical_flow_send(mavlink_channel_t chan, uin is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_hil_optical_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) +static inline void mavlink_msg_hil_optical_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, uint32_t integration_time_us, float integrated_x, float integrated_y, float integrated_xgyro, float integrated_ygyro, float integrated_zgyro, int16_t temperature, uint8_t quality, uint32_t time_delta_distance_us, float distance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, flow_comp_m_x); - _mav_put_float(buf, 12, flow_comp_m_y); - _mav_put_float(buf, 16, ground_distance); - _mav_put_int16_t(buf, 20, flow_x); - _mav_put_int16_t(buf, 22, flow_y); - _mav_put_uint8_t(buf, 24, sensor_id); - _mav_put_uint8_t(buf, 25, quality); + _mav_put_uint32_t(buf, 8, integration_time_us); + _mav_put_float(buf, 12, integrated_x); + _mav_put_float(buf, 16, integrated_y); + _mav_put_float(buf, 20, integrated_xgyro); + _mav_put_float(buf, 24, integrated_ygyro); + _mav_put_float(buf, 28, integrated_zgyro); + _mav_put_uint32_t(buf, 32, time_delta_distance_us); + _mav_put_float(buf, 36, distance); + _mav_put_int16_t(buf, 40, temperature); + _mav_put_uint8_t(buf, 42, sensor_id); + _mav_put_uint8_t(buf, 43, quality); #if MAVLINK_CRC_EXTRA _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); @@ -252,11 +300,15 @@ static inline void mavlink_msg_hil_optical_flow_send_buf(mavlink_message_t *msgb #else mavlink_hil_optical_flow_t *packet = (mavlink_hil_optical_flow_t *)msgbuf; packet->time_usec = time_usec; - packet->flow_comp_m_x = flow_comp_m_x; - packet->flow_comp_m_y = flow_comp_m_y; - packet->ground_distance = ground_distance; - packet->flow_x = flow_x; - packet->flow_y = flow_y; + packet->integration_time_us = integration_time_us; + packet->integrated_x = integrated_x; + packet->integrated_y = integrated_y; + packet->integrated_xgyro = integrated_xgyro; + packet->integrated_ygyro = integrated_ygyro; + packet->integrated_zgyro = integrated_zgyro; + packet->time_delta_distance_us = time_delta_distance_us; + packet->distance = distance; + packet->temperature = temperature; packet->sensor_id = sensor_id; packet->quality = quality; @@ -277,7 +329,7 @@ static inline void mavlink_msg_hil_optical_flow_send_buf(mavlink_message_t *msgb /** * @brief Get field time_usec from hil_optical_flow message * - * @return Timestamp (UNIX) + * @return Timestamp (microseconds, synced to UNIX time or since system boot) */ static inline uint64_t mavlink_msg_hil_optical_flow_get_time_usec(const mavlink_message_t* msg) { @@ -291,67 +343,107 @@ static inline uint64_t mavlink_msg_hil_optical_flow_get_time_usec(const mavlink_ */ static inline uint8_t mavlink_msg_hil_optical_flow_get_sensor_id(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 24); + return _MAV_RETURN_uint8_t(msg, 42); } /** - * @brief Get field flow_x from hil_optical_flow message + * @brief Get field integration_time_us from hil_optical_flow message * - * @return Flow in pixels in x-sensor direction + * @return Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. */ -static inline int16_t mavlink_msg_hil_optical_flow_get_flow_x(const mavlink_message_t* msg) +static inline uint32_t mavlink_msg_hil_optical_flow_get_integration_time_us(const mavlink_message_t* msg) { - return _MAV_RETURN_int16_t(msg, 20); + return _MAV_RETURN_uint32_t(msg, 8); } /** - * @brief Get field flow_y from hil_optical_flow message + * @brief Get field integrated_x from hil_optical_flow message * - * @return Flow in pixels in y-sensor direction + * @return Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) */ -static inline int16_t mavlink_msg_hil_optical_flow_get_flow_y(const mavlink_message_t* msg) +static inline float mavlink_msg_hil_optical_flow_get_integrated_x(const mavlink_message_t* msg) { - return _MAV_RETURN_int16_t(msg, 22); + return _MAV_RETURN_float(msg, 12); } /** - * @brief Get field flow_comp_m_x from hil_optical_flow message + * @brief Get field integrated_y from hil_optical_flow message * - * @return Flow in meters in x-sensor direction, angular-speed compensated + * @return Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) */ -static inline float mavlink_msg_hil_optical_flow_get_flow_comp_m_x(const mavlink_message_t* msg) +static inline float mavlink_msg_hil_optical_flow_get_integrated_y(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 16); } /** - * @brief Get field flow_comp_m_y from hil_optical_flow message + * @brief Get field integrated_xgyro from hil_optical_flow message * - * @return Flow in meters in y-sensor direction, angular-speed compensated + * @return RH rotation around X axis (rad) */ -static inline float mavlink_msg_hil_optical_flow_get_flow_comp_m_y(const mavlink_message_t* msg) +static inline float mavlink_msg_hil_optical_flow_get_integrated_xgyro(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field integrated_ygyro from hil_optical_flow message + * + * @return RH rotation around Y axis (rad) + */ +static inline float mavlink_msg_hil_optical_flow_get_integrated_ygyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field integrated_zgyro from hil_optical_flow message + * + * @return RH rotation around Z axis (rad) + */ +static inline float mavlink_msg_hil_optical_flow_get_integrated_zgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field temperature from hil_optical_flow message + * + * @return Temperature * 100 in centi-degrees Celsius + */ +static inline int16_t mavlink_msg_hil_optical_flow_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 40); } /** * @brief Get field quality from hil_optical_flow message * - * @return Optical flow quality / confidence. 0: bad, 255: maximum quality + * @return Optical flow quality / confidence. 0: no valid flow, 255: maximum quality */ static inline uint8_t mavlink_msg_hil_optical_flow_get_quality(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 25); + return _MAV_RETURN_uint8_t(msg, 43); } /** - * @brief Get field ground_distance from hil_optical_flow message + * @brief Get field time_delta_distance_us from hil_optical_flow message * - * @return Ground distance in meters. Positive value: distance known. Negative value: Unknown distance + * @return Time in microseconds since the distance was sampled. */ -static inline float mavlink_msg_hil_optical_flow_get_ground_distance(const mavlink_message_t* msg) +static inline uint32_t mavlink_msg_hil_optical_flow_get_time_delta_distance_us(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 16); + return _MAV_RETURN_uint32_t(msg, 32); +} + +/** + * @brief Get field distance from hil_optical_flow message + * + * @return Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. + */ +static inline float mavlink_msg_hil_optical_flow_get_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); } /** @@ -364,11 +456,15 @@ static inline void mavlink_msg_hil_optical_flow_decode(const mavlink_message_t* { #if MAVLINK_NEED_BYTE_SWAP hil_optical_flow->time_usec = mavlink_msg_hil_optical_flow_get_time_usec(msg); - hil_optical_flow->flow_comp_m_x = mavlink_msg_hil_optical_flow_get_flow_comp_m_x(msg); - hil_optical_flow->flow_comp_m_y = mavlink_msg_hil_optical_flow_get_flow_comp_m_y(msg); - hil_optical_flow->ground_distance = mavlink_msg_hil_optical_flow_get_ground_distance(msg); - hil_optical_flow->flow_x = mavlink_msg_hil_optical_flow_get_flow_x(msg); - hil_optical_flow->flow_y = mavlink_msg_hil_optical_flow_get_flow_y(msg); + hil_optical_flow->integration_time_us = mavlink_msg_hil_optical_flow_get_integration_time_us(msg); + hil_optical_flow->integrated_x = mavlink_msg_hil_optical_flow_get_integrated_x(msg); + hil_optical_flow->integrated_y = mavlink_msg_hil_optical_flow_get_integrated_y(msg); + hil_optical_flow->integrated_xgyro = mavlink_msg_hil_optical_flow_get_integrated_xgyro(msg); + hil_optical_flow->integrated_ygyro = mavlink_msg_hil_optical_flow_get_integrated_ygyro(msg); + hil_optical_flow->integrated_zgyro = mavlink_msg_hil_optical_flow_get_integrated_zgyro(msg); + hil_optical_flow->time_delta_distance_us = mavlink_msg_hil_optical_flow_get_time_delta_distance_us(msg); + hil_optical_flow->distance = mavlink_msg_hil_optical_flow_get_distance(msg); + hil_optical_flow->temperature = mavlink_msg_hil_optical_flow_get_temperature(msg); hil_optical_flow->sensor_id = mavlink_msg_hil_optical_flow_get_sensor_id(msg); hil_optical_flow->quality = mavlink_msg_hil_optical_flow_get_quality(msg); #else diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow_rad.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow_rad.h index 316dd99..427b56a 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow_rad.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow_rad.h @@ -6,33 +6,41 @@ typedef struct __mavlink_optical_flow_rad_t { uint64_t time_usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot) uint32_t integration_time_us; ///< Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. - float integrated_x; ///< Flow in radians around X axis (in Y direction, clockwise rotation inducing positive speed, motion ALONG the positive Y axis inducing positive flow AROUND the X axis) - float integrated_y; ///< Flow in radians around Y axis (in X direction, clockwise rotation inducing positive speed, motion ALONG the positive X axis inducing positive flow AROUND the Y axis) + float integrated_x; ///< Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + float integrated_y; ///< Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + float integrated_xgyro; ///< RH rotation around X axis (rad) + float integrated_ygyro; ///< RH rotation around Y axis (rad) + float integrated_zgyro; ///< RH rotation around Z axis (rad) uint32_t time_delta_distance_us; ///< Time in microseconds since the distance was sampled. float distance; ///< Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. + int16_t temperature; ///< Temperature * 100 in centi-degrees Celsius uint8_t sensor_id; ///< Sensor ID uint8_t quality; ///< Optical flow quality / confidence. 0: no valid flow, 255: maximum quality } mavlink_optical_flow_rad_t; -#define MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN 30 -#define MAVLINK_MSG_ID_106_LEN 30 +#define MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN 44 +#define MAVLINK_MSG_ID_106_LEN 44 -#define MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_CRC 102 -#define MAVLINK_MSG_ID_106_CRC 102 +#define MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_CRC 138 +#define MAVLINK_MSG_ID_106_CRC 138 #define MAVLINK_MESSAGE_INFO_OPTICAL_FLOW_RAD { \ "OPTICAL_FLOW_RAD", \ - 8, \ + 12, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_optical_flow_rad_t, time_usec) }, \ { "integration_time_us", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_optical_flow_rad_t, integration_time_us) }, \ { "integrated_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_optical_flow_rad_t, integrated_x) }, \ { "integrated_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_optical_flow_rad_t, integrated_y) }, \ - { "time_delta_distance_us", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_optical_flow_rad_t, time_delta_distance_us) }, \ - { "distance", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_optical_flow_rad_t, distance) }, \ - { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_optical_flow_rad_t, sensor_id) }, \ - { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_optical_flow_rad_t, quality) }, \ + { "integrated_xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_optical_flow_rad_t, integrated_xgyro) }, \ + { "integrated_ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_optical_flow_rad_t, integrated_ygyro) }, \ + { "integrated_zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_optical_flow_rad_t, integrated_zgyro) }, \ + { "time_delta_distance_us", NULL, MAVLINK_TYPE_UINT32_T, 0, 32, offsetof(mavlink_optical_flow_rad_t, time_delta_distance_us) }, \ + { "distance", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_optical_flow_rad_t, distance) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_optical_flow_rad_t, temperature) }, \ + { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_optical_flow_rad_t, sensor_id) }, \ + { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_optical_flow_rad_t, quality) }, \ } \ } @@ -46,15 +54,19 @@ typedef struct __mavlink_optical_flow_rad_t * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param sensor_id Sensor ID * @param integration_time_us Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. - * @param integrated_x Flow in radians around X axis (in Y direction, clockwise rotation inducing positive speed, motion ALONG the positive Y axis inducing positive flow AROUND the X axis) - * @param integrated_y Flow in radians around Y axis (in X direction, clockwise rotation inducing positive speed, motion ALONG the positive X axis inducing positive flow AROUND the Y axis) + * @param integrated_x Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + * @param integrated_y Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + * @param integrated_xgyro RH rotation around X axis (rad) + * @param integrated_ygyro RH rotation around Y axis (rad) + * @param integrated_zgyro RH rotation around Z axis (rad) + * @param temperature Temperature * 100 in centi-degrees Celsius * @param quality Optical flow quality / confidence. 0: no valid flow, 255: maximum quality * @param time_delta_distance_us Time in microseconds since the distance was sampled. * @param distance Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_optical_flow_rad_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t sensor_id, uint32_t integration_time_us, float integrated_x, float integrated_y, uint8_t quality, uint32_t time_delta_distance_us, float distance) + uint64_t time_usec, uint8_t sensor_id, uint32_t integration_time_us, float integrated_x, float integrated_y, float integrated_xgyro, float integrated_ygyro, float integrated_zgyro, int16_t temperature, uint8_t quality, uint32_t time_delta_distance_us, float distance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN]; @@ -62,10 +74,14 @@ static inline uint16_t mavlink_msg_optical_flow_rad_pack(uint8_t system_id, uint _mav_put_uint32_t(buf, 8, integration_time_us); _mav_put_float(buf, 12, integrated_x); _mav_put_float(buf, 16, integrated_y); - _mav_put_uint32_t(buf, 20, time_delta_distance_us); - _mav_put_float(buf, 24, distance); - _mav_put_uint8_t(buf, 28, sensor_id); - _mav_put_uint8_t(buf, 29, quality); + _mav_put_float(buf, 20, integrated_xgyro); + _mav_put_float(buf, 24, integrated_ygyro); + _mav_put_float(buf, 28, integrated_zgyro); + _mav_put_uint32_t(buf, 32, time_delta_distance_us); + _mav_put_float(buf, 36, distance); + _mav_put_int16_t(buf, 40, temperature); + _mav_put_uint8_t(buf, 42, sensor_id); + _mav_put_uint8_t(buf, 43, quality); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); #else @@ -74,8 +90,12 @@ static inline uint16_t mavlink_msg_optical_flow_rad_pack(uint8_t system_id, uint packet.integration_time_us = integration_time_us; packet.integrated_x = integrated_x; packet.integrated_y = integrated_y; + packet.integrated_xgyro = integrated_xgyro; + packet.integrated_ygyro = integrated_ygyro; + packet.integrated_zgyro = integrated_zgyro; packet.time_delta_distance_us = time_delta_distance_us; packet.distance = distance; + packet.temperature = temperature; packet.sensor_id = sensor_id; packet.quality = quality; @@ -99,8 +119,12 @@ static inline uint16_t mavlink_msg_optical_flow_rad_pack(uint8_t system_id, uint * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param sensor_id Sensor ID * @param integration_time_us Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. - * @param integrated_x Flow in radians around X axis (in Y direction, clockwise rotation inducing positive speed, motion ALONG the positive Y axis inducing positive flow AROUND the X axis) - * @param integrated_y Flow in radians around Y axis (in X direction, clockwise rotation inducing positive speed, motion ALONG the positive X axis inducing positive flow AROUND the Y axis) + * @param integrated_x Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + * @param integrated_y Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + * @param integrated_xgyro RH rotation around X axis (rad) + * @param integrated_ygyro RH rotation around Y axis (rad) + * @param integrated_zgyro RH rotation around Z axis (rad) + * @param temperature Temperature * 100 in centi-degrees Celsius * @param quality Optical flow quality / confidence. 0: no valid flow, 255: maximum quality * @param time_delta_distance_us Time in microseconds since the distance was sampled. * @param distance Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. @@ -108,7 +132,7 @@ static inline uint16_t mavlink_msg_optical_flow_rad_pack(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_optical_flow_rad_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,uint8_t sensor_id,uint32_t integration_time_us,float integrated_x,float integrated_y,uint8_t quality,uint32_t time_delta_distance_us,float distance) + uint64_t time_usec,uint8_t sensor_id,uint32_t integration_time_us,float integrated_x,float integrated_y,float integrated_xgyro,float integrated_ygyro,float integrated_zgyro,int16_t temperature,uint8_t quality,uint32_t time_delta_distance_us,float distance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN]; @@ -116,10 +140,14 @@ static inline uint16_t mavlink_msg_optical_flow_rad_pack_chan(uint8_t system_id, _mav_put_uint32_t(buf, 8, integration_time_us); _mav_put_float(buf, 12, integrated_x); _mav_put_float(buf, 16, integrated_y); - _mav_put_uint32_t(buf, 20, time_delta_distance_us); - _mav_put_float(buf, 24, distance); - _mav_put_uint8_t(buf, 28, sensor_id); - _mav_put_uint8_t(buf, 29, quality); + _mav_put_float(buf, 20, integrated_xgyro); + _mav_put_float(buf, 24, integrated_ygyro); + _mav_put_float(buf, 28, integrated_zgyro); + _mav_put_uint32_t(buf, 32, time_delta_distance_us); + _mav_put_float(buf, 36, distance); + _mav_put_int16_t(buf, 40, temperature); + _mav_put_uint8_t(buf, 42, sensor_id); + _mav_put_uint8_t(buf, 43, quality); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); #else @@ -128,8 +156,12 @@ static inline uint16_t mavlink_msg_optical_flow_rad_pack_chan(uint8_t system_id, packet.integration_time_us = integration_time_us; packet.integrated_x = integrated_x; packet.integrated_y = integrated_y; + packet.integrated_xgyro = integrated_xgyro; + packet.integrated_ygyro = integrated_ygyro; + packet.integrated_zgyro = integrated_zgyro; packet.time_delta_distance_us = time_delta_distance_us; packet.distance = distance; + packet.temperature = temperature; packet.sensor_id = sensor_id; packet.quality = quality; @@ -154,7 +186,7 @@ static inline uint16_t mavlink_msg_optical_flow_rad_pack_chan(uint8_t system_id, */ static inline uint16_t mavlink_msg_optical_flow_rad_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_optical_flow_rad_t* optical_flow_rad) { - return mavlink_msg_optical_flow_rad_pack(system_id, component_id, msg, optical_flow_rad->time_usec, optical_flow_rad->sensor_id, optical_flow_rad->integration_time_us, optical_flow_rad->integrated_x, optical_flow_rad->integrated_y, optical_flow_rad->quality, optical_flow_rad->time_delta_distance_us, optical_flow_rad->distance); + return mavlink_msg_optical_flow_rad_pack(system_id, component_id, msg, optical_flow_rad->time_usec, optical_flow_rad->sensor_id, optical_flow_rad->integration_time_us, optical_flow_rad->integrated_x, optical_flow_rad->integrated_y, optical_flow_rad->integrated_xgyro, optical_flow_rad->integrated_ygyro, optical_flow_rad->integrated_zgyro, optical_flow_rad->temperature, optical_flow_rad->quality, optical_flow_rad->time_delta_distance_us, optical_flow_rad->distance); } /** @@ -168,7 +200,7 @@ static inline uint16_t mavlink_msg_optical_flow_rad_encode(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_optical_flow_rad_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_optical_flow_rad_t* optical_flow_rad) { - return mavlink_msg_optical_flow_rad_pack_chan(system_id, component_id, chan, msg, optical_flow_rad->time_usec, optical_flow_rad->sensor_id, optical_flow_rad->integration_time_us, optical_flow_rad->integrated_x, optical_flow_rad->integrated_y, optical_flow_rad->quality, optical_flow_rad->time_delta_distance_us, optical_flow_rad->distance); + return mavlink_msg_optical_flow_rad_pack_chan(system_id, component_id, chan, msg, optical_flow_rad->time_usec, optical_flow_rad->sensor_id, optical_flow_rad->integration_time_us, optical_flow_rad->integrated_x, optical_flow_rad->integrated_y, optical_flow_rad->integrated_xgyro, optical_flow_rad->integrated_ygyro, optical_flow_rad->integrated_zgyro, optical_flow_rad->temperature, optical_flow_rad->quality, optical_flow_rad->time_delta_distance_us, optical_flow_rad->distance); } /** @@ -178,15 +210,19 @@ static inline uint16_t mavlink_msg_optical_flow_rad_encode_chan(uint8_t system_i * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param sensor_id Sensor ID * @param integration_time_us Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. - * @param integrated_x Flow in radians around X axis (in Y direction, clockwise rotation inducing positive speed, motion ALONG the positive Y axis inducing positive flow AROUND the X axis) - * @param integrated_y Flow in radians around Y axis (in X direction, clockwise rotation inducing positive speed, motion ALONG the positive X axis inducing positive flow AROUND the Y axis) + * @param integrated_x Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + * @param integrated_y Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + * @param integrated_xgyro RH rotation around X axis (rad) + * @param integrated_ygyro RH rotation around Y axis (rad) + * @param integrated_zgyro RH rotation around Z axis (rad) + * @param temperature Temperature * 100 in centi-degrees Celsius * @param quality Optical flow quality / confidence. 0: no valid flow, 255: maximum quality * @param time_delta_distance_us Time in microseconds since the distance was sampled. * @param distance Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_optical_flow_rad_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, uint32_t integration_time_us, float integrated_x, float integrated_y, uint8_t quality, uint32_t time_delta_distance_us, float distance) +static inline void mavlink_msg_optical_flow_rad_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, uint32_t integration_time_us, float integrated_x, float integrated_y, float integrated_xgyro, float integrated_ygyro, float integrated_zgyro, int16_t temperature, uint8_t quality, uint32_t time_delta_distance_us, float distance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN]; @@ -194,10 +230,14 @@ static inline void mavlink_msg_optical_flow_rad_send(mavlink_channel_t chan, uin _mav_put_uint32_t(buf, 8, integration_time_us); _mav_put_float(buf, 12, integrated_x); _mav_put_float(buf, 16, integrated_y); - _mav_put_uint32_t(buf, 20, time_delta_distance_us); - _mav_put_float(buf, 24, distance); - _mav_put_uint8_t(buf, 28, sensor_id); - _mav_put_uint8_t(buf, 29, quality); + _mav_put_float(buf, 20, integrated_xgyro); + _mav_put_float(buf, 24, integrated_ygyro); + _mav_put_float(buf, 28, integrated_zgyro); + _mav_put_uint32_t(buf, 32, time_delta_distance_us); + _mav_put_float(buf, 36, distance); + _mav_put_int16_t(buf, 40, temperature); + _mav_put_uint8_t(buf, 42, sensor_id); + _mav_put_uint8_t(buf, 43, quality); #if MAVLINK_CRC_EXTRA _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_CRC); @@ -210,8 +250,12 @@ static inline void mavlink_msg_optical_flow_rad_send(mavlink_channel_t chan, uin packet.integration_time_us = integration_time_us; packet.integrated_x = integrated_x; packet.integrated_y = integrated_y; + packet.integrated_xgyro = integrated_xgyro; + packet.integrated_ygyro = integrated_ygyro; + packet.integrated_zgyro = integrated_zgyro; packet.time_delta_distance_us = time_delta_distance_us; packet.distance = distance; + packet.temperature = temperature; packet.sensor_id = sensor_id; packet.quality = quality; @@ -231,7 +275,7 @@ static inline void mavlink_msg_optical_flow_rad_send(mavlink_channel_t chan, uin is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_optical_flow_rad_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, uint32_t integration_time_us, float integrated_x, float integrated_y, uint8_t quality, uint32_t time_delta_distance_us, float distance) +static inline void mavlink_msg_optical_flow_rad_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, uint32_t integration_time_us, float integrated_x, float integrated_y, float integrated_xgyro, float integrated_ygyro, float integrated_zgyro, int16_t temperature, uint8_t quality, uint32_t time_delta_distance_us, float distance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -239,10 +283,14 @@ static inline void mavlink_msg_optical_flow_rad_send_buf(mavlink_message_t *msgb _mav_put_uint32_t(buf, 8, integration_time_us); _mav_put_float(buf, 12, integrated_x); _mav_put_float(buf, 16, integrated_y); - _mav_put_uint32_t(buf, 20, time_delta_distance_us); - _mav_put_float(buf, 24, distance); - _mav_put_uint8_t(buf, 28, sensor_id); - _mav_put_uint8_t(buf, 29, quality); + _mav_put_float(buf, 20, integrated_xgyro); + _mav_put_float(buf, 24, integrated_ygyro); + _mav_put_float(buf, 28, integrated_zgyro); + _mav_put_uint32_t(buf, 32, time_delta_distance_us); + _mav_put_float(buf, 36, distance); + _mav_put_int16_t(buf, 40, temperature); + _mav_put_uint8_t(buf, 42, sensor_id); + _mav_put_uint8_t(buf, 43, quality); #if MAVLINK_CRC_EXTRA _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_CRC); @@ -255,8 +303,12 @@ static inline void mavlink_msg_optical_flow_rad_send_buf(mavlink_message_t *msgb packet->integration_time_us = integration_time_us; packet->integrated_x = integrated_x; packet->integrated_y = integrated_y; + packet->integrated_xgyro = integrated_xgyro; + packet->integrated_ygyro = integrated_ygyro; + packet->integrated_zgyro = integrated_zgyro; packet->time_delta_distance_us = time_delta_distance_us; packet->distance = distance; + packet->temperature = temperature; packet->sensor_id = sensor_id; packet->quality = quality; @@ -291,7 +343,7 @@ static inline uint64_t mavlink_msg_optical_flow_rad_get_time_usec(const mavlink_ */ static inline uint8_t mavlink_msg_optical_flow_rad_get_sensor_id(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 28); + return _MAV_RETURN_uint8_t(msg, 42); } /** @@ -307,7 +359,7 @@ static inline uint32_t mavlink_msg_optical_flow_rad_get_integration_time_us(cons /** * @brief Get field integrated_x from optical_flow_rad message * - * @return Flow in radians around X axis (in Y direction, clockwise rotation inducing positive speed, motion ALONG the positive Y axis inducing positive flow AROUND the X axis) + * @return Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) */ static inline float mavlink_msg_optical_flow_rad_get_integrated_x(const mavlink_message_t* msg) { @@ -317,13 +369,53 @@ static inline float mavlink_msg_optical_flow_rad_get_integrated_x(const mavlink_ /** * @brief Get field integrated_y from optical_flow_rad message * - * @return Flow in radians around Y axis (in X direction, clockwise rotation inducing positive speed, motion ALONG the positive X axis inducing positive flow AROUND the Y axis) + * @return Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) */ static inline float mavlink_msg_optical_flow_rad_get_integrated_y(const mavlink_message_t* msg) { return _MAV_RETURN_float(msg, 16); } +/** + * @brief Get field integrated_xgyro from optical_flow_rad message + * + * @return RH rotation around X axis (rad) + */ +static inline float mavlink_msg_optical_flow_rad_get_integrated_xgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field integrated_ygyro from optical_flow_rad message + * + * @return RH rotation around Y axis (rad) + */ +static inline float mavlink_msg_optical_flow_rad_get_integrated_ygyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field integrated_zgyro from optical_flow_rad message + * + * @return RH rotation around Z axis (rad) + */ +static inline float mavlink_msg_optical_flow_rad_get_integrated_zgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field temperature from optical_flow_rad message + * + * @return Temperature * 100 in centi-degrees Celsius + */ +static inline int16_t mavlink_msg_optical_flow_rad_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 40); +} + /** * @brief Get field quality from optical_flow_rad message * @@ -331,7 +423,7 @@ static inline float mavlink_msg_optical_flow_rad_get_integrated_y(const mavlink_ */ static inline uint8_t mavlink_msg_optical_flow_rad_get_quality(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 29); + return _MAV_RETURN_uint8_t(msg, 43); } /** @@ -341,7 +433,7 @@ static inline uint8_t mavlink_msg_optical_flow_rad_get_quality(const mavlink_mes */ static inline uint32_t mavlink_msg_optical_flow_rad_get_time_delta_distance_us(const mavlink_message_t* msg) { - return _MAV_RETURN_uint32_t(msg, 20); + return _MAV_RETURN_uint32_t(msg, 32); } /** @@ -351,7 +443,7 @@ static inline uint32_t mavlink_msg_optical_flow_rad_get_time_delta_distance_us(c */ static inline float mavlink_msg_optical_flow_rad_get_distance(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 24); + return _MAV_RETURN_float(msg, 36); } /** @@ -367,8 +459,12 @@ static inline void mavlink_msg_optical_flow_rad_decode(const mavlink_message_t* optical_flow_rad->integration_time_us = mavlink_msg_optical_flow_rad_get_integration_time_us(msg); optical_flow_rad->integrated_x = mavlink_msg_optical_flow_rad_get_integrated_x(msg); optical_flow_rad->integrated_y = mavlink_msg_optical_flow_rad_get_integrated_y(msg); + optical_flow_rad->integrated_xgyro = mavlink_msg_optical_flow_rad_get_integrated_xgyro(msg); + optical_flow_rad->integrated_ygyro = mavlink_msg_optical_flow_rad_get_integrated_ygyro(msg); + optical_flow_rad->integrated_zgyro = mavlink_msg_optical_flow_rad_get_integrated_zgyro(msg); optical_flow_rad->time_delta_distance_us = mavlink_msg_optical_flow_rad_get_time_delta_distance_us(msg); optical_flow_rad->distance = mavlink_msg_optical_flow_rad_get_distance(msg); + optical_flow_rad->temperature = mavlink_msg_optical_flow_rad_get_temperature(msg); optical_flow_rad->sensor_id = mavlink_msg_optical_flow_rad_get_sensor_id(msg); optical_flow_rad->quality = mavlink_msg_optical_flow_rad_get_quality(msg); #else diff --git a/mavlink/include/mavlink/v1.0/common/testsuite.h b/mavlink/include/mavlink/v1.0/common/testsuite.h index 4c51037..6ccbd92 100644 --- a/mavlink/include/mavlink/v1.0/common/testsuite.h +++ b/mavlink/include/mavlink/v1.0/common/testsuite.h @@ -3540,7 +3540,7 @@ static void mavlink_test_optical_flow_rad(uint8_t system_id, uint8_t component_i uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_optical_flow_rad_t packet_in = { - 93372036854775807ULL,963497880,101.0,129.0,963498504,185.0,89,156 + 93372036854775807ULL,963497880,101.0,129.0,157.0,185.0,213.0,963499128,269.0,19315,3,70 }; mavlink_optical_flow_rad_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3548,8 +3548,12 @@ static void mavlink_test_optical_flow_rad(uint8_t system_id, uint8_t component_i packet1.integration_time_us = packet_in.integration_time_us; packet1.integrated_x = packet_in.integrated_x; packet1.integrated_y = packet_in.integrated_y; + packet1.integrated_xgyro = packet_in.integrated_xgyro; + packet1.integrated_ygyro = packet_in.integrated_ygyro; + packet1.integrated_zgyro = packet_in.integrated_zgyro; packet1.time_delta_distance_us = packet_in.time_delta_distance_us; packet1.distance = packet_in.distance; + packet1.temperature = packet_in.temperature; packet1.sensor_id = packet_in.sensor_id; packet1.quality = packet_in.quality; @@ -3561,12 +3565,12 @@ static void mavlink_test_optical_flow_rad(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_optical_flow_rad_pack(system_id, component_id, &msg , packet1.time_usec , packet1.sensor_id , packet1.integration_time_us , packet1.integrated_x , packet1.integrated_y , packet1.quality , packet1.time_delta_distance_us , packet1.distance ); + mavlink_msg_optical_flow_rad_pack(system_id, component_id, &msg , packet1.time_usec , packet1.sensor_id , packet1.integration_time_us , packet1.integrated_x , packet1.integrated_y , packet1.integrated_xgyro , packet1.integrated_ygyro , packet1.integrated_zgyro , packet1.temperature , packet1.quality , packet1.time_delta_distance_us , packet1.distance ); mavlink_msg_optical_flow_rad_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_optical_flow_rad_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.sensor_id , packet1.integration_time_us , packet1.integrated_x , packet1.integrated_y , packet1.quality , packet1.time_delta_distance_us , packet1.distance ); + mavlink_msg_optical_flow_rad_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.sensor_id , packet1.integration_time_us , packet1.integrated_x , packet1.integrated_y , packet1.integrated_xgyro , packet1.integrated_ygyro , packet1.integrated_zgyro , packet1.temperature , packet1.quality , packet1.time_delta_distance_us , packet1.distance ); mavlink_msg_optical_flow_rad_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -3579,7 +3583,7 @@ static void mavlink_test_optical_flow_rad(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.sensor_id , packet1.integration_time_us , packet1.integrated_x , packet1.integrated_y , packet1.quality , packet1.time_delta_distance_us , packet1.distance ); + mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.sensor_id , packet1.integration_time_us , packet1.integrated_x , packet1.integrated_y , packet1.integrated_xgyro , packet1.integrated_ygyro , packet1.integrated_zgyro , packet1.temperature , packet1.quality , packet1.time_delta_distance_us , packet1.distance ); mavlink_msg_optical_flow_rad_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -3860,16 +3864,20 @@ static void mavlink_test_hil_optical_flow(uint8_t system_id, uint8_t component_i uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_hil_optical_flow_t packet_in = { - 93372036854775807ULL,73.0,101.0,129.0,18275,18379,77,144 + 93372036854775807ULL,963497880,101.0,129.0,157.0,185.0,213.0,963499128,269.0,19315,3,70 }; mavlink_hil_optical_flow_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.time_usec = packet_in.time_usec; - packet1.flow_comp_m_x = packet_in.flow_comp_m_x; - packet1.flow_comp_m_y = packet_in.flow_comp_m_y; - packet1.ground_distance = packet_in.ground_distance; - packet1.flow_x = packet_in.flow_x; - packet1.flow_y = packet_in.flow_y; + packet1.integration_time_us = packet_in.integration_time_us; + packet1.integrated_x = packet_in.integrated_x; + packet1.integrated_y = packet_in.integrated_y; + packet1.integrated_xgyro = packet_in.integrated_xgyro; + packet1.integrated_ygyro = packet_in.integrated_ygyro; + packet1.integrated_zgyro = packet_in.integrated_zgyro; + packet1.time_delta_distance_us = packet_in.time_delta_distance_us; + packet1.distance = packet_in.distance; + packet1.temperature = packet_in.temperature; packet1.sensor_id = packet_in.sensor_id; packet1.quality = packet_in.quality; @@ -3881,12 +3889,12 @@ static void mavlink_test_hil_optical_flow(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_hil_optical_flow_pack(system_id, component_id, &msg , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance ); + mavlink_msg_hil_optical_flow_pack(system_id, component_id, &msg , packet1.time_usec , packet1.sensor_id , packet1.integration_time_us , packet1.integrated_x , packet1.integrated_y , packet1.integrated_xgyro , packet1.integrated_ygyro , packet1.integrated_zgyro , packet1.temperature , packet1.quality , packet1.time_delta_distance_us , packet1.distance ); mavlink_msg_hil_optical_flow_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_hil_optical_flow_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance ); + mavlink_msg_hil_optical_flow_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.sensor_id , packet1.integration_time_us , packet1.integrated_x , packet1.integrated_y , packet1.integrated_xgyro , packet1.integrated_ygyro , packet1.integrated_zgyro , packet1.temperature , packet1.quality , packet1.time_delta_distance_us , packet1.distance ); mavlink_msg_hil_optical_flow_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -3899,7 +3907,7 @@ static void mavlink_test_hil_optical_flow(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_hil_optical_flow_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance ); + mavlink_msg_hil_optical_flow_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.sensor_id , packet1.integration_time_us , packet1.integrated_x , packet1.integrated_y , packet1.integrated_xgyro , packet1.integrated_ygyro , packet1.integrated_zgyro , packet1.temperature , packet1.quality , packet1.time_delta_distance_us , packet1.distance ); mavlink_msg_hil_optical_flow_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } diff --git a/mavlink/include/mavlink/v1.0/common/version.h b/mavlink/include/mavlink/v1.0/common/version.h index 0d1e547..3a639f8 100644 --- a/mavlink/include/mavlink/v1.0/common/version.h +++ b/mavlink/include/mavlink/v1.0/common/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Wed Oct 29 16:57:54 2014" +#define MAVLINK_BUILD_DATE "Thu Oct 30 15:09:14 2014" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 From 876d0fd06ee733ecdda27d5e9abb85c2cabff21a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 30 Oct 2014 15:38:22 +0100 Subject: [PATCH 09/14] Removed unused dialect --- .../include/mavlink/v1.0/minimal/mavlink.h | 27 -- .../v1.0/minimal/mavlink_msg_heartbeat.h | 326 ------------------ .../include/mavlink/v1.0/minimal/minimal.h | 154 --------- .../include/mavlink/v1.0/minimal/testsuite.h | 83 ----- .../include/mavlink/v1.0/minimal/version.h | 12 - 5 files changed, 602 deletions(-) delete mode 100644 mavlink/include/mavlink/v1.0/minimal/mavlink.h delete mode 100644 mavlink/include/mavlink/v1.0/minimal/mavlink_msg_heartbeat.h delete mode 100644 mavlink/include/mavlink/v1.0/minimal/minimal.h delete mode 100644 mavlink/include/mavlink/v1.0/minimal/testsuite.h delete mode 100644 mavlink/include/mavlink/v1.0/minimal/version.h diff --git a/mavlink/include/mavlink/v1.0/minimal/mavlink.h b/mavlink/include/mavlink/v1.0/minimal/mavlink.h deleted file mode 100644 index 396f7da..0000000 --- a/mavlink/include/mavlink/v1.0/minimal/mavlink.h +++ /dev/null @@ -1,27 +0,0 @@ -/** @file - * @brief MAVLink comm protocol built from minimal.xml - * @see http://mavlink.org - */ -#ifndef MAVLINK_H -#define MAVLINK_H - -#ifndef MAVLINK_STX -#define MAVLINK_STX 254 -#endif - -#ifndef MAVLINK_ENDIAN -#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN -#endif - -#ifndef MAVLINK_ALIGNED_FIELDS -#define MAVLINK_ALIGNED_FIELDS 1 -#endif - -#ifndef MAVLINK_CRC_EXTRA -#define MAVLINK_CRC_EXTRA 1 -#endif - -#include "version.h" -#include "minimal.h" - -#endif // MAVLINK_H diff --git a/mavlink/include/mavlink/v1.0/minimal/mavlink_msg_heartbeat.h b/mavlink/include/mavlink/v1.0/minimal/mavlink_msg_heartbeat.h deleted file mode 100644 index 37e682a..0000000 --- a/mavlink/include/mavlink/v1.0/minimal/mavlink_msg_heartbeat.h +++ /dev/null @@ -1,326 +0,0 @@ -// 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_FLAGS ENUM in mavlink/include/mavlink_types.h - uint8_t system_status; ///< System status flag, see MAV_STATE ENUM - uint8_t mavlink_version; ///< MAVLink version -} mavlink_heartbeat_t; - -#define MAVLINK_MSG_ID_HEARTBEAT_LEN 9 -#define MAVLINK_MSG_ID_0_LEN 9 - -#define MAVLINK_MSG_ID_HEARTBEAT_CRC 50 -#define MAVLINK_MSG_ID_0_CRC 50 - - - -#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) }, \ - { "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) }, \ - { "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) }, \ - } \ -} - - -/** - * @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 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_FLAGS 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) -{ -#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); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_LEN); -#endif -} - -/** - * @brief Pack a heartbeat message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param 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_FLAGS 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) -{ -#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_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 -} - -/** - * @brief Encode a heartbeat struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param heartbeat C-struct to read the message contents from - */ -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); -} - -/** - * @brief Encode a heartbeat struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param heartbeat C-struct to read the message contents from - */ -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); -} - -/** - * @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_FLAGS 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); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN); -#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); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); -#endif -#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 - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_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); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE HEARTBEAT UNPACKING - - -/** - * @brief Get field type from heartbeat message - * - * @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); -} - -/** - * @brief Get field autopilot from heartbeat message - * - * @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); -} - -/** - * @brief Get field base_mode from heartbeat message - * - * @return System mode bitfield, see MAV_MODE_FLAGS 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); -} - -/** - * @brief Get field custom_mode from heartbeat message - * - * @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); -} - -/** - * @brief Get field system_status from heartbeat message - * - * @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); -} - -/** - * @brief Get field mavlink_version from heartbeat message - * - * @return MAVLink version - */ -static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 8); -} - -/** - * @brief Decode a heartbeat message into a struct - * - * @param msg The message to decode - * @param heartbeat C-struct to decode the message contents into - */ -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); -#else - memcpy(heartbeat, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HEARTBEAT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/minimal/minimal.h b/mavlink/include/mavlink/v1.0/minimal/minimal.h deleted file mode 100644 index 6935d82..0000000 --- a/mavlink/include/mavlink/v1.0/minimal/minimal.h +++ /dev/null @@ -1,154 +0,0 @@ -/** @file - * @brief MAVLink comm protocol generated from minimal.xml - * @see http://mavlink.org - */ -#ifndef MAVLINK_MINIMAL_H -#define MAVLINK_MINIMAL_H - -#ifndef MAVLINK_H - #error Wrong include order: MAVLINK_MINIMAL.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call. -#endif - -#ifdef __cplusplus -extern "C" { -#endif - -// MESSAGE LENGTHS AND CRCS - -#ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 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, 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, 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, 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} -#endif - -#ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 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, 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, 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, 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} -#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}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} -#endif - -#include "../protocol.h" - -#define MAVLINK_ENABLED_MINIMAL - -// ENUM DEFINITIONS - - -/** @brief Micro air vehicle / autopilot classes. This identifies the individual model. */ -#ifndef HAVE_ENUM_MAV_AUTOPILOT -#define HAVE_ENUM_MAV_AUTOPILOT -typedef enum MAV_AUTOPILOT -{ - MAV_AUTOPILOT_GENERIC=0, /* Generic autopilot, full support for everything | */ - MAV_AUTOPILOT_PIXHAWK=1, /* PIXHAWK autopilot, http://pixhawk.ethz.ch | */ - MAV_AUTOPILOT_SLUGS=2, /* SLUGS autopilot, http://slugsuav.soe.ucsc.edu | */ - MAV_AUTOPILOT_ARDUPILOTMEGA=3, /* ArduPilotMega / ArduCopter, http://diydrones.com | */ - MAV_AUTOPILOT_OPENPILOT=4, /* OpenPilot, http://openpilot.org | */ - MAV_AUTOPILOT_GENERIC_WAYPOINTS_ONLY=5, /* Generic autopilot only supporting simple waypoints | */ - MAV_AUTOPILOT_GENERIC_WAYPOINTS_AND_SIMPLE_NAVIGATION_ONLY=6, /* Generic autopilot supporting waypoints and other simple navigation commands | */ - MAV_AUTOPILOT_GENERIC_MISSION_FULL=7, /* Generic autopilot supporting the full mission command set | */ - MAV_AUTOPILOT_INVALID=8, /* No valid autopilot, e.g. a GCS or other MAVLink component | */ - MAV_AUTOPILOT_PPZ=9, /* PPZ UAV - http://nongnu.org/paparazzi | */ - MAV_AUTOPILOT_UDB=10, /* UAV Dev Board | */ - MAV_AUTOPILOT_FP=11, /* FlexiPilot | */ - MAV_AUTOPILOT_ENUM_END=12, /* | */ -} MAV_AUTOPILOT; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_MAV_TYPE -#define HAVE_ENUM_MAV_TYPE -typedef enum MAV_TYPE -{ - MAV_TYPE_GENERIC=0, /* Generic micro air vehicle. | */ - MAV_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */ - MAV_TYPE_QUADROTOR=2, /* Quadrotor | */ - MAV_TYPE_COAXIAL=3, /* Coaxial helicopter | */ - MAV_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */ - MAV_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */ - MAV_TYPE_GCS=6, /* Operator control unit / ground control station | */ - MAV_TYPE_AIRSHIP=7, /* Airship, controlled | */ - MAV_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */ - MAV_TYPE_ROCKET=9, /* Rocket | */ - MAV_TYPE_GROUND_ROVER=10, /* Ground rover | */ - MAV_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */ - MAV_TYPE_SUBMARINE=12, /* Submarine | */ - MAV_TYPE_HEXAROTOR=13, /* Hexarotor | */ - MAV_TYPE_OCTOROTOR=14, /* Octorotor | */ - MAV_TYPE_TRICOPTER=15, /* Octorotor | */ - MAV_TYPE_FLAPPING_WING=16, /* Flapping wing | */ - MAV_TYPE_ENUM_END=17, /* | */ -} MAV_TYPE; -#endif - -/** @brief These flags encode the MAV mode. */ -#ifndef HAVE_ENUM_MAV_MODE_FLAG -#define HAVE_ENUM_MAV_MODE_FLAG -typedef enum MAV_MODE_FLAG -{ - MAV_MODE_FLAG_CUSTOM_MODE_ENABLED=1, /* 0b00000001 Reserved for future use. | */ - MAV_MODE_FLAG_TEST_ENABLED=2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ - MAV_MODE_FLAG_AUTO_ENABLED=4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ - MAV_MODE_FLAG_GUIDED_ENABLED=8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */ - MAV_MODE_FLAG_STABILIZE_ENABLED=16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ - MAV_MODE_FLAG_HIL_ENABLED=32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ - MAV_MODE_FLAG_MANUAL_INPUT_ENABLED=64, /* 0b01000000 remote control input is enabled. | */ - MAV_MODE_FLAG_SAFETY_ARMED=128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */ - MAV_MODE_FLAG_ENUM_END=129, /* | */ -} MAV_MODE_FLAG; -#endif - -/** @brief These values encode the bit positions of the decode position. These values can be used to read the value of a flag bit by combining the base_mode variable with AND with the flag position value. The result will be either 0 or 1, depending on if the flag is set or not. */ -#ifndef HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION -#define HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION -typedef enum MAV_MODE_FLAG_DECODE_POSITION -{ - MAV_MODE_FLAG_DECODE_POSITION_CUSTOM_MODE=1, /* Eighth bit: 00000001 | */ - MAV_MODE_FLAG_DECODE_POSITION_TEST=2, /* Seventh bit: 00000010 | */ - MAV_MODE_FLAG_DECODE_POSITION_AUTO=4, /* Sixt bit: 00000100 | */ - MAV_MODE_FLAG_DECODE_POSITION_GUIDED=8, /* Fifth bit: 00001000 | */ - MAV_MODE_FLAG_DECODE_POSITION_STABILIZE=16, /* Fourth bit: 00010000 | */ - MAV_MODE_FLAG_DECODE_POSITION_HIL=32, /* Third bit: 00100000 | */ - MAV_MODE_FLAG_DECODE_POSITION_MANUAL=64, /* Second bit: 01000000 | */ - MAV_MODE_FLAG_DECODE_POSITION_SAFETY=128, /* First bit: 10000000 | */ - MAV_MODE_FLAG_DECODE_POSITION_ENUM_END=129, /* | */ -} MAV_MODE_FLAG_DECODE_POSITION; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_MAV_STATE -#define HAVE_ENUM_MAV_STATE -typedef enum MAV_STATE -{ - MAV_STATE_UNINIT=0, /* Uninitialized system, state is unknown. | */ - MAV_STATE_BOOT=1, /* System is booting up. | */ - MAV_STATE_CALIBRATING=2, /* System is calibrating and not flight-ready. | */ - MAV_STATE_STANDBY=3, /* System is grounded and on standby. It can be launched any time. | */ - MAV_STATE_ACTIVE=4, /* System is active and might be already airborne. Motors are engaged. | */ - MAV_STATE_CRITICAL=5, /* System is in a non-normal flight mode. It can however still navigate. | */ - MAV_STATE_EMERGENCY=6, /* System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. | */ - MAV_STATE_POWEROFF=7, /* System just initialized its power-down sequence, will shut down now. | */ - MAV_STATE_ENUM_END=8, /* | */ -} MAV_STATE; -#endif - - - -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -// MESSAGE DEFINITIONS -#include "./mavlink_msg_heartbeat.h" - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // MAVLINK_MINIMAL_H diff --git a/mavlink/include/mavlink/v1.0/minimal/testsuite.h b/mavlink/include/mavlink/v1.0/minimal/testsuite.h deleted file mode 100644 index 8e02237..0000000 --- a/mavlink/include/mavlink/v1.0/minimal/testsuite.h +++ /dev/null @@ -1,83 +0,0 @@ -/** @file - * @brief MAVLink comm protocol testsuite generated from minimal.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef MINIMAL_TESTSUITE_H -#define MINIMAL_TESTSUITE_H - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef MAVLINK_TEST_ALL -#define MAVLINK_TEST_ALL - -static void mavlink_test_minimal(uint8_t, uint8_t, mavlink_message_t *last_msg); - -static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - - mavlink_test_minimal(system_id, component_id, last_msg); -} -#endif - - - - -static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - 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 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; - - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_heartbeat_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_heartbeat_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_heartbeat_pack(system_id, component_id, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status ); - mavlink_msg_heartbeat_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_heartbeat_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status ); - mavlink_msg_heartbeat_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_to_send_buffer(buffer, &msg); - for (i=0; i Date: Thu, 30 Oct 2014 16:28:19 +0100 Subject: [PATCH 10/14] added optical_flow_rad mavlink message, added gyro_temperature to I2C and mavlink interface --- inc/gyro.h | 2 +- inc/i2c.h | 2 +- inc/i2c_frame.h | 1 + src/gyro.c | 4 +++- src/i2c.c | 3 ++- src/main.c | 62 +++++++++++++++++++++++++++++++++++++++++-------- 6 files changed, 60 insertions(+), 14 deletions(-) diff --git a/inc/gyro.h b/inc/gyro.h index 67855f5..5cdce37 100644 --- a/inc/gyro.h +++ b/inc/gyro.h @@ -145,7 +145,7 @@ void gyro_config(void); /** * @brief Read out newest gyro value */ -void gyro_read(float* x_rate, float* y_rate, float* z_rate); +void gyro_read(float* x_rate, float* y_rate, float* z_rate, int16_t* gyro_temp); /* Low layer functions */ void spi_config(void); diff --git a/inc/i2c.h b/inc/i2c.h index 6712757..f5788eb 100644 --- a/inc/i2c.h +++ b/inc/i2c.h @@ -39,7 +39,7 @@ void i2c_init(); void update_TX_buffer(float pixel_flow_x, float pixel_flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t qual, - float ground_distance, float x_rate, float y_rate, float z_rate); + float ground_distance, float x_rate, float y_rate, float z_rate, int16_t gyro_temp); char i2c_get_ownaddress1(); #endif /* I2C_H_ */ diff --git a/inc/i2c_frame.h b/inc/i2c_frame.h index 6a993cb..5c2e5bf 100644 --- a/inc/i2c_frame.h +++ b/inc/i2c_frame.h @@ -66,6 +66,7 @@ typedef struct i2c_integral_frame uint32_t integration_timespan; uint32_t sonar_timestamp; uint16_t ground_distance; + int16_t gyro_temperature; uint8_t qual; } __attribute__((packed)) i2c_integral_frame; diff --git a/src/gyro.c b/src/gyro.c index d8e6484..e396217 100644 --- a/src/gyro.c +++ b/src/gyro.c @@ -74,7 +74,7 @@ void gyro_config() * @param y_rate Return value y rate * @param z_rate Return value z rate */ -void gyro_read(float* x_rate, float* y_rate, float* z_rate) +void gyro_read(float* x_rate, float* y_rate, float* z_rate,int16_t* gyro_temp) { int16_t x_rate_raw, y_rate_raw, z_rate_raw; x_rate_raw = 0; @@ -96,6 +96,8 @@ void gyro_read(float* x_rate, float* y_rate, float* z_rate) *x_rate = x_rate_raw * gyro_scale - x_rate_offset; *y_rate = y_rate_raw * gyro_scale - y_rate_offset; *z_rate = z_rate_raw * gyro_scale - z_rate_offset; + + *gyro_temp = 789; } /** diff --git a/src/i2c.c b/src/i2c.c index 99d67d4..2a1b95b 100644 --- a/src/i2c.c +++ b/src/i2c.c @@ -210,7 +210,7 @@ void I2C1_ER_IRQHandler(void) { void update_TX_buffer(float pixel_flow_x, float pixel_flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t qual, float ground_distance, float gyro_x_rate, float gyro_y_rate, - float gyro_z_rate) { + float gyro_z_rate, int16_t gyro_temp) { static uint16_t frame_count = 0; int i; union { @@ -309,6 +309,7 @@ void update_TX_buffer(float pixel_flow_x, float pixel_flow_y, u_integral.f.sonar_timestamp = time_since_last_sonar_update; //microseconds u_integral.f.qual = (uint8_t) (accumulated_quality / accumulated_framecount); //0-255 linear quality measurement 0=bad, 255=best + u_integral.f.gyro_temperature = gyro_temp;//Temperature * 100 in centi-degrees Celsius notpublishedIndexFrame1 = 1 - publishedIndexFrame1; // choose not the current published 1 buffer notpublishedIndexFrame2 = 1 - publishedIndexFrame2; // choose not the current published 2 buffer diff --git a/src/main.c b/src/main.c index 6ddf06d..35ef264 100644 --- a/src/main.c +++ b/src/main.c @@ -287,6 +287,18 @@ int main(void) int valid_frame_count = 0; int pixel_flow_count = 0; + static float accumulated_flow_x = 0; + static float accumulated_flow_y = 0; + static float accumulated_gyro_x = 0; + static float accumulated_gyro_y = 0; + static float accumulated_gyro_z = 0; + static uint16_t accumulated_framecount = 0; + static uint16_t accumulated_quality = 0; + static uint32_t integration_timespan = 0; + static uint32_t lasttime = 0; + uint32_t time_since_last_sonar_update= 0; + + /* main loop */ while (1) { @@ -341,7 +353,8 @@ int main(void) /* new gyroscope data */ float x_rate_sensor, y_rate_sensor, z_rate_sensor; - gyro_read(&x_rate_sensor, &y_rate_sensor, &z_rate_sensor); + int16_t gyro_temp; + gyro_read(&x_rate_sensor, &y_rate_sensor, &z_rate_sensor,&gyro_temp); /* gyroscope coordinate transformation */ float x_rate = y_rate_sensor; // change x and y rates @@ -351,14 +364,6 @@ int main(void) /* calculate focal_length in pixel */ const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 6.0f) * 1000.0f; //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled - /* debug */ - float x_rate_pixel = x_rate * (get_time_between_images() / 1000000.0f) * focal_length_px; - float y_rate_pixel = y_rate * (get_time_between_images() / 1000000.0f) * focal_length_px; - - //FIXME for the old sensor PX4FLOW v1.2 uncomment this!!!! -// x_rate = x_rate_raw_sensor; // change x and y rates -// y_rate = y_rate_raw_sensor; - /* get sonar data */ sonar_read(&sonar_distance_filtered, &sonar_distance_raw); @@ -397,12 +402,24 @@ int main(void) float new_velocity_x = - flow_compx * sonar_distance_filtered; float new_velocity_y = - flow_compy * sonar_distance_filtered; + time_since_last_sonar_update = (get_boot_time_us()- get_sonar_measure_time()); + if (qual > 0) { velocity_x_sum += new_velocity_x; velocity_y_sum += new_velocity_y; valid_frame_count++; + uint32_t deltatime = (get_boot_time_us() - lasttime); + integration_timespan += deltatime; + accumulated_flow_x += pixel_flow_y / focal_length_px * 1.0f; //rad axis swapped to align x flow around y axis + accumulated_flow_y += pixel_flow_x / focal_length_px * -1.0f;//rad + accumulated_gyro_x += x_rate * deltatime / 1000000.0f; //rad + accumulated_gyro_y += y_rate * deltatime / 1000000.0f; //rad + accumulated_gyro_z += z_rate * deltatime / 1000000.0f; //rad + accumulated_framecount++; + accumulated_quality += qual; + /* lowpass velocity output */ velocity_x_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_x + (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp; @@ -422,6 +439,8 @@ int main(void) velocity_x_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp; velocity_y_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp; } + //update lasttime + lasttime = get_boot_time_us(); pixel_flow_x_sum += pixel_flow_x; pixel_flow_y_sum += pixel_flow_y; @@ -449,7 +468,7 @@ int main(void) //update I2C transmitbuffer update_TX_buffer(pixel_flow_x, pixel_flow_y, velocity_x_sum/valid_frame_count, velocity_y_sum/valid_frame_count, qual, - ground_distance, x_rate, y_rate, z_rate); + ground_distance, x_rate, y_rate, z_rate, gyro_temp); //serial mavlink + usb mavlink output throttled if (counter % (uint32_t)global_data.param[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] == 0)//throttling factor @@ -475,11 +494,24 @@ int main(void) pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, flow_comp_m_x, flow_comp_m_y, qual, ground_distance); + mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], + integration_timespan, accumulated_flow_x, accumulated_flow_y, + accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z, + gyro_temp, accumulated_quality/accumulated_framecount, + time_since_last_sonar_update,ground_distance); + if (global_data.param[PARAM_USB_SEND_FLOW]) { mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, flow_comp_m_x, flow_comp_m_y, qual, ground_distance); + + + mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID], + integration_timespan, accumulated_flow_x, accumulated_flow_y, + accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z, + gyro_temp, accumulated_quality/accumulated_framecount, + time_since_last_sonar_update,ground_distance); } @@ -488,6 +520,16 @@ int main(void) mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_us(), x_rate, y_rate, z_rate); } + + integration_timespan = 0; + accumulated_flow_x = 0; + accumulated_flow_y = 0; + accumulated_framecount = 0; + accumulated_quality = 0; + accumulated_gyro_x = 0; + accumulated_gyro_y = 0; + accumulated_gyro_z = 0; + velocity_x_sum = 0.0f; velocity_y_sum = 0.0f; pixel_flow_x_sum = 0.0f; From 316b04ebc69ae1a7a10deb50bde721b5f3759839 Mon Sep 17 00:00:00 2001 From: dominiho Date: Mon, 3 Nov 2014 09:48:03 +0100 Subject: [PATCH 11/14] fixed gyro scaling issue, added temperature readout (not tested) --- src/gyro.c | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/gyro.c b/src/gyro.c index e396217..6531bc5 100644 --- a/src/gyro.c +++ b/src/gyro.c @@ -49,11 +49,11 @@ static int sensor_range; enum { - GYRO_DPS50 = 0, GYRO_DPS100, GYRO_DPS150, GYRO_DPS200, GYRO_DPS250, GYRO_DPS500, GYRO_DPS1000, GYRO_DPS2000 + GYRO_DPS250 = 0, GYRO_DPS500, GYRO_DPS2000 } GYRO_DPS_Values; -static int scaling_factors[] = -{ 37548, 18774, 12516, 9387, 7510, 3755, 1877, 939 }; +static float scaling_factors[] = +{8.75f, 17.5f, 70.0f};//mdps/digit /** * @brief Configures Gyro @@ -97,7 +97,9 @@ void gyro_read(float* x_rate, float* y_rate, float* z_rate,int16_t* gyro_temp) *y_rate = y_rate_raw * gyro_scale - y_rate_offset; *z_rate = z_rate_raw * gyro_scale - z_rate_offset; - *gyro_temp = 789; + int8_t temp = 0; + temp = l3gd20_SendHalfWord(0x8000 | 0x2600); + *gyro_temp = (int16_t)temp; } /** @@ -138,7 +140,7 @@ void l3gd20_config() sensor_range = GYRO_DPS500; } - gyro_scale = (global_data.param[PARAM_GYRO_SENSITIVITY_DPS]) / (32768.0f) * 3.1416f / 180.0f; // 32768 -> short range, degree to radian + gyro_scale = scaling_factors[sensor_range] / (1000.0f) * 3.1416f / 180.0f; // scaling_factors in mdps/digit to dps/digit, degree to radian } /** From 55317cd29edf1bc3d4cd827cac85158bd5b70cf5 Mon Sep 17 00:00:00 2001 From: dominiho Date: Mon, 3 Nov 2014 14:01:40 +0100 Subject: [PATCH 12/14] added proper M_PI in degree to rad conversion, added gyro temperature offset and converted to temperature*100 centi-degrees celsius resolution --- inc/gyro.h | 2 ++ src/gyro.c | 12 +++++++----- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/inc/gyro.h b/inc/gyro.h index 5cdce37..3d420f9 100644 --- a/inc/gyro.h +++ b/inc/gyro.h @@ -78,6 +78,8 @@ #define ADDR_REFERENCE 0x25 #define ADDR_TEMPERATURE 0x26 +#define L3GD20_TEMP_OFFSET_CELSIUS 40 + #define ADDR_STATUS_REG 0x27 #define STATUS_ZYXOR (1<<7) #define SATAUS_ZOR (1<<6) diff --git a/src/gyro.c b/src/gyro.c index 6531bc5..4b9796c 100644 --- a/src/gyro.c +++ b/src/gyro.c @@ -41,12 +41,14 @@ #include "stm32f4xx_rcc.h" #include "misc.h" #include "gyro.h" +#include float gyro_scale; float x_rate_offset = 0.0f, y_rate_offset = 0.0f, z_rate_offset = 0.0f; const float gyro_offset_lp_gain = 0.0001; static int sensor_range; + enum { GYRO_DPS250 = 0, GYRO_DPS500, GYRO_DPS2000 @@ -74,7 +76,7 @@ void gyro_config() * @param y_rate Return value y rate * @param z_rate Return value z rate */ -void gyro_read(float* x_rate, float* y_rate, float* z_rate,int16_t* gyro_temp) +void gyro_read(float* x_rate, float* y_rate, float* z_rate, int16_t* gyro_temp) { int16_t x_rate_raw, y_rate_raw, z_rate_raw; x_rate_raw = 0; @@ -97,9 +99,9 @@ void gyro_read(float* x_rate, float* y_rate, float* z_rate,int16_t* gyro_temp) *y_rate = y_rate_raw * gyro_scale - y_rate_offset; *z_rate = z_rate_raw * gyro_scale - z_rate_offset; - int8_t temp = 0; - temp = l3gd20_SendHalfWord(0x8000 | 0x2600); - *gyro_temp = (int16_t)temp; + int8_t temp_raw = 0; + temp_raw = (int8_t)l3gd20_SendHalfWord(0x8000 | 0x2600); + *gyro_temp = (L3GD20_TEMP_OFFSET_CELSIUS-(int16_t)temp_raw)*100;//Temperature * 100 in centi-degrees Celsius [degcelsius*100] } /** @@ -140,7 +142,7 @@ void l3gd20_config() sensor_range = GYRO_DPS500; } - gyro_scale = scaling_factors[sensor_range] / (1000.0f) * 3.1416f / 180.0f; // scaling_factors in mdps/digit to dps/digit, degree to radian + gyro_scale = scaling_factors[sensor_range] / (1000.0f) * (float)M_PI / 180.0f; // scaling_factors in mdps/digit to dps/digit, degree to radian } /** From 3f3c6aae70547c0c05446bcd7a14ecb25e5afdfb Mon Sep 17 00:00:00 2001 From: priseborough Date: Mon, 10 Nov 2014 18:32:44 +1100 Subject: [PATCH 13/14] flow: Add KLT flow method --- inc/flow.h | 6 + src/flow.c | 336 +++++++++++++++++++++++++++++++++++++++++------------ 2 files changed, 270 insertions(+), 72 deletions(-) mode change 100644 => 100755 inc/flow.h mode change 100644 => 100755 src/flow.c diff --git a/inc/flow.h b/inc/flow.h old mode 100644 new mode 100755 index af5989b..d182fdd --- a/inc/flow.h +++ b/inc/flow.h @@ -42,4 +42,10 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, float *histflowx, float *histflowy); +/** + * @brief Computes pixel flow from image1 to image2 with KLT method + */ +uint8_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, + float *pixel_flow_x, float *pixel_flow_y); + #endif /* FLOW_H_ */ diff --git a/src/flow.c b/src/flow.c old mode 100644 new mode 100755 index b485850..4e17392 --- a/src/flow.c +++ b/src/flow.c @@ -48,10 +48,18 @@ #define __ASM asm #include "core_cm4_simd.h" -#define FRAME_SIZE global_data.param[PARAM_IMAGE_WIDTH] -#define SEARCH_SIZE global_data.param[PARAM_MAX_FLOW_PIXEL] // maximum offset to search: 4 + 1/2 pixels +#define FRAME_SIZE BOTTOM_FLOW_IMAGE_WIDTH +#define SEARCH_SIZE BOTTOM_FLOW_SEARCH_WINDOW_SIZE // maximum offset to search: 4 + 1/2 pixels #define TILE_SIZE 8 // x & y tile size -#define NUM_BLOCKS 5 // x & y number of tiles to check +#define NUM_BLOCKS 3 // x & y number of tiles to check + +//this are the settings for KLT based flow +#define PYR_LVLS 2 +#define HALF_PATCH_SIZE 4 //this is half the wanted patch size minus 1 +#define PATCH_SIZE (HALF_PATCH_SIZE*2+1) + +float Jx[PATCH_SIZE*PATCH_SIZE]; +float Jy[PATCH_SIZE*PATCH_SIZE]; #define sign(x) (( x > 0 ) - ( x < 0 )) @@ -402,8 +410,8 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat /* variables */ uint16_t pixLo = SEARCH_SIZE + 1; - uint16_t pixHi = FRAME_SIZE - (SEARCH_SIZE + 1) - TILE_SIZE; - uint16_t pixStep = (pixHi - pixLo) / NUM_BLOCKS + 1; + uint16_t pixHi = FRAME_SIZE - (SEARCH_SIZE + 1); + uint16_t pixStep = (pixHi - pixLo) / (NUM_BLOCKS-1); uint16_t i, j; uint32_t acc[8]; // subpixels uint16_t histx[hist_size]; // counter for x shift @@ -417,6 +425,8 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat float histflowx = 0.0f; float histflowy = 0.0f; + int xxx = 0; + /* initialize with 0 */ for (j = 0; j < hist_size; j++) { histx[j] = 0; histy[j] = 0; } @@ -426,6 +436,7 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat { for (i = pixLo; i < pixHi; i += pixStep) { + xxx++; /* test pixel if it is suitable for flow tracking */ uint32_t diff = compute_diff(image1, i, j, (uint16_t) global_data.param[PARAM_IMAGE_WIDTH]); if (diff < global_data.param[PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD]) @@ -658,73 +669,9 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat } - /* compensate rotation */ - /* calculate focal_length in pixel */ - const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 6.0f) * 1000.0f; //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled - - /* - * gyro compensation - * the compensated value is clamped to - * the maximum measurable flow value (param BFLOW_MAX_PIX) +0.5 - * (sub pixel flow can add half pixel to the value) - * - * -y_rate gives x flow - * x_rates gives y_flow - */ - if (global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION]) - { - if(fabsf(y_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) - { - /* calc pixel of gyro */ - float y_rate_pixel = y_rate * (get_time_between_images() / 1000000.0f) * focal_length_px; - float comp_x = histflowx + y_rate_pixel; - - /* clamp value to maximum search window size plus half pixel from subpixel search */ - if (comp_x < (-SEARCH_SIZE - 0.5f)) - *pixel_flow_x = (-SEARCH_SIZE - 0.5f); - else if (comp_x > (SEARCH_SIZE + 0.5f)) - *pixel_flow_x = (SEARCH_SIZE + 0.5f); - else - *pixel_flow_x = comp_x; - } - else - { - *pixel_flow_x = histflowx; - } - - if(fabsf(x_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD]) - { - /* calc pixel of gyro */ - float x_rate_pixel = x_rate * (get_time_between_images() / 1000000.0f) * focal_length_px; - float comp_y = histflowy - x_rate_pixel; - - /* clamp value to maximum search window size plus/minus half pixel from subpixel search */ - if (comp_y < (-SEARCH_SIZE - 0.5f)) - *pixel_flow_y = (-SEARCH_SIZE - 0.5f); - else if (comp_y > (SEARCH_SIZE + 0.5f)) - *pixel_flow_y = (SEARCH_SIZE + 0.5f); - else - *pixel_flow_y = comp_y; - } - else - { - *pixel_flow_y = histflowy; - } - - /* alternative compensation */ -// /* compensate y rotation */ -// *pixel_flow_x = histflowx + y_rate_pixel; -// -// /* compensate x rotation */ -// *pixel_flow_y = histflowy - x_rate_pixel; - - } else - { - /* without gyro compensation */ - *pixel_flow_x = histflowx; - *pixel_flow_y = histflowy; - } - + /* write results */ + *pixel_flow_x = histflowx; + *pixel_flow_y = histflowy; } else { @@ -745,3 +692,248 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat return qual; } + + +/** + * @brief Computes pixel flow from image1 to image2 + * + * Searches the corresponding position in the new image (image2) of max. 64 pixels from the old image (image1) + * with the KLT method and outputs the average value of all flow vectors + * + * @param image1 previous image buffer + * @param image2 current image buffer (new) + * @param x_rate gyro x rate + * @param y_rate gyro y rate + * @param z_rate gyro z rate + * + * @return quality of flow calculation + */ +uint8_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, float *pixel_flow_x, float *pixel_flow_y) +{ + /* variables */ + uint16_t i, j; + + float meanflowx = 0.0f; + float meanflowy = 0.0f; + uint16_t meancount = 0; + + /* + * compute image pyramid for current frame + * there is 188*120 bytes per buffer, we are only using 64*64 per buffer, + * so just add the pyramid levels after the image + */ + //first compute the offsets in the memory for the pyramid levels + uint16_t lvl_off[PYR_LVLS]; + uint16_t s = FRAME_SIZE; + uint16_t off = 0; + for (int l = 0; l < PYR_LVLS; l++) + { + lvl_off[l] = off; + off += s*s; + s /= 2; + } + + //then subsample the images consecutively, no blurring is done before the subsampling (if someone volunteers, please go ahead...) + for (int l = 1; l < PYR_LVLS; l++) + { + uint16_t src_size = FRAME_SIZE >> (l-1); + uint16_t tar_size = FRAME_SIZE >> l; + uint8_t *source = &image2[lvl_off[l-1]]; //pointer to the beginning of the previous level + uint8_t *target = &image2[lvl_off[l]]; //pointer to the beginning of the current level + for (j = 0; j < tar_size; j++) + for (i = 0; i < tar_size; i+=2) + { + //subsample the image by 2, use the halving-add instruction to do so + uint32_t l1 = (__UHADD8(*((uint32_t*) &source[(j*2+0)*src_size + i*2]), *((uint32_t*) &source[(j*2+0)*src_size + i*2+1]))); + uint32_t l2 = (__UHADD8(*((uint32_t*) &source[(j*2+1)*src_size + i*2]), *((uint32_t*) &source[(j*2+1)*src_size + i*2+1]))); + uint32_t r = __UHADD8(l1, l2); + + //the first and the third byte are the values we want to have + target[j*tar_size + i+0] = (uint8_t) r; + target[j*tar_size + i+1] = (uint8_t) (r>>16); + } + } + + //need to store the flow values between pyramid level changes + float us[NUM_BLOCKS*NUM_BLOCKS]; + float vs[NUM_BLOCKS*NUM_BLOCKS]; + uint16_t is[NUM_BLOCKS*NUM_BLOCKS]; + uint16_t js[NUM_BLOCKS*NUM_BLOCKS]; + + + //initialize flow values with the pixel value of the previous image + uint16_t pixLo = FRAME_SIZE / (NUM_BLOCKS + 1); + if (pixLo < PATCH_SIZE) pixLo = PYR_LVLS * PATCH_SIZE; + uint16_t pixHi = (uint16_t)FRAME_SIZE * NUM_BLOCKS / (NUM_BLOCKS + 1); + if (pixHi > (FRAME_SIZE - PATCH_SIZE)) pixHi = FRAME_SIZE - (PYR_LVLS * PATCH_SIZE); + uint16_t pixStep = (pixHi - pixLo) / (NUM_BLOCKS-1); + + j = pixLo; + for (int y = 0; y < NUM_BLOCKS; y++, j += pixStep) + { + i = pixLo; + for (int x = 0; x < NUM_BLOCKS; x++, i += pixStep) + { + //TODO: for proper rotation compensation, insert gyro values here and then substract at the end + us[y*NUM_BLOCKS+x] = i; //position in new image at level 0 + vs[y*NUM_BLOCKS+x] = j; + is[y*NUM_BLOCKS+x] = i; //position in previous image at level 0 + js[y*NUM_BLOCKS+x] = j; + } + } + + //for all pyramid levels, start from the smallest level + for (int l = PYR_LVLS-1; l >= 0; l--) + { + //iterate over all patterns + for (int k = 0; k < NUM_BLOCKS*NUM_BLOCKS; k++) + { + uint16_t i = is[k] >> l; //reference pixel for the current level + uint16_t j = js[k] >> l; + + uint16_t iwidth = FRAME_SIZE >> l; + uint8_t *base1 = image1 + lvl_off[l] + j * iwidth + i; + + float JTJ[4]; //the 2x2 Hessian + JTJ[0] = 0; + JTJ[1] = 0; + JTJ[2] = 0; + JTJ[3] = 0; + int c = 0; + + //compute jacobians and the hessian for the patch at the current location + for (int8_t jj = -HALF_PATCH_SIZE; jj <= HALF_PATCH_SIZE; jj++) + { + uint8_t *left = base1 + jj*iwidth; + for (int8_t ii = -HALF_PATCH_SIZE; ii <= HALF_PATCH_SIZE; ii++) + { + const float jx = ((uint16_t)left[ii+1] - (uint16_t)left[ii-1]) * 0.5f; + const float jy = ((uint16_t)left[ii+iwidth] - (uint16_t)left[ii-iwidth]) * 0.5f; + Jx[c] = jx; + Jy[c] = jy; + JTJ[0] += jx*jx; + JTJ[1] += jx*jy; + JTJ[2] += jx*jy; + JTJ[3] += jy*jy; + c++; + } + } + + // us and vs store the sample position in level 0 pixel coordinates + float u = (us[k] / (1< chi_sq) + { + //compute inverse of hessian + float det = (JTJ[0]*JTJ[3]-JTJ[1]*JTJ[2]); + if (det != 0.f) + { + float detinv = 1.f / det; + float JTJinv[4]; + JTJinv[0] = detinv * JTJ[3]; + JTJinv[1] = detinv * -JTJ[1]; + JTJinv[2] = detinv * -JTJ[2]; + JTJinv[3] = detinv * JTJ[0]; + + //compute update and shift current position accordingly + float updx = JTJinv[0]*JTe_x + JTJinv[1]*JTe_y; + float updy = JTJinv[2]*JTe_x + JTJinv[3]*JTe_y; + float new_u = u-updx; + float new_v = v-updy; + + //check if we drifted outside the image + if (((int16_t)new_u < HALF_PATCH_SIZE) || (int16_t)new_u > (iwidth-HALF_PATCH_SIZE-1) || ((int16_t)new_v < HALF_PATCH_SIZE) || (int16_t)new_v > (iwidth-HALF_PATCH_SIZE-1)) + { + break; + } + else + { + u = new_u; + v = new_v; + } + } + else + break; + } + else + break; + + chi_sq_previous = chi_sq; + } + + if (l > 0) + { + us[k] = u * (1< 0) + { + meanflowx /= meancount; + meanflowy /= meancount; + + *pixel_flow_x = -meanflowx; + *pixel_flow_y = -meanflowy; +} +else +{ + *pixel_flow_x = 0.0f; + *pixel_flow_y = 0.0f; + return 0; +} + +/* return quality */ +return (uint8_t)(meancount * 255 / (NUM_BLOCKS*NUM_BLOCKS)); +} From 41f4ac99142753422cdb9518802c4e27b1b9cd62 Mon Sep 17 00:00:00 2001 From: priseborough Date: Mon, 10 Nov 2014 18:32:59 +1100 Subject: [PATCH 14/14] main: Add KLT flow method --- src/main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main.c b/src/main.c index 35ef264..525ecdb 100644 --- a/src/main.c +++ b/src/main.c @@ -374,7 +374,7 @@ int main(void) dma_copy_image_buffers(¤t_image, &previous_image, image_size, 1); /* compute optical flow */ - qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); + qual = compute_klt(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); if (sonar_distance_filtered > 5.0f || sonar_distance_filtered == 0.0f) {