From 357a9ba01796e7eaabd644969f6617a22367e2d1 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Thu, 27 Oct 2011 22:36:25 -0700 Subject: [PATCH 1/5] Log updates Added motor logging for different frame types. Switched the PM log to some new debugging values and speed up the writing of the value to the logs. --- ArduCopter/ArduCopter.pde | 8 +-- ArduCopter/Log.pde | 139 ++++++++++++++++++++++++++++++-------- 2 files changed, 116 insertions(+), 31 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index b51e0d59b3..9628eb8d4c 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -249,9 +249,9 @@ static const char* flight_mode_strings[] = { // test #if ACCEL_ALT_HOLD == 1 Vector3f accels_rot; -static int accels_rot_count; -static float accels_rot_sum; -static float alt_hold_gain = ACCEL_ALT_HOLD_GAIN; +static int accels_rot_count; +static float accels_rot_sum; +static float alt_hold_gain = ACCEL_ALT_HOLD_GAIN; #endif // temp @@ -535,7 +535,7 @@ void loop() counter_one_herz = 0; } - if (millis() - perf_mon_timer > 20000) { + if (millis() - perf_mon_timer > 1200 /*20000*/) { if (g.log_bitmask & MASK_LOG_PM) Log_Write_Performance(); diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 56ec0460f7..ba7d02a9a0 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -474,10 +474,54 @@ static void Log_Write_Motors() DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_MOTORS_MSG); - DataFlash.WriteInt(motor_out[CH_1]); - DataFlash.WriteInt(motor_out[CH_2]); - DataFlash.WriteInt(motor_out[CH_3]); - DataFlash.WriteInt(motor_out[CH_4]); + #if FRAME_CONFIG == TRI_FRAME + DataFlash.WriteInt(motor_out[CH_1]);//1 + DataFlash.WriteInt(motor_out[CH_2]);//2 + DataFlash.WriteInt(motor_out[CH_4]);//3 + DataFlash.WriteInt(g.rc_4.radio_out);//4 + + #elif FRAME_CONFIG == HEXA_FRAME + DataFlash.WriteInt(motor_out[CH_1]);//1 + DataFlash.WriteInt(motor_out[CH_2]);//2 + DataFlash.WriteInt(motor_out[CH_3]);//3 + DataFlash.WriteInt(motor_out[CH_4]);//4 + DataFlash.WriteInt(motor_out[CH_7]);//5 + DataFlash.WriteInt(motor_out[CH_8]);//6 + + #elif FRAME_CONFIG == Y6_FRAME + //left + DataFlash.WriteInt(motor_out[CH_2]);//1 + DataFlash.WriteInt(motor_out[CH_3]);//2 + //right + DataFlash.WriteInt(motor_out[CH_7]);//3 + DataFlash.WriteInt(motor_out[CH_1]);//4 + //back + DataFlash.WriteInt(motor_out[CH_8]);//5 + DataFlash.WriteInt(motor_out[CH_4]);//6 + + #elif FRAME_CONFIG == OCTA_FRAME || FRAME_CONFIG == OCTA_QUAD_FRAME + DataFlash.WriteInt(motor_out[CH_1]);//1 + DataFlash.WriteInt(motor_out[CH_2]);//2 + DataFlash.WriteInt(motor_out[CH_3]);//3 + DataFlash.WriteInt(motor_out[CH_4]);//4 + DataFlash.WriteInt(motor_out[CH_7]);//5 + DataFlash.WriteInt(motor_out[CH_8]); //6 + DataFlash.WriteInt(motor_out[CH_10]);//7 + DataFlash.WriteInt(motor_out[CH_11]);//8 + + #elif FRAME_CONFIG == HELI_FRAME + DataFlash.WriteInt(heli_servo_out[0]);//1 + DataFlash.WriteInt(heli_servo_out[1]);//2 + DataFlash.WriteInt(heli_servo_out[2]);//3 + DataFlash.WriteInt(heli_servo_out[3]);//4 + DataFlash.WriteInt(g.heli_ext_gyro_gain);//5 + + #else // quads + DataFlash.WriteInt(motor_out[CH_1]);//1 + DataFlash.WriteInt(motor_out[CH_2]);//2 + DataFlash.WriteInt(motor_out[CH_3]);//3 + DataFlash.WriteInt(motor_out[CH_4]);//4 + #endif DataFlash.WriteByte(END_BYTE); } @@ -485,11 +529,46 @@ static void Log_Write_Motors() // Read a Current packet static void Log_Read_Motors() { + #if FRAME_CONFIG == HEXA_FRAME || FRAME_CONFIG == Y6_FRAME + // 1 2 3 4 5 6 + Serial.printf_P(PSTR("MOT: %d, %d, %d, %d, %d, %d\n"), + DataFlash.ReadInt(), //1 + DataFlash.ReadInt(), //2 + DataFlash.ReadInt(), //3 + DataFlash.ReadInt(), //4 + DataFlash.ReadInt(), //5 + DataFlash.ReadInt()); //6 + + #elif FRAME_CONFIG == OCTA_FRAME || FRAME_CONFIG == OCTA_QUAD_FRAME + // 1 2 3 4 5 6 7 8 + Serial.printf_P(PSTR("MOT: %d, %d, %d, %d, %d, %d, %d, %d\n"), + DataFlash.ReadInt(), //1 + DataFlash.ReadInt(), //2 + DataFlash.ReadInt(), //3 + DataFlash.ReadInt(), //4 + + DataFlash.ReadInt(), //5 + DataFlash.ReadInt(), //6 + DataFlash.ReadInt(), //7 + DataFlash.ReadInt()); //8 + + #elif FRAME_CONFIG == HELI_FRAME + // 1 2 3 4 5 + Serial.printf_P(PSTR("MOT: %d, %d, %d, %d, %d\n"), + DataFlash.ReadInt(), //1 + DataFlash.ReadInt(), //2 + DataFlash.ReadInt(), //3 + DataFlash.ReadInt(), //4 + DataFlash.ReadInt()); //5 + + #else // quads, TRIs + // 1 2 3 4 Serial.printf_P(PSTR("MOT: %d, %d, %d, %d\n"), - DataFlash.ReadInt(), - DataFlash.ReadInt(), - DataFlash.ReadInt(), - DataFlash.ReadInt()); + DataFlash.ReadInt(), //1 + DataFlash.ReadInt(), //2 + DataFlash.ReadInt(), //3 + DataFlash.ReadInt()); //4; + #endif } #ifdef OPTFLOW_ENABLED @@ -601,8 +680,9 @@ static void Log_Write_Control_Tuning() DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG); + // yaw - DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); //1 + DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); //2 DataFlash.WriteInt((int)(nav_yaw/100)); //2 DataFlash.WriteInt((int)yaw_error/100); //3 @@ -627,7 +707,7 @@ static void Log_Read_Control_Tuning() "%d, %d\n"), // Control - //DataFlash.ReadInt(), + //DataFlash.ReadByte(), //DataFlash.ReadInt(), // yaw @@ -658,37 +738,42 @@ static void Log_Write_Performance() //* //DataFlash.WriteLong( millis()- perf_mon_timer); - DataFlash.WriteByte( dcm.gyro_sat_count); //2 - DataFlash.WriteByte( imu.adc_constraints); //3 - DataFlash.WriteByte( dcm.renorm_sqrt_count); //4 - DataFlash.WriteByte( dcm.renorm_blowup_count); //5 - DataFlash.WriteByte( gps_fix_count); //6 + //DataFlash.WriteByte( dcm.gyro_sat_count); //2 + //DataFlash.WriteByte( imu.adc_constraints); //3 + //DataFlash.WriteByte( dcm.renorm_sqrt_count); //4 + //DataFlash.WriteByte( dcm.renorm_blowup_count); //5 + //DataFlash.WriteByte( gps_fix_count); //6 - DataFlash.WriteInt ( (int)(dcm.get_health() * 1000)); //7 - DataFlash.WriteLong ( throttle_integrator); //8 + + //DataFlash.WriteInt ( (int)(dcm.get_health() * 1000)); //7 + + + + // control_mode + DataFlash.WriteByte(control_mode); //1 + DataFlash.WriteByte(yaw_mode); //2 + DataFlash.WriteByte(roll_pitch_mode); //3 + DataFlash.WriteByte(throttle_mode); //4 + DataFlash.WriteInt(g.throttle_cruise.get()); //5 + DataFlash.WriteLong(throttle_integrator); //6 DataFlash.WriteByte(END_BYTE); } // Read a performance packet static void Log_Read_Performance() -{ - Serial.printf_P(PSTR( "PM, %d, %d, " - "%d, %d, %d, " - "%d, %ld\n"), +{ //1 2 3 4 5 6 + Serial.printf_P(PSTR( "PM, %d, %d, %d, %d, %d, %ld\n"), // Control //DataFlash.ReadLong(), //DataFlash.ReadInt(), + DataFlash.ReadByte(), //1 DataFlash.ReadByte(), //2 DataFlash.ReadByte(), //3 - DataFlash.ReadByte(), //4 - DataFlash.ReadByte(), //5 - DataFlash.ReadByte(), //6 - - DataFlash.ReadInt(), //7 - DataFlash.ReadLong()); //8 + DataFlash.ReadInt(), //5 + DataFlash.ReadLong()); //6 } // Write a command processing packet. From a88424b15280cf36e468eb0128e530ea6ab5bf1b Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 28 Oct 2011 21:29:10 -0700 Subject: [PATCH 2/5] Added a slew rate to the alt change rate. Added better reporting of throttle control upped constraints for alt hold Additional logging for CTUN lay hold --- ArduCopter/ArduCopter.pde | 28 ++++++++++++++++++--- ArduCopter/Attitude.pde | 27 ++------------------ ArduCopter/Log.pde | 49 ++++++++++++++++++++---------------- ArduCopter/config.h | 2 +- ArduCopter/control_modes.pde | 4 --- ArduCopter/system.pde | 6 +++++ 6 files changed, 62 insertions(+), 54 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 9628eb8d4c..721d3e3938 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -334,6 +334,9 @@ static int waypoint_speed_gov; static bool do_flip = false; #endif +static boolean trim_flag; +static int CH7_wp_index = 0; + // Airspeed // -------- static int airspeed; // m/s * 100 @@ -342,6 +345,7 @@ static int airspeed; // m/s * 100 // --------------- static long altitude_error; // meters * 100 we are off in altitude static long old_altitude; +static int old_rate; static long yaw_error; // how off are we pointed static long long_error, lat_error; // temp for debugging @@ -381,6 +385,8 @@ static boolean land_complete; static long old_alt; // used for managing altitude rates static int velocity_land; static byte yaw_tracking = MAV_ROI_WPNEXT; // no tracking, point at next wp, or at a target +static int manual_boost; // used in adjust altitude to make changing alt faster +static int angle_boost; // used in adjust altitude to make changing alt faster // Loiter management // ----------------- @@ -1022,7 +1028,8 @@ void update_throttle_mode(void) case THROTTLE_MANUAL: if (g.rc_3.control_in > 0){ - g.rc_3.servo_out = g.rc_3.control_in + get_angle_boost(g.rc_3.control_in); + angle_boost = get_angle_boost(g.rc_3.control_in); + g.rc_3.servo_out = g.rc_3.control_in + angle_boost; }else{ g.pi_stabilize_roll.reset_I(); g.pi_stabilize_pitch.reset_I(); @@ -1051,8 +1058,8 @@ void update_throttle_mode(void) // clear the new data flag invalid_throttle = false; } - - g.rc_3.servo_out = g.throttle_cruise + nav_throttle + get_angle_boost(g.throttle_cruise); + angle_boost = get_angle_boost(g.throttle_cruise); + g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + manual_boost; break; } } @@ -1213,7 +1220,19 @@ static void update_altitude() current_loc.alt = baro_alt + home.alt; } + // calc the accel rate limit to 2m/s altitude_rate = (current_loc.alt - old_altitude) * 10; // 10 hz timer + + // rate limiter to reduce some of the motor pulsing + if (altitude_rate > 0){ + // going up + altitude_rate = min(altitude_rate, old_rate + 20); + }else{ + // going down + altitude_rate = max(altitude_rate, old_rate - 20); + } + + old_rate = altitude_rate; old_altitude = current_loc.alt; #endif } @@ -1225,9 +1244,12 @@ adjust_altitude() next_WP.alt -= 1; // 1 meter per second next_WP.alt = max(next_WP.alt, (current_loc.alt - 500)); // don't go less than 4 meters below current location next_WP.alt = max(next_WP.alt, 100); // don't go less than 1 meter + //manual_boost = (g.rc_3.control_in == 0) ? -20 : 0; + }else if (g.rc_3.control_in > 700){ next_WP.alt += 1; // 1 meter per second next_WP.alt = min(next_WP.alt, (current_loc.alt + 500)); // don't go more than 4 meters below current location + //manual_boost = (g.rc_3.control_in == 800) ? 20 : 0; } } diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index e7b232b359..772828c104 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -83,7 +83,7 @@ get_stabilize_yaw(long target_angle) return (int)constrain(rate, -2500, 2500); } -#define ALT_ERROR_MAX 400 +#define ALT_ERROR_MAX 500 static int get_nav_throttle(long z_error) { @@ -94,33 +94,10 @@ get_nav_throttle(long z_error) rate_error = rate_error - altitude_rate; // limit the rate - rate_error = constrain(rate_error, -100, 120); + rate_error = constrain(rate_error, -120, 140); return (int)g.pi_throttle.get_pi(rate_error, .1); } -#define ALT_ERROR_MAX2 300 -static int -get_nav_throttle2(long z_error) -{ - if (z_error > ALT_ERROR_MAX2){ - return g.pi_throttle.kP() * 80; - - }else if (z_error < -ALT_ERROR_MAX2){ - return g.pi_throttle.kP() * -60; - - } else{ - // limit error to prevent I term run up - z_error = constrain(z_error, -ALT_ERROR_MAX2, ALT_ERROR_MAX2); - int rate_error = g.pi_alt_hold.get_pi(z_error, .1); //_p = .85 - - rate_error = rate_error - altitude_rate; - - // limit the rate - rate_error = constrain(rate_error, -100, 120); - return (int)g.pi_throttle.get_pi(rate_error, .1) + alt_hold_velocity(); - } -} - static int get_rate_roll(long target_rate) { diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index ba7d02a9a0..7a92352510 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -680,19 +680,23 @@ static void Log_Write_Control_Tuning() DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG); - // yaw - DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); //2 - DataFlash.WriteInt((int)(nav_yaw/100)); //2 - DataFlash.WriteInt((int)yaw_error/100); //3 + DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); //1 + DataFlash.WriteInt((int)(nav_yaw/100)); //2 + DataFlash.WriteInt((int)yaw_error/100); //3 // Alt hold - DataFlash.WriteInt(g.rc_3.servo_out); //4 - DataFlash.WriteInt(sonar_alt); //5 - DataFlash.WriteInt(baro_alt); //6 + DataFlash.WriteInt(sonar_alt); //4 + DataFlash.WriteInt(baro_alt); //5 + DataFlash.WriteInt((int)next_WP.alt); //6 - DataFlash.WriteInt((int)next_WP.alt); //7 - DataFlash.WriteInt((int)g.pi_throttle.get_integrator());//8 + DataFlash.WriteInt(nav_throttle); //7 + DataFlash.WriteInt(angle_boost); //8 + DataFlash.WriteByte(manual_boost); //9 + + DataFlash.WriteInt(g.rc_3.servo_out); //10 + DataFlash.WriteInt((int)g.pi_alt_hold.get_integrator()); //11 + DataFlash.WriteInt((int)g.pi_throttle.get_integrator()); //12 DataFlash.WriteByte(END_BYTE); } @@ -701,27 +705,30 @@ static void Log_Write_Control_Tuning() // Read an control tuning packet static void Log_Read_Control_Tuning() { - Serial.printf_P(PSTR( "CTUN, " - "%d, %d, %d, " - "%d, %d, %d, " - "%d, %d\n"), + // 1 2 3 4 5 6 7 8 9 10 11 12 + Serial.printf_P(PSTR( "CTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"), // Control //DataFlash.ReadByte(), //DataFlash.ReadInt(), // yaw - DataFlash.ReadInt(), - DataFlash.ReadInt(), - DataFlash.ReadInt(), + DataFlash.ReadInt(), //1 + DataFlash.ReadInt(), //2 + DataFlash.ReadInt(), //3 // Alt Hold - DataFlash.ReadInt(), - DataFlash.ReadInt(), - DataFlash.ReadInt(), + DataFlash.ReadInt(), //4 + DataFlash.ReadInt(), //5 + DataFlash.ReadInt(), //6 - DataFlash.ReadInt(), - DataFlash.ReadInt()); + DataFlash.ReadInt(), //7 + DataFlash.ReadInt(), //8 + DataFlash.ReadByte(), //9 + + DataFlash.ReadInt(), //10 + DataFlash.ReadInt(), //11 + DataFlash.ReadInt()); //12 } // Write a performance monitoring packet. Total length : 19 bytes diff --git a/ArduCopter/config.h b/ArduCopter/config.h index f0b8e5ee2d..e05c8bb2a8 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -520,7 +520,7 @@ // RATE control #ifndef THROTTLE_P -# define THROTTLE_P 0.6 // +# define THROTTLE_P 0.8 // #endif #ifndef THROTTLE_I # define THROTTLE_I 0.10 // with 4m error, 12.5s windup diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 3d604f612d..632d69c862 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -42,10 +42,6 @@ static void reset_control_switch() read_control_switch(); } -#if CH7_OPTION == CH7_SET_HOVER -static boolean trim_flag; -#endif - // read at 10 hz // set this to your trainer switch static void read_trim_switch() diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 1979ee328a..c19cc4bdb5 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -348,6 +348,12 @@ static void set_mode(byte mode) // used to stop fly_aways motor_auto_armed = (g.rc_3.control_in > 0); + // clearing value used in interactive alt hold + manual_boost = 0; + + // clearing value used to set WP's dynamically. + CH7_wp_index = 0; + Serial.println(flight_mode_strings[control_mode]); // report the GPS and Motor arming status From 4ccc99557661c3499d8dc531b07b0078c8e84272 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 28 Oct 2011 21:46:31 -0700 Subject: [PATCH 3/5] Added save WP note to APM_Config.h --- ArduCopter/APM_Config.h | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 1bc096f9df..d97a104670 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -36,6 +36,7 @@ CH7_RTL CH7_AUTO_TRIM CH7_ADC_FILTER (experimental) + CH7_SAVE_WP */ #define ACCEL_ALT_HOLD 0 // disabled by default, work in progress From 3d48ad2569b33bacf9722691a8de01077bf9ee0f Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 28 Oct 2011 21:59:47 -0700 Subject: [PATCH 4/5] logging type change in CTUN --- ArduCopter/Log.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 7a92352510..81c42c1dcb 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -692,7 +692,7 @@ static void Log_Write_Control_Tuning() DataFlash.WriteInt(nav_throttle); //7 DataFlash.WriteInt(angle_boost); //8 - DataFlash.WriteByte(manual_boost); //9 + DataFlash.WriteInt(manual_boost); //9 DataFlash.WriteInt(g.rc_3.servo_out); //10 DataFlash.WriteInt((int)g.pi_alt_hold.get_integrator()); //11 @@ -724,7 +724,7 @@ static void Log_Read_Control_Tuning() DataFlash.ReadInt(), //7 DataFlash.ReadInt(), //8 - DataFlash.ReadByte(), //9 + DataFlash.ReadInt(), //9 DataFlash.ReadInt(), //10 DataFlash.ReadInt(), //11 From 8cc449dc4371fb636d1957f2b18045ca9b6cd842 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 29 Oct 2011 18:07:09 +1100 Subject: [PATCH 5/5] updated to latest upstream mavlink headers this includes the new APM_Camera headers from ardupilotmega.xml --- .../include/ardupilotmega/ardupilotmega.h | 73 +- .../ardupilotmega/mavlink_msg_ap_timing.h | 210 --- .../mavlink_msg_digicam_configure.h | 364 ++++++ .../mavlink_msg_digicam_control.h | 342 +++++ .../mavlink_msg_mount_configure.h | 254 ++++ .../ardupilotmega/mavlink_msg_mount_control.h | 254 ++++ .../ardupilotmega/mavlink_msg_mount_status.h | 232 ++++ .../include/ardupilotmega/testsuite.h | 306 ++++- .../include/ardupilotmega/version.h | 2 +- libraries/GCS_MAVLink/include/bittest.c | 39 - libraries/GCS_MAVLink/include/common/common.h | 52 +- .../mavlink_msg_attitude_controller_output.h | 169 --- .../include/common/mavlink_msg_attitude_new.h | 268 ---- .../include/common/mavlink_msg_full_state.h | 428 ------ .../mavlink_msg_position_controller_output.h | 169 --- ...ink_msg_request_dynamic_gyro_calibration.h | 177 --- .../mavlink_msg_request_static_calibration.h | 138 -- .../common/mavlink_msg_set_roll_pitch_yaw.h | 184 --- .../mavlink_msg_set_roll_pitch_yaw_speed.h | 184 --- ...avlink_msg_waypoint_set_global_reference.h | 294 ----- .../GCS_MAVLink/include/common/version.h | 2 +- .../GCS_MAVLink/include/documentation.dox | 41 - .../GCS_MAVLink/include/mavlink_helpers.h | 8 +- .../GCS_MAVLink/include/minimal/mavlink.h | 11 - .../include/minimal/mavlink_msg_heartbeat.h | 132 -- .../GCS_MAVLink/include/minimal/minimal.h | 45 - .../GCS_MAVLink/include/pixhawk/mavlink.h | 11 - .../pixhawk/mavlink_msg_attitude_control.h | 257 ---- .../include/pixhawk/mavlink_msg_aux_status.h | 204 --- .../pixhawk/mavlink_msg_brief_feature.h | 249 ---- .../pixhawk/mavlink_msg_control_status.h | 203 --- .../mavlink_msg_data_transmission_handshake.h | 174 --- .../include/pixhawk/mavlink_msg_debug_vect.h | 156 --- .../pixhawk/mavlink_msg_encapsulated_data.h | 124 -- .../pixhawk/mavlink_msg_encapsulated_image.h | 76 -- .../pixhawk/mavlink_msg_get_image_ack.h | 104 -- .../pixhawk/mavlink_msg_image_available.h | 586 --------- .../mavlink_msg_image_trigger_control.h | 101 -- .../pixhawk/mavlink_msg_image_triggered.h | 352 ----- .../pixhawk/mavlink_msg_manual_control.h | 191 --- .../include/pixhawk/mavlink_msg_marker.h | 236 ---- .../pixhawk/mavlink_msg_pattern_detected.h | 160 --- .../pixhawk/mavlink_msg_point_of_interest.h | 241 ---- ...mavlink_msg_point_of_interest_connection.h | 307 ----- .../mavlink_msg_position_control_offset_set.h | 206 --- .../mavlink_msg_position_control_setpoint.h | 192 --- ...avlink_msg_position_control_setpoint_set.h | 226 ---- .../include/pixhawk/mavlink_msg_raw_aux.h | 226 ---- .../pixhawk/mavlink_msg_request_data_stream.h | 118 -- ...ink_msg_request_dynamic_gyro_calibration.h | 123 -- .../mavlink_msg_request_static_calibration.h | 90 -- .../pixhawk/mavlink_msg_set_altitude.h | 78 -- .../pixhawk/mavlink_msg_set_cam_shutter.h | 197 --- .../mavlink_msg_vicon_position_estimate.h | 242 ---- .../mavlink_msg_vision_position_estimate.h | 242 ---- .../mavlink_msg_vision_speed_estimate.h | 176 --- .../pixhawk/mavlink_msg_watchdog_command.h | 158 --- .../pixhawk/mavlink_msg_watchdog_heartbeat.h | 124 -- .../mavlink_msg_watchdog_process_info.h | 186 --- .../mavlink_msg_watchdog_process_status.h | 200 --- .../GCS_MAVLink/include/pixhawk/pixhawk.h | 79 -- libraries/GCS_MAVLink/include/slugs/mavlink.h | 11 - .../include/slugs/mavlink_msg_air_data.h | 148 --- .../include/slugs/mavlink_msg_cpu_load.h | 138 -- .../include/slugs/mavlink_msg_ctrl_srfc_pt.h | 121 -- .../include/slugs/mavlink_msg_data_log.h | 216 ---- .../include/slugs/mavlink_msg_diagnostic.h | 210 --- .../include/slugs/mavlink_msg_filtered_data.h | 216 ---- .../include/slugs/mavlink_msg_gps_date_time.h | 203 --- .../include/slugs/mavlink_msg_mid_lvl_cmds.h | 167 --- .../include/slugs/mavlink_msg_pid.h | 130 -- .../include/slugs/mavlink_msg_pilot_console.h | 145 --- .../include/slugs/mavlink_msg_pwm_commands.h | 235 ---- .../include/slugs/mavlink_msg_sensor_bias.h | 216 ---- .../include/slugs/mavlink_msg_slugs_action.h | 138 -- .../slugs/mavlink_msg_slugs_navigation.h | 272 ---- libraries/GCS_MAVLink/include/slugs/slugs.h | 56 - .../GCS_MAVLink/include/ualberta/mavlink.h | 11 - .../ualberta/mavlink_msg_nav_filter_bias.h | 242 ---- .../ualberta/mavlink_msg_radio_calibration.h | 204 --- .../mavlink_msg_request_radio_calibration.h | 59 - .../mavlink_msg_request_rc_channels.h | 101 -- .../mavlink_msg_ualberta_sys_status.h | 135 -- .../GCS_MAVLink/include/ualberta/ualberta.h | 79 -- .../ardupilotmega/ardupilotmega.h | 72 +- .../ardupilotmega/ardupilotmega_testsuite.h | 22 - .../mavlink_msg_digicam_configure.h | 364 ++++++ .../mavlink_msg_digicam_control.h | 342 +++++ .../mavlink_msg_mount_configure.h | 254 ++++ .../ardupilotmega/mavlink_msg_mount_control.h | 254 ++++ .../ardupilotmega/mavlink_msg_mount_status.h | 232 ++++ .../include_v1.0/ardupilotmega/testsuite.h | 286 ++++ .../include_v1.0/ardupilotmega/version.h | 2 +- .../GCS_MAVLink/include_v1.0/common/common.h | 165 +-- .../common/mavlink_msg_command_long.h | 86 +- .../common/mavlink_msg_command_short.h | 298 ----- .../common/mavlink_msg_global_position.h | 276 ---- .../common/mavlink_msg_global_position_int.h | 4 +- .../include_v1.0/common/mavlink_msg_gps_raw.h | 342 ----- .../mavlink_msg_gps_set_global_origin.h | 232 ---- .../common/mavlink_msg_mission_count.h | 10 +- .../common/mavlink_msg_mission_item.h | 106 +- ...mavlink_msg_mission_request_partial_list.h | 210 +++ .../mavlink_msg_mission_write_partial_list.h | 210 +++ .../common/mavlink_msg_position_target.h | 210 --- .../common/mavlink_msg_rc_channels_scaled.h | 4 +- .../common/mavlink_msg_servo_output_raw.h | 4 +- .../common/mavlink_msg_set_flight_mode.h | 166 --- .../common/mavlink_msg_set_safety_mode.h | 166 --- .../common/mavlink_msg_system_time_utc.h | 166 --- .../common/mavlink_msg_waypoint.h | 430 ------ .../common/mavlink_msg_waypoint_ack.h | 188 --- .../common/mavlink_msg_waypoint_clear_all.h | 166 --- .../common/mavlink_msg_waypoint_count.h | 188 --- .../common/mavlink_msg_waypoint_current.h | 144 --- .../common/mavlink_msg_waypoint_reached.h | 144 --- .../common/mavlink_msg_waypoint_request.h | 188 --- .../mavlink_msg_waypoint_request_list.h | 166 --- .../common/mavlink_msg_waypoint_set_current.h | 188 --- .../include_v1.0/common/testsuite.h | 298 +++-- .../GCS_MAVLink/include_v1.0/common/version.h | 2 +- .../include_v1.0/minimal/mavlink.h | 27 - .../minimal/mavlink_msg_heartbeat.h | 251 ---- .../include_v1.0/minimal/minimal.h | 53 - .../include_v1.0/minimal/testsuite.h | 88 -- .../include_v1.0/minimal/version.h | 12 - .../include_v1.0/pixhawk/mavlink.h | 27 - .../pixhawk/mavlink_msg_attitude_control.h | 320 ----- .../pixhawk/mavlink_msg_brief_feature.h | 292 ----- .../mavlink_msg_data_transmission_handshake.h | 232 ---- .../pixhawk/mavlink_msg_encapsulated_data.h | 160 --- .../pixhawk/mavlink_msg_image_available.h | 628 --------- .../mavlink_msg_image_trigger_control.h | 144 --- .../pixhawk/mavlink_msg_image_triggered.h | 386 ------ .../include_v1.0/pixhawk/mavlink_msg_marker.h | 276 ---- .../pixhawk/mavlink_msg_pattern_detected.h | 204 --- .../pixhawk/mavlink_msg_point_of_interest.h | 292 ----- ...mavlink_msg_point_of_interest_connection.h | 358 ----- .../mavlink_msg_position_control_offset_set.h | 254 ---- .../mavlink_msg_position_control_setpoint.h | 232 ---- ...avlink_msg_position_control_setpoint_set.h | 276 ---- .../pixhawk/mavlink_msg_raw_aux.h | 276 ---- .../pixhawk/mavlink_msg_set_cam_shutter.h | 254 ---- .../mavlink_msg_vicon_position_estimate.h | 276 ---- .../mavlink_msg_vision_position_estimate.h | 276 ---- .../mavlink_msg_vision_speed_estimate.h | 210 --- .../pixhawk/mavlink_msg_watchdog_command.h | 210 --- .../pixhawk/mavlink_msg_watchdog_heartbeat.h | 166 --- .../mavlink_msg_watchdog_process_info.h | 227 ---- .../mavlink_msg_watchdog_process_status.h | 254 ---- .../include_v1.0/pixhawk/pixhawk.h | 80 -- .../include_v1.0/pixhawk/testsuite.h | 1150 ----------------- .../include_v1.0/pixhawk/version.h | 12 - .../GCS_MAVLink/include_v1.0/slugs/mavlink.h | 27 - .../include_v1.0/slugs/mavlink_msg_air_data.h | 188 --- .../include_v1.0/slugs/mavlink_msg_cpu_load.h | 188 --- .../slugs/mavlink_msg_ctrl_srfc_pt.h | 166 --- .../include_v1.0/slugs/mavlink_msg_data_log.h | 254 ---- .../slugs/mavlink_msg_diagnostic.h | 254 ---- .../slugs/mavlink_msg_gps_date_time.h | 276 ---- .../slugs/mavlink_msg_mid_lvl_cmds.h | 210 --- .../slugs/mavlink_msg_sensor_bias.h | 254 ---- .../slugs/mavlink_msg_slugs_action.h | 188 --- .../slugs/mavlink_msg_slugs_navigation.h | 320 ----- .../GCS_MAVLink/include_v1.0/slugs/slugs.h | 62 - .../include_v1.0/slugs/testsuite.h | 552 -------- .../GCS_MAVLink/include_v1.0/slugs/version.h | 12 - .../GCS_MAVLink/include_v1.0/test/mavlink.h | 27 - .../test/mavlink_msg_test_types.h | 610 --------- .../GCS_MAVLink/include_v1.0/test/test.h | 53 - .../GCS_MAVLink/include_v1.0/test/testsuite.h | 120 -- .../GCS_MAVLink/include_v1.0/test/version.h | 12 - .../include_v1.0/ualberta/mavlink.h | 27 - .../ualberta/mavlink_msg_nav_filter_bias.h | 276 ---- .../ualberta/mavlink_msg_radio_calibration.h | 259 ---- .../mavlink_msg_ualberta_sys_status.h | 188 --- .../include_v1.0/ualberta/testsuite.h | 192 --- .../include_v1.0/ualberta/ualberta.h | 84 -- .../include_v1.0/ualberta/version.h | 12 - .../message_definitions/ardupilotmega.xml | 122 ++ .../ardupilotmega.xml | 122 ++ .../message_definitions_v1.0/common.xml | 197 ++- .../message_definitions_v1.0/pixhawk.xml | 12 +- 183 files changed, 4807 insertions(+), 29371 deletions(-) delete mode 100644 libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_timing.h create mode 100644 libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_digicam_configure.h create mode 100644 libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_digicam_control.h create mode 100644 libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_mount_configure.h create mode 100644 libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_mount_control.h create mode 100644 libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_mount_status.h delete mode 100644 libraries/GCS_MAVLink/include/bittest.c delete mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_attitude_controller_output.h delete mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_attitude_new.h delete mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_full_state.h delete mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_position_controller_output.h delete mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_request_dynamic_gyro_calibration.h delete mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_request_static_calibration.h delete mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_set_roll_pitch_yaw.h delete mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_set_roll_pitch_yaw_speed.h delete mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_set_global_reference.h delete mode 100644 libraries/GCS_MAVLink/include/documentation.dox delete mode 100644 libraries/GCS_MAVLink/include/minimal/mavlink.h delete mode 100644 libraries/GCS_MAVLink/include/minimal/mavlink_msg_heartbeat.h delete mode 100644 libraries/GCS_MAVLink/include/minimal/minimal.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_attitude_control.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_aux_status.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_brief_feature.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_control_status.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_data_transmission_handshake.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_debug_vect.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_encapsulated_data.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_encapsulated_image.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_get_image_ack.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_image_available.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_image_trigger_control.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_image_triggered.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_manual_control.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_marker.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_pattern_detected.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_point_of_interest.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_point_of_interest_connection.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_position_control_offset_set.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_position_control_setpoint.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_raw_aux.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_request_data_stream.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_request_dynamic_gyro_calibration.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_request_static_calibration.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_set_altitude.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_set_cam_shutter.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_vicon_position_estimate.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_vision_position_estimate.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_vision_speed_estimate.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_watchdog_command.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_watchdog_process_info.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_watchdog_process_status.h delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/pixhawk.h delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink.h delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_air_data.h delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_cpu_load.h delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_ctrl_srfc_pt.h delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_data_log.h delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_diagnostic.h delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_filtered_data.h delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_gps_date_time.h delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_mid_lvl_cmds.h delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_pid.h delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_pilot_console.h delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_pwm_commands.h delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_sensor_bias.h delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_slugs_action.h delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_slugs_navigation.h delete mode 100644 libraries/GCS_MAVLink/include/slugs/slugs.h delete mode 100644 libraries/GCS_MAVLink/include/ualberta/mavlink.h delete mode 100644 libraries/GCS_MAVLink/include/ualberta/mavlink_msg_nav_filter_bias.h delete mode 100644 libraries/GCS_MAVLink/include/ualberta/mavlink_msg_radio_calibration.h delete mode 100644 libraries/GCS_MAVLink/include/ualberta/mavlink_msg_request_radio_calibration.h delete mode 100644 libraries/GCS_MAVLink/include/ualberta/mavlink_msg_request_rc_channels.h delete mode 100644 libraries/GCS_MAVLink/include/ualberta/mavlink_msg_ualberta_sys_status.h delete mode 100644 libraries/GCS_MAVLink/include/ualberta/ualberta.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/ardupilotmega/ardupilotmega_testsuite.h create mode 100644 libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_digicam_configure.h create mode 100644 libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_digicam_control.h create mode 100644 libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_mount_configure.h create mode 100644 libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_mount_control.h create mode 100644 libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_mount_status.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_command_short.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_global_position.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_gps_raw.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_gps_set_global_origin.h create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_request_partial_list.h create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_write_partial_list.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_position_target.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_set_flight_mode.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_set_safety_mode.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_system_time_utc.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_ack.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_clear_all.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_count.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_current.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_reached.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_request.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_request_list.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_set_current.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/minimal/mavlink.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/minimal/mavlink_msg_heartbeat.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/minimal/minimal.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/minimal/testsuite.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/minimal/version.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_attitude_control.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_brief_feature.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_data_transmission_handshake.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_encapsulated_data.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_image_available.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_image_trigger_control.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_image_triggered.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_marker.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_pattern_detected.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_point_of_interest.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_position_control_offset_set.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_position_control_setpoint.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_position_control_setpoint_set.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_raw_aux.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_set_cam_shutter.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_vicon_position_estimate.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_vision_position_estimate.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_vision_speed_estimate.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_watchdog_command.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_watchdog_process_info.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_watchdog_process_status.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/pixhawk.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/testsuite.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/pixhawk/version.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/slugs/mavlink.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_air_data.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_cpu_load.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_ctrl_srfc_pt.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_data_log.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_diagnostic.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_gps_date_time.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_mid_lvl_cmds.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_sensor_bias.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_slugs_action.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_slugs_navigation.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/slugs/slugs.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/slugs/testsuite.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/slugs/version.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/test/mavlink.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/test/mavlink_msg_test_types.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/test/test.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/test/testsuite.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/test/version.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/ualberta/mavlink.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/ualberta/mavlink_msg_nav_filter_bias.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/ualberta/mavlink_msg_radio_calibration.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/ualberta/mavlink_msg_ualberta_sys_status.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/ualberta/testsuite.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/ualberta/ualberta.h delete mode 100644 libraries/GCS_MAVLink/include_v1.0/ualberta/version.h diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h index e650159666..378cb03948 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5} +#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7} +#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 118, 242, 19, 97, 233, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, 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, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, 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} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, 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, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, 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, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, 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} #endif #include "../protocol.h" @@ -43,12 +43,79 @@ extern "C" { // 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 waypoint. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at waypoint for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the waypoint 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 waypoint (rotary wing)| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this waypoint an unlimited amount of time |Empty| Empty| Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this waypoint for X turns |Turns| Empty| Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this waypoint for X seconds |Seconds (decimal)| Empty| Radius around waypoint, 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)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + 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 capturing. |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 + devices such as cameras. + |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| 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_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| Empty| Empty| Empty| */ + 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_ENUM_END=246, /* | */ +}; +#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" #ifdef __cplusplus } diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_timing.h b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_timing.h deleted file mode 100644 index cc71f71bfa..0000000000 --- a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_timing.h +++ /dev/null @@ -1,210 +0,0 @@ -// MESSAGE AP_TIMING PACKING - -#define MAVLINK_MSG_ID_AP_TIMING 155 - -typedef struct __mavlink_ap_timing_t -{ - uint16_t min_50Hz_delta; ///< mininum delta for 50Hz loop - uint16_t max_50Hz_delta; ///< maximum delta for 50Hz loop - uint16_t min_200Hz_delta; ///< mininum delta for 200Hz loop - uint16_t max_200Hz_delta; ///< maximum delta for 200Hz loop -} mavlink_ap_timing_t; - -#define MAVLINK_MSG_ID_AP_TIMING_LEN 8 -#define MAVLINK_MSG_ID_155_LEN 8 - - - -#define MAVLINK_MESSAGE_INFO_AP_TIMING { \ - "AP_TIMING", \ - 4, \ - { { "min_50Hz_delta", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_ap_timing_t, min_50Hz_delta) }, \ - { "max_50Hz_delta", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_ap_timing_t, max_50Hz_delta) }, \ - { "min_200Hz_delta", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_ap_timing_t, min_200Hz_delta) }, \ - { "max_200Hz_delta", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_ap_timing_t, max_200Hz_delta) }, \ - } \ -} - - -/** - * @brief Pack a ap_timing 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 min_50Hz_delta mininum delta for 50Hz loop - * @param max_50Hz_delta maximum delta for 50Hz loop - * @param min_200Hz_delta mininum delta for 200Hz loop - * @param max_200Hz_delta maximum delta for 200Hz loop - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ap_timing_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t min_50Hz_delta, uint16_t max_50Hz_delta, uint16_t min_200Hz_delta, uint16_t max_200Hz_delta) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[8]; - _mav_put_uint16_t(buf, 0, min_50Hz_delta); - _mav_put_uint16_t(buf, 2, max_50Hz_delta); - _mav_put_uint16_t(buf, 4, min_200Hz_delta); - _mav_put_uint16_t(buf, 6, max_200Hz_delta); - - memcpy(_MAV_PAYLOAD(msg), buf, 8); -#else - mavlink_ap_timing_t packet; - packet.min_50Hz_delta = min_50Hz_delta; - packet.max_50Hz_delta = max_50Hz_delta; - packet.min_200Hz_delta = min_200Hz_delta; - packet.max_200Hz_delta = max_200Hz_delta; - - memcpy(_MAV_PAYLOAD(msg), &packet, 8); -#endif - - msg->msgid = MAVLINK_MSG_ID_AP_TIMING; - return mavlink_finalize_message(msg, system_id, component_id, 8); -} - -/** - * @brief Pack a ap_timing 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param min_50Hz_delta mininum delta for 50Hz loop - * @param max_50Hz_delta maximum delta for 50Hz loop - * @param min_200Hz_delta mininum delta for 200Hz loop - * @param max_200Hz_delta maximum delta for 200Hz loop - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ap_timing_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t min_50Hz_delta,uint16_t max_50Hz_delta,uint16_t min_200Hz_delta,uint16_t max_200Hz_delta) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[8]; - _mav_put_uint16_t(buf, 0, min_50Hz_delta); - _mav_put_uint16_t(buf, 2, max_50Hz_delta); - _mav_put_uint16_t(buf, 4, min_200Hz_delta); - _mav_put_uint16_t(buf, 6, max_200Hz_delta); - - memcpy(_MAV_PAYLOAD(msg), buf, 8); -#else - mavlink_ap_timing_t packet; - packet.min_50Hz_delta = min_50Hz_delta; - packet.max_50Hz_delta = max_50Hz_delta; - packet.min_200Hz_delta = min_200Hz_delta; - packet.max_200Hz_delta = max_200Hz_delta; - - memcpy(_MAV_PAYLOAD(msg), &packet, 8); -#endif - - msg->msgid = MAVLINK_MSG_ID_AP_TIMING; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8); -} - -/** - * @brief Encode a ap_timing struct into 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 ap_timing C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ap_timing_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ap_timing_t* ap_timing) -{ - return mavlink_msg_ap_timing_pack(system_id, component_id, msg, ap_timing->min_50Hz_delta, ap_timing->max_50Hz_delta, ap_timing->min_200Hz_delta, ap_timing->max_200Hz_delta); -} - -/** - * @brief Send a ap_timing message - * @param chan MAVLink channel to send the message - * - * @param min_50Hz_delta mininum delta for 50Hz loop - * @param max_50Hz_delta maximum delta for 50Hz loop - * @param min_200Hz_delta mininum delta for 200Hz loop - * @param max_200Hz_delta maximum delta for 200Hz loop - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_ap_timing_send(mavlink_channel_t chan, uint16_t min_50Hz_delta, uint16_t max_50Hz_delta, uint16_t min_200Hz_delta, uint16_t max_200Hz_delta) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[8]; - _mav_put_uint16_t(buf, 0, min_50Hz_delta); - _mav_put_uint16_t(buf, 2, max_50Hz_delta); - _mav_put_uint16_t(buf, 4, min_200Hz_delta); - _mav_put_uint16_t(buf, 6, max_200Hz_delta); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_TIMING, buf, 8); -#else - mavlink_ap_timing_t packet; - packet.min_50Hz_delta = min_50Hz_delta; - packet.max_50Hz_delta = max_50Hz_delta; - packet.min_200Hz_delta = min_200Hz_delta; - packet.max_200Hz_delta = max_200Hz_delta; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_TIMING, (const char *)&packet, 8); -#endif -} - -#endif - -// MESSAGE AP_TIMING UNPACKING - - -/** - * @brief Get field min_50Hz_delta from ap_timing message - * - * @return mininum delta for 50Hz loop - */ -static inline uint16_t mavlink_msg_ap_timing_get_min_50Hz_delta(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field max_50Hz_delta from ap_timing message - * - * @return maximum delta for 50Hz loop - */ -static inline uint16_t mavlink_msg_ap_timing_get_max_50Hz_delta(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Get field min_200Hz_delta from ap_timing message - * - * @return mininum delta for 200Hz loop - */ -static inline uint16_t mavlink_msg_ap_timing_get_min_200Hz_delta(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field max_200Hz_delta from ap_timing message - * - * @return maximum delta for 200Hz loop - */ -static inline uint16_t mavlink_msg_ap_timing_get_max_200Hz_delta(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Decode a ap_timing message into a struct - * - * @param msg The message to decode - * @param ap_timing C-struct to decode the message contents into - */ -static inline void mavlink_msg_ap_timing_decode(const mavlink_message_t* msg, mavlink_ap_timing_t* ap_timing) -{ -#if MAVLINK_NEED_BYTE_SWAP - ap_timing->min_50Hz_delta = mavlink_msg_ap_timing_get_min_50Hz_delta(msg); - ap_timing->max_50Hz_delta = mavlink_msg_ap_timing_get_max_50Hz_delta(msg); - ap_timing->min_200Hz_delta = mavlink_msg_ap_timing_get_min_200Hz_delta(msg); - ap_timing->max_200Hz_delta = mavlink_msg_ap_timing_get_max_200Hz_delta(msg); -#else - memcpy(ap_timing, _MAV_PAYLOAD(msg), 8); -#endif -} diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_digicam_configure.h b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_digicam_configure.h new file mode 100644 index 0000000000..92263b86a2 --- /dev/null +++ b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_digicam_configure.h @@ -0,0 +1,364 @@ +// MESSAGE DIGICAM_CONFIGURE PACKING + +#define MAVLINK_MSG_ID_DIGICAM_CONFIGURE 154 + +typedef struct __mavlink_digicam_configure_t +{ + 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) + uint16_t shutter_speed; ///< Divisor number //e.g. 1000 means 1/1000 (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) + float extra_value; ///< Correspondent value to given extra_param +} mavlink_digicam_configure_t; + +#define MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN 15 +#define MAVLINK_MSG_ID_154_LEN 15 + + + +#define MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE { \ + "DIGICAM_CONFIGURE", \ + 11, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_digicam_configure_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_digicam_configure_t, target_component) }, \ + { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_digicam_configure_t, mode) }, \ + { "shutter_speed", NULL, MAVLINK_TYPE_UINT16_T, 0, 3, offsetof(mavlink_digicam_configure_t, shutter_speed) }, \ + { "aperture", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_digicam_configure_t, aperture) }, \ + { "iso", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_digicam_configure_t, iso) }, \ + { "exposure_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_digicam_configure_t, exposure_type) }, \ + { "command_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_digicam_configure_t, command_id) }, \ + { "engine_cut_off", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_digicam_configure_t, engine_cut_off) }, \ + { "extra_param", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_digicam_configure_t, extra_param) }, \ + { "extra_value", NULL, MAVLINK_TYPE_FLOAT, 0, 11, offsetof(mavlink_digicam_configure_t, extra_value) }, \ + } \ +} + + +/** + * @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[15]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mode); + _mav_put_uint16_t(buf, 3, shutter_speed); + _mav_put_uint8_t(buf, 5, aperture); + _mav_put_uint8_t(buf, 6, iso); + _mav_put_uint8_t(buf, 7, exposure_type); + _mav_put_uint8_t(buf, 8, command_id); + _mav_put_uint8_t(buf, 9, engine_cut_off); + _mav_put_uint8_t(buf, 10, extra_param); + _mav_put_float(buf, 11, extra_value); + + memcpy(_MAV_PAYLOAD(msg), buf, 15); +#else + mavlink_digicam_configure_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.mode = mode; + packet.shutter_speed = shutter_speed; + 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; + packet.extra_value = extra_value; + + memcpy(_MAV_PAYLOAD(msg), &packet, 15); +#endif + + msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONFIGURE; + return mavlink_finalize_message(msg, system_id, component_id, 15); +} + +/** + * @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 was 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[15]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mode); + _mav_put_uint16_t(buf, 3, shutter_speed); + _mav_put_uint8_t(buf, 5, aperture); + _mav_put_uint8_t(buf, 6, iso); + _mav_put_uint8_t(buf, 7, exposure_type); + _mav_put_uint8_t(buf, 8, command_id); + _mav_put_uint8_t(buf, 9, engine_cut_off); + _mav_put_uint8_t(buf, 10, extra_param); + _mav_put_float(buf, 11, extra_value); + + memcpy(_MAV_PAYLOAD(msg), buf, 15); +#else + mavlink_digicam_configure_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.mode = mode; + packet.shutter_speed = shutter_speed; + 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; + packet.extra_value = extra_value; + + memcpy(_MAV_PAYLOAD(msg), &packet, 15); +#endif + + msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONFIGURE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 15); +} + +/** + * @brief Encode a digicam_configure struct into 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 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 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[15]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mode); + _mav_put_uint16_t(buf, 3, shutter_speed); + _mav_put_uint8_t(buf, 5, aperture); + _mav_put_uint8_t(buf, 6, iso); + _mav_put_uint8_t(buf, 7, exposure_type); + _mav_put_uint8_t(buf, 8, command_id); + _mav_put_uint8_t(buf, 9, engine_cut_off); + _mav_put_uint8_t(buf, 10, extra_param); + _mav_put_float(buf, 11, extra_value); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, 15); +#else + mavlink_digicam_configure_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.mode = mode; + packet.shutter_speed = shutter_speed; + 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; + packet.extra_value = extra_value; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)&packet, 15); +#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, 0); +} + +/** + * @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, 1); +} + +/** + * @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, 2); +} + +/** + * @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, 3); +} + +/** + * @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, 5); +} + +/** + * @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, 6); +} + +/** + * @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, 7); +} + +/** + * @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, 8); +} + +/** + * @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, 9); +} + +/** + * @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, 10); +} + +/** + * @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, 11); +} + +/** + * @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->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->shutter_speed = mavlink_msg_digicam_configure_get_shutter_speed(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); + digicam_configure->extra_value = mavlink_msg_digicam_configure_get_extra_value(msg); +#else + memcpy(digicam_configure, _MAV_PAYLOAD(msg), 15); +#endif +} diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_digicam_control.h b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_digicam_control.h new file mode 100644 index 0000000000..ee7d68124c --- /dev/null +++ b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_digicam_control.h @@ -0,0 +1,342 @@ +// MESSAGE DIGICAM_CONTROL PACKING + +#define MAVLINK_MSG_ID_DIGICAM_CONTROL 155 + +typedef struct __mavlink_digicam_control_t +{ + 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) + float extra_value; ///< Correspondent value to given extra_param +} mavlink_digicam_control_t; + +#define MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN 13 +#define MAVLINK_MSG_ID_155_LEN 13 + + + +#define MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL { \ + "DIGICAM_CONTROL", \ + 10, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_digicam_control_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_digicam_control_t, target_component) }, \ + { "session", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_digicam_control_t, session) }, \ + { "zoom_pos", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_digicam_control_t, zoom_pos) }, \ + { "zoom_step", NULL, MAVLINK_TYPE_INT8_T, 0, 4, offsetof(mavlink_digicam_control_t, zoom_step) }, \ + { "focus_lock", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_digicam_control_t, focus_lock) }, \ + { "shot", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_digicam_control_t, shot) }, \ + { "command_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_digicam_control_t, command_id) }, \ + { "extra_param", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_digicam_control_t, extra_param) }, \ + { "extra_value", NULL, MAVLINK_TYPE_FLOAT, 0, 9, offsetof(mavlink_digicam_control_t, extra_value) }, \ + } \ +} + + +/** + * @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[13]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, session); + _mav_put_uint8_t(buf, 3, zoom_pos); + _mav_put_int8_t(buf, 4, zoom_step); + _mav_put_uint8_t(buf, 5, focus_lock); + _mav_put_uint8_t(buf, 6, shot); + _mav_put_uint8_t(buf, 7, command_id); + _mav_put_uint8_t(buf, 8, extra_param); + _mav_put_float(buf, 9, extra_value); + + memcpy(_MAV_PAYLOAD(msg), buf, 13); +#else + mavlink_digicam_control_t packet; + 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; + packet.extra_value = extra_value; + + memcpy(_MAV_PAYLOAD(msg), &packet, 13); +#endif + + msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONTROL; + return mavlink_finalize_message(msg, system_id, component_id, 13); +} + +/** + * @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 was 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[13]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, session); + _mav_put_uint8_t(buf, 3, zoom_pos); + _mav_put_int8_t(buf, 4, zoom_step); + _mav_put_uint8_t(buf, 5, focus_lock); + _mav_put_uint8_t(buf, 6, shot); + _mav_put_uint8_t(buf, 7, command_id); + _mav_put_uint8_t(buf, 8, extra_param); + _mav_put_float(buf, 9, extra_value); + + memcpy(_MAV_PAYLOAD(msg), buf, 13); +#else + mavlink_digicam_control_t packet; + 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; + packet.extra_value = extra_value; + + memcpy(_MAV_PAYLOAD(msg), &packet, 13); +#endif + + msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONTROL; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 13); +} + +/** + * @brief Encode a digicam_control struct into 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 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 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[13]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, session); + _mav_put_uint8_t(buf, 3, zoom_pos); + _mav_put_int8_t(buf, 4, zoom_step); + _mav_put_uint8_t(buf, 5, focus_lock); + _mav_put_uint8_t(buf, 6, shot); + _mav_put_uint8_t(buf, 7, command_id); + _mav_put_uint8_t(buf, 8, extra_param); + _mav_put_float(buf, 9, extra_value); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, 13); +#else + mavlink_digicam_control_t packet; + 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; + packet.extra_value = extra_value; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)&packet, 13); +#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, 0); +} + +/** + * @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, 1); +} + +/** + * @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, 2); +} + +/** + * @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, 3); +} + +/** + * @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, 4); +} + +/** + * @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, 5); +} + +/** + * @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, 6); +} + +/** + * @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, 7); +} + +/** + * @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, 8); +} + +/** + * @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, 9); +} + +/** + * @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->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); + digicam_control->extra_value = mavlink_msg_digicam_control_get_extra_value(msg); +#else + memcpy(digicam_control, _MAV_PAYLOAD(msg), 13); +#endif +} diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_mount_configure.h b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_mount_configure.h new file mode 100644 index 0000000000..f2c73d301f --- /dev/null +++ b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_mount_configure.h @@ -0,0 +1,254 @@ +// 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_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[6]; + _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(msg), buf, 6); +#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(msg), &packet, 6); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_CONFIGURE; + return mavlink_finalize_message(msg, system_id, component_id, 6); +} + +/** + * @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 was 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[6]; + _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(msg), buf, 6); +#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(msg), &packet, 6); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_CONFIGURE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 6); +} + +/** + * @brief Encode a mount_configure struct into 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 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 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[6]; + _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); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, buf, 6); +#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; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, (const char *)&packet, 6); +#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), 6); +#endif +} diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_mount_control.h b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_mount_control.h new file mode 100644 index 0000000000..5f1d13eb8e --- /dev/null +++ b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_mount_control.h @@ -0,0 +1,254 @@ +// MESSAGE MOUNT_CONTROL PACKING + +#define MAVLINK_MSG_ID_MOUNT_CONTROL 157 + +typedef struct __mavlink_mount_control_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + 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 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_MESSAGE_INFO_MOUNT_CONTROL { \ + "MOUNT_CONTROL", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mount_control_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mount_control_t, target_component) }, \ + { "input_a", NULL, MAVLINK_TYPE_INT32_T, 0, 2, offsetof(mavlink_mount_control_t, input_a) }, \ + { "input_b", NULL, MAVLINK_TYPE_INT32_T, 0, 6, offsetof(mavlink_mount_control_t, input_b) }, \ + { "input_c", NULL, MAVLINK_TYPE_INT32_T, 0, 10, offsetof(mavlink_mount_control_t, input_c) }, \ + { "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[15]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_int32_t(buf, 2, input_a); + _mav_put_int32_t(buf, 6, input_b); + _mav_put_int32_t(buf, 10, input_c); + _mav_put_uint8_t(buf, 14, save_position); + + memcpy(_MAV_PAYLOAD(msg), buf, 15); +#else + mavlink_mount_control_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.input_a = input_a; + packet.input_b = input_b; + packet.input_c = input_c; + packet.save_position = save_position; + + memcpy(_MAV_PAYLOAD(msg), &packet, 15); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_CONTROL; + return mavlink_finalize_message(msg, system_id, component_id, 15); +} + +/** + * @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 was 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[15]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_int32_t(buf, 2, input_a); + _mav_put_int32_t(buf, 6, input_b); + _mav_put_int32_t(buf, 10, input_c); + _mav_put_uint8_t(buf, 14, save_position); + + memcpy(_MAV_PAYLOAD(msg), buf, 15); +#else + mavlink_mount_control_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.input_a = input_a; + packet.input_b = input_b; + packet.input_c = input_c; + packet.save_position = save_position; + + memcpy(_MAV_PAYLOAD(msg), &packet, 15); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_CONTROL; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 15); +} + +/** + * @brief Encode a mount_control struct into 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 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 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[15]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_int32_t(buf, 2, input_a); + _mav_put_int32_t(buf, 6, input_b); + _mav_put_int32_t(buf, 10, input_c); + _mav_put_uint8_t(buf, 14, save_position); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, buf, 15); +#else + mavlink_mount_control_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.input_a = input_a; + packet.input_b = input_b; + packet.input_c = input_c; + packet.save_position = save_position; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, (const char *)&packet, 15); +#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, 0); +} + +/** + * @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, 1); +} + +/** + * @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, 2); +} + +/** + * @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, 6); +} + +/** + * @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, 10); +} + +/** + * @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->target_system = mavlink_msg_mount_control_get_target_system(msg); + mount_control->target_component = mavlink_msg_mount_control_get_target_component(msg); + 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->save_position = mavlink_msg_mount_control_get_save_position(msg); +#else + memcpy(mount_control, _MAV_PAYLOAD(msg), 15); +#endif +} diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_mount_status.h b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_mount_status.h new file mode 100644 index 0000000000..04bddae287 --- /dev/null +++ b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_mount_status.h @@ -0,0 +1,232 @@ +// MESSAGE MOUNT_STATUS PACKING + +#define MAVLINK_MSG_ID_MOUNT_STATUS 158 + +typedef struct __mavlink_mount_status_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + 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 +} mavlink_mount_status_t; + +#define MAVLINK_MSG_ID_MOUNT_STATUS_LEN 14 +#define MAVLINK_MSG_ID_158_LEN 14 + + + +#define MAVLINK_MESSAGE_INFO_MOUNT_STATUS { \ + "MOUNT_STATUS", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mount_status_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mount_status_t, target_component) }, \ + { "pointing_a", NULL, MAVLINK_TYPE_INT32_T, 0, 2, offsetof(mavlink_mount_status_t, pointing_a) }, \ + { "pointing_b", NULL, MAVLINK_TYPE_INT32_T, 0, 6, offsetof(mavlink_mount_status_t, pointing_b) }, \ + { "pointing_c", NULL, MAVLINK_TYPE_INT32_T, 0, 10, offsetof(mavlink_mount_status_t, pointing_c) }, \ + } \ +} + + +/** + * @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[14]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_int32_t(buf, 2, pointing_a); + _mav_put_int32_t(buf, 6, pointing_b); + _mav_put_int32_t(buf, 10, pointing_c); + + memcpy(_MAV_PAYLOAD(msg), buf, 14); +#else + mavlink_mount_status_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.pointing_a = pointing_a; + packet.pointing_b = pointing_b; + packet.pointing_c = pointing_c; + + memcpy(_MAV_PAYLOAD(msg), &packet, 14); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, 14); +} + +/** + * @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 was 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[14]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_int32_t(buf, 2, pointing_a); + _mav_put_int32_t(buf, 6, pointing_b); + _mav_put_int32_t(buf, 10, pointing_c); + + memcpy(_MAV_PAYLOAD(msg), buf, 14); +#else + mavlink_mount_status_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.pointing_a = pointing_a; + packet.pointing_b = pointing_b; + packet.pointing_c = pointing_c; + + memcpy(_MAV_PAYLOAD(msg), &packet, 14); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14); +} + +/** + * @brief Encode a mount_status struct into 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 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 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[14]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_int32_t(buf, 2, pointing_a); + _mav_put_int32_t(buf, 6, pointing_b); + _mav_put_int32_t(buf, 10, pointing_c); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, buf, 14); +#else + mavlink_mount_status_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.pointing_a = pointing_a; + packet.pointing_b = pointing_b; + packet.pointing_c = pointing_c; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, (const char *)&packet, 14); +#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, 0); +} + +/** + * @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, 1); +} + +/** + * @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, 2); +} + +/** + * @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, 6); +} + +/** + * @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, 10); +} + +/** + * @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->target_system = mavlink_msg_mount_status_get_target_system(msg); + mount_status->target_component = mavlink_msg_mount_status_get_target_component(msg); + 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); +#else + memcpy(mount_status, _MAV_PAYLOAD(msg), 14); +#endif +} diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h index 6b7ebfe000..05e04127e2 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h @@ -200,14 +200,14 @@ static void mavlink_test_ap_adc(uint8_t system_id, uint8_t component_id, mavlink }; mavlink_ap_adc_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.adc1 = packet_in.adc1; - packet1.adc2 = packet_in.adc2; - packet1.adc3 = packet_in.adc3; - packet1.adc4 = packet_in.adc4; - packet1.adc5 = packet_in.adc5; - packet1.adc6 = packet_in.adc6; - - + packet1.adc1 = packet_in.adc1; + packet1.adc2 = packet_in.adc2; + packet1.adc3 = packet_in.adc3; + packet1.adc4 = packet_in.adc4; + packet1.adc5 = packet_in.adc5; + packet1.adc6 = packet_in.adc6; + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_ap_adc_encode(system_id, component_id, &msg, &packet1); @@ -227,23 +227,309 @@ static void mavlink_test_ap_adc(uint8_t system_id, uint8_t component_id, mavlink memset(&packet2, 0, sizeof(packet2)); mavlink_msg_to_send_buffer(buffer, &msg); for (i=0; i -#include - -int main(int argc, char* argv[]) -{ - - uint8_t bitfield = 254; // 11111110 - - uint8_t mask = 128; // 10000000 - - uint8_t result = (bitfield & mask); - - printf("0x%02x\n", bitfield); - - // Transform into network order - - generic_32bit bin; - bin.i = 1; - printf("First byte in (little endian) 0x%02x\n", bin.b[0]); - generic_32bit bout; - bout.b[0] = bin.b[3]; - bout.b[1] = bin.b[2]; - bout.b[2] = bin.b[1]; - bout.b[3] = bin.b[0]; - printf("Last byte out (big endian) 0x%02x\n", bout.b[3]); - - uint8_t n = 5; - printf("Mask is 0x%02x\n", ((uint32_t)(1 << n)) - 1); // = 2^n - 1 - - int32_t encoded = 2; - uint8_t bits = 2; - - uint8_t packet[MAVLINK_MAX_PACKET_LEN]; - uint8_t packet_index = 0; - uint8_t bit_index = 0; - - put_bitfield_n_by_index(encoded, bits, packet_index, bit_index, &bit_index, packet); - -} diff --git a/libraries/GCS_MAVLink/include/common/common.h b/libraries/GCS_MAVLink/include/common/common.h index 602ddea696..d61e06153d 100644 --- a/libraries/GCS_MAVLink/include/common/common.h +++ b/libraries/GCS_MAVLink/include/common/common.h @@ -43,56 +43,12 @@ extern "C" { // ENUM DEFINITIONS -/** @brief Commands to be executed by the MAV. They can be executed on user request, - or as part of a mission script. If the action is used in a mission, the parameter mapping - to the waypoint/mission message is as follows: - Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what - ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. */ -enum MAV_CMD -{ - MAV_CMD_NAV_WAYPOINT=16, /* Navigate to waypoint. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at waypoint for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the waypoint 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 waypoint (rotary wing)| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this waypoint an unlimited amount of time |Empty| Empty| Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this waypoint for X turns |Turns| Empty| Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this waypoint for X seconds |Seconds (decimal)| Empty| Radius around waypoint, 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)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - 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 capturing. |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 - devices such as cameras. - |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| 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| Empty| Empty| Empty| */ - 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_ENUM_END=246, /* | */ -}; - /** @brief Data stream IDs. A data stream is not a fixed set of messages, but rather a recommendation to the autopilot software. Individual autopilots may or may not obey the recommended messages. */ +#ifndef HAVE_ENUM_MAV_DATA_STREAM +#define HAVE_ENUM_MAV_DATA_STREAM enum MAV_DATA_STREAM { MAV_DATA_STREAM_ALL=0, /* Enable all data streams | */ @@ -106,11 +62,14 @@ enum MAV_DATA_STREAM MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot | */ MAV_DATA_STREAM_ENUM_END=13, /* | */ }; +#endif /** @brief The ROI (region of interest) for the vehicle. This can be be used by the vehicle for camera/vehicle attitude alignment (see MAV_CMD_NAV_ROI). */ +#ifndef HAVE_ENUM_MAV_ROI +#define HAVE_ENUM_MAV_ROI enum MAV_ROI { MAV_ROI_NONE=0, /* No region of interest. | */ @@ -120,6 +79,7 @@ enum MAV_ROI MAV_ROI_TARGET=4, /* Point toward of given id. | */ MAV_ROI_ENUM_END=5, /* | */ }; +#endif // MESSAGE DEFINITIONS #include "./mavlink_msg_heartbeat.h" diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_attitude_controller_output.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_attitude_controller_output.h deleted file mode 100644 index 60fa775540..0000000000 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_attitude_controller_output.h +++ /dev/null @@ -1,169 +0,0 @@ -// MESSAGE ATTITUDE_CONTROLLER_OUTPUT PACKING - -#define MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT 60 - -typedef struct __mavlink_attitude_controller_output_t -{ - uint8_t enabled; ///< 1: enabled, 0: disabled - int8_t roll; ///< Attitude roll: -128: -100%, 127: +100% - int8_t pitch; ///< Attitude pitch: -128: -100%, 127: +100% - int8_t yaw; ///< Attitude yaw: -128: -100%, 127: +100% - int8_t thrust; ///< Attitude thrust: -128: -100%, 127: +100% - -} mavlink_attitude_controller_output_t; - - - -/** - * @brief Pack a attitude_controller_output 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 enabled 1: enabled, 0: disabled - * @param roll Attitude roll: -128: -100%, 127: +100% - * @param pitch Attitude pitch: -128: -100%, 127: +100% - * @param yaw Attitude yaw: -128: -100%, 127: +100% - * @param thrust Attitude thrust: -128: -100%, 127: +100% - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_attitude_controller_output_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t enabled, int8_t roll, int8_t pitch, int8_t yaw, int8_t thrust) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT; - - i += put_uint8_t_by_index(enabled, i, msg->payload); // 1: enabled, 0: disabled - i += put_int8_t_by_index(roll, i, msg->payload); // Attitude roll: -128: -100%, 127: +100% - i += put_int8_t_by_index(pitch, i, msg->payload); // Attitude pitch: -128: -100%, 127: +100% - i += put_int8_t_by_index(yaw, i, msg->payload); // Attitude yaw: -128: -100%, 127: +100% - i += put_int8_t_by_index(thrust, i, msg->payload); // Attitude thrust: -128: -100%, 127: +100% - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a attitude_controller_output message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param enabled 1: enabled, 0: disabled - * @param roll Attitude roll: -128: -100%, 127: +100% - * @param pitch Attitude pitch: -128: -100%, 127: +100% - * @param yaw Attitude yaw: -128: -100%, 127: +100% - * @param thrust Attitude thrust: -128: -100%, 127: +100% - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_attitude_controller_output_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t enabled, int8_t roll, int8_t pitch, int8_t yaw, int8_t thrust) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT; - - i += put_uint8_t_by_index(enabled, i, msg->payload); // 1: enabled, 0: disabled - i += put_int8_t_by_index(roll, i, msg->payload); // Attitude roll: -128: -100%, 127: +100% - i += put_int8_t_by_index(pitch, i, msg->payload); // Attitude pitch: -128: -100%, 127: +100% - i += put_int8_t_by_index(yaw, i, msg->payload); // Attitude yaw: -128: -100%, 127: +100% - i += put_int8_t_by_index(thrust, i, msg->payload); // Attitude thrust: -128: -100%, 127: +100% - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a attitude_controller_output struct into 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 attitude_controller_output C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_attitude_controller_output_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_controller_output_t* attitude_controller_output) -{ - return mavlink_msg_attitude_controller_output_pack(system_id, component_id, msg, attitude_controller_output->enabled, attitude_controller_output->roll, attitude_controller_output->pitch, attitude_controller_output->yaw, attitude_controller_output->thrust); -} - -/** - * @brief Send a attitude_controller_output message - * @param chan MAVLink channel to send the message - * - * @param enabled 1: enabled, 0: disabled - * @param roll Attitude roll: -128: -100%, 127: +100% - * @param pitch Attitude pitch: -128: -100%, 127: +100% - * @param yaw Attitude yaw: -128: -100%, 127: +100% - * @param thrust Attitude thrust: -128: -100%, 127: +100% - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_attitude_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t roll, int8_t pitch, int8_t yaw, int8_t thrust) -{ - mavlink_message_t msg; - mavlink_msg_attitude_controller_output_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, enabled, roll, pitch, yaw, thrust); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE ATTITUDE_CONTROLLER_OUTPUT UNPACKING - -/** - * @brief Get field enabled from attitude_controller_output message - * - * @return 1: enabled, 0: disabled - */ -static inline uint8_t mavlink_msg_attitude_controller_output_get_enabled(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field roll from attitude_controller_output message - * - * @return Attitude roll: -128: -100%, 127: +100% - */ -static inline int8_t mavlink_msg_attitude_controller_output_get_roll(const mavlink_message_t* msg) -{ - return (int8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field pitch from attitude_controller_output message - * - * @return Attitude pitch: -128: -100%, 127: +100% - */ -static inline int8_t mavlink_msg_attitude_controller_output_get_pitch(const mavlink_message_t* msg) -{ - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t))[0]; -} - -/** - * @brief Get field yaw from attitude_controller_output message - * - * @return Attitude yaw: -128: -100%, 127: +100% - */ -static inline int8_t mavlink_msg_attitude_controller_output_get_yaw(const mavlink_message_t* msg) -{ - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t)+sizeof(int8_t))[0]; -} - -/** - * @brief Get field thrust from attitude_controller_output message - * - * @return Attitude thrust: -128: -100%, 127: +100% - */ -static inline int8_t mavlink_msg_attitude_controller_output_get_thrust(const mavlink_message_t* msg) -{ - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t)+sizeof(int8_t)+sizeof(int8_t))[0]; -} - -/** - * @brief Decode a attitude_controller_output message into a struct - * - * @param msg The message to decode - * @param attitude_controller_output C-struct to decode the message contents into - */ -static inline void mavlink_msg_attitude_controller_output_decode(const mavlink_message_t* msg, mavlink_attitude_controller_output_t* attitude_controller_output) -{ - attitude_controller_output->enabled = mavlink_msg_attitude_controller_output_get_enabled(msg); - attitude_controller_output->roll = mavlink_msg_attitude_controller_output_get_roll(msg); - attitude_controller_output->pitch = mavlink_msg_attitude_controller_output_get_pitch(msg); - attitude_controller_output->yaw = mavlink_msg_attitude_controller_output_get_yaw(msg); - attitude_controller_output->thrust = mavlink_msg_attitude_controller_output_get_thrust(msg); -} diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_attitude_new.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_attitude_new.h deleted file mode 100644 index cb55e3cd91..0000000000 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_attitude_new.h +++ /dev/null @@ -1,268 +0,0 @@ -// MESSAGE ATTITUDE PACKING - -#define MAVLINK_MSG_ID_ATTITUDE_NEW 30 -#define MAVLINK_MSG_LENGTH_ATTITUDE_NEW 8+4+4+4+4+4+4 - -#ifndef MAVLINK_DEACTIVATE_STRUCTS - -typedef struct __mavlink_attitude_new_t -{ - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - float roll; ///< Roll angle (rad) - float pitch; ///< Pitch angle (rad) - float yaw; ///< Yaw angle (rad) - float rollspeed; ///< Roll angular speed (rad/s) - float pitchspeed; ///< Pitch angular speed (rad/s) - float yawspeed; ///< Yaw angular speed (rad/s) - -} mavlink_attitude_new_t; - - - -/** - * @brief Pack a attitude_new 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 usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param rollspeed Roll angular speed (rad/s) - * @param pitchspeed Pitch angular speed (rad/s) - * @param yawspeed Yaw angular speed (rad/s) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_attitude_new_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_ATTITUDE; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad) - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad) - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad) - i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s) - i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s) - i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s) - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a attitude_new message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param rollspeed Roll angular speed (rad/s) - * @param pitchspeed Pitch angular speed (rad/s) - * @param yawspeed Yaw angular speed (rad/s) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_attitude_new_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_ATTITUDE; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad) - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad) - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad) - i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s) - i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s) - i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s) - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a attitude_new struct into 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 attitude_new C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_attitude_new_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_new_t* attitude_new) -{ - return mavlink_msg_attitude_new_pack(system_id, component_id, msg, attitude_new->usec, attitude_new->roll, attitude_new->pitch, attitude_new->yaw, attitude_new->rollspeed, attitude_new->pitchspeed, attitude_new->yawspeed); -} - -#endif - -/** - * @brief Send a attitude_new message - * @param chan MAVLink channel to send the message - * - * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param rollspeed Roll angular speed (rad/s) - * @param pitchspeed Pitch angular speed (rad/s) - * @param yawspeed Yaw angular speed (rad/s) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_attitude_new_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) -{ - uint16_t checksum; - comm_send_ch(chan, MAVLINK_STX); - crc_init(&checksum); - mavlink_send_uart_uint8_t(chan, MAVLINK_MSG_LENGTH_ATTITUDE_NEW, &checksum); - mavlink_send_uart_uint8_t(chan, mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq, &checksum); - // Increase sequence - mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1; - mavlink_send_uart_uint8_t(chan, mavlink_system.sysid, &checksum); - mavlink_send_uart_uint8_t(chan, mavlink_system.compid, &checksum); - mavlink_send_uart_uint8_t(chan, MAVLINK_MSG_ID_ATTITUDE_NEW, &checksum); - mavlink_send_uart_uint64_t(chan, usec, &checksum); - mavlink_send_uart_float(chan, roll, &checksum); - mavlink_send_uart_float(chan, pitch, &checksum); - mavlink_send_uart_float(chan, yaw, &checksum); - mavlink_send_uart_float(chan, rollspeed, &checksum); - mavlink_send_uart_float(chan, pitchspeed, &checksum); - mavlink_send_uart_float(chan, yawspeed, &checksum); - // Checksum complete, send it - comm_send_ch(chan, (uint8_t)(checksum & 0xFF)); - comm_send_ch(chan, (uint8_t)(checksum >> 8)); -} - -#endif -// MESSAGE ATTITUDE UNPACKING - -/** - * @brief Get field usec from attitude_new message - * - * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) - */ -static inline uint64_t mavlink_msg_attitude_new_get_usec(const mavlink_message_t* msg) -{ - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; -} - -/** - * @brief Get field roll from attitude_new message - * - * @return Roll angle (rad) - */ -static inline float mavlink_msg_attitude_new_get_roll(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (float)r.f; -} - -/** - * @brief Get field pitch from attitude_new message - * - * @return Pitch angle (rad) - */ -static inline float mavlink_msg_attitude_new_get_pitch(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field yaw from attitude_new message - * - * @return Yaw angle (rad) - */ -static inline float mavlink_msg_attitude_new_get_yaw(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field rollspeed from attitude_new message - * - * @return Roll angular speed (rad/s) - */ -static inline float mavlink_msg_attitude_new_get_rollspeed(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field pitchspeed from attitude_new message - * - * @return Pitch angular speed (rad/s) - */ -static inline float mavlink_msg_attitude_new_get_pitchspeed(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field yawspeed from attitude_new message - * - * @return Yaw angular speed (rad/s) - */ -static inline float mavlink_msg_attitude_new_get_yawspeed(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -#ifndef MAVLINK_DEACTIVATE_STRUCTS - -/** - * @brief Decode a attitude_new message into a struct - * - * @param msg The message to decode - * @param attitude_new C-struct to decode the message contents into - */ -static inline void mavlink_msg_attitude_new_decode(const mavlink_message_t* msg, mavlink_attitude_new_t* attitude_new) -{ - attitude_new->usec = mavlink_msg_attitude_new_get_usec(msg); - attitude_new->roll = mavlink_msg_attitude_new_get_roll(msg); - attitude_new->pitch = mavlink_msg_attitude_new_get_pitch(msg); - attitude_new->yaw = mavlink_msg_attitude_new_get_yaw(msg); - attitude_new->rollspeed = mavlink_msg_attitude_new_get_rollspeed(msg); - attitude_new->pitchspeed = mavlink_msg_attitude_new_get_pitchspeed(msg); - attitude_new->yawspeed = mavlink_msg_attitude_new_get_yawspeed(msg); -} - -#endif diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_full_state.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_full_state.h deleted file mode 100644 index ed177e22e2..0000000000 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_full_state.h +++ /dev/null @@ -1,428 +0,0 @@ -// MESSAGE FULL_STATE PACKING - -#define MAVLINK_MSG_ID_FULL_STATE 67 - -typedef struct __mavlink_full_state_t -{ - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - float roll; ///< Roll angle (rad) - float pitch; ///< Pitch angle (rad) - float yaw; ///< Yaw angle (rad) - float rollspeed; ///< Roll angular speed (rad/s) - float pitchspeed; ///< Pitch angular speed (rad/s) - float yawspeed; ///< Yaw angular speed (rad/s) - int32_t lat; ///< Latitude, expressed as * 1E7 - int32_t lon; ///< Longitude, expressed as * 1E7 - int32_t alt; ///< Altitude 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 - int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 - int16_t xacc; ///< X acceleration (mg) - int16_t yacc; ///< Y acceleration (mg) - int16_t zacc; ///< Z acceleration (mg) - -} mavlink_full_state_t; - - - -/** - * @brief Pack a full_state 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 usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param rollspeed Roll angular speed (rad/s) - * @param pitchspeed Pitch angular speed (rad/s) - * @param yawspeed Yaw angular speed (rad/s) - * @param lat Latitude, expressed as * 1E7 - * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude 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 - * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param xacc X acceleration (mg) - * @param yacc Y acceleration (mg) - * @param zacc Z acceleration (mg) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_full_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t 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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_FULL_STATE; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad) - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad) - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad) - i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s) - i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s) - i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s) - i += put_int32_t_by_index(lat, i, msg->payload); // Latitude, expressed as * 1E7 - i += put_int32_t_by_index(lon, i, msg->payload); // Longitude, expressed as * 1E7 - i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in meters, expressed as * 1000 (millimeters) - i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed (Latitude), expressed as m/s * 100 - i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed (Longitude), expressed as m/s * 100 - i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed (Altitude), expressed as m/s * 100 - i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (mg) - i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (mg) - i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (mg) - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a full_state message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param rollspeed Roll angular speed (rad/s) - * @param pitchspeed Pitch angular speed (rad/s) - * @param yawspeed Yaw angular speed (rad/s) - * @param lat Latitude, expressed as * 1E7 - * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude 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 - * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param xacc X acceleration (mg) - * @param yacc Y acceleration (mg) - * @param zacc Z acceleration (mg) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_full_state_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t 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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_FULL_STATE; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad) - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad) - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad) - i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s) - i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s) - i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s) - i += put_int32_t_by_index(lat, i, msg->payload); // Latitude, expressed as * 1E7 - i += put_int32_t_by_index(lon, i, msg->payload); // Longitude, expressed as * 1E7 - i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in meters, expressed as * 1000 (millimeters) - i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed (Latitude), expressed as m/s * 100 - i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed (Longitude), expressed as m/s * 100 - i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed (Altitude), expressed as m/s * 100 - i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (mg) - i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (mg) - i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (mg) - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a full_state struct into 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 full_state C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_full_state_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_full_state_t* full_state) -{ - return mavlink_msg_full_state_pack(system_id, component_id, msg, full_state->usec, full_state->roll, full_state->pitch, full_state->yaw, full_state->rollspeed, full_state->pitchspeed, full_state->yawspeed, full_state->lat, full_state->lon, full_state->alt, full_state->vx, full_state->vy, full_state->vz, full_state->xacc, full_state->yacc, full_state->zacc); -} - -/** - * @brief Send a full_state message - * @param chan MAVLink channel to send the message - * - * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param rollspeed Roll angular speed (rad/s) - * @param pitchspeed Pitch angular speed (rad/s) - * @param yawspeed Yaw angular speed (rad/s) - * @param lat Latitude, expressed as * 1E7 - * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude 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 - * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param xacc X acceleration (mg) - * @param yacc Y acceleration (mg) - * @param zacc Z acceleration (mg) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_full_state_send(mavlink_channel_t chan, uint64_t 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) -{ - mavlink_message_t msg; - mavlink_msg_full_state_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE FULL_STATE UNPACKING - -/** - * @brief Get field usec from full_state message - * - * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) - */ -static inline uint64_t mavlink_msg_full_state_get_usec(const mavlink_message_t* msg) -{ - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; -} - -/** - * @brief Get field roll from full_state message - * - * @return Roll angle (rad) - */ -static inline float mavlink_msg_full_state_get_roll(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (float)r.f; -} - -/** - * @brief Get field pitch from full_state message - * - * @return Pitch angle (rad) - */ -static inline float mavlink_msg_full_state_get_pitch(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field yaw from full_state message - * - * @return Yaw angle (rad) - */ -static inline float mavlink_msg_full_state_get_yaw(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field rollspeed from full_state message - * - * @return Roll angular speed (rad/s) - */ -static inline float mavlink_msg_full_state_get_rollspeed(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field pitchspeed from full_state message - * - * @return Pitch angular speed (rad/s) - */ -static inline float mavlink_msg_full_state_get_pitchspeed(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field yawspeed from full_state message - * - * @return Yaw angular speed (rad/s) - */ -static inline float mavlink_msg_full_state_get_yawspeed(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field lat from full_state message - * - * @return Latitude, expressed as * 1E7 - */ -static inline int32_t mavlink_msg_full_state_get_lat(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (int32_t)r.i; -} - -/** - * @brief Get field lon from full_state message - * - * @return Longitude, expressed as * 1E7 - */ -static inline int32_t mavlink_msg_full_state_get_lon(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[3]; - return (int32_t)r.i; -} - -/** - * @brief Get field alt from full_state message - * - * @return Altitude in meters, expressed as * 1000 (millimeters) - */ -static inline int32_t mavlink_msg_full_state_get_alt(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[3]; - return (int32_t)r.i; -} - -/** - * @brief Get field vx from full_state message - * - * @return Ground X Speed (Latitude), expressed as m/s * 100 - */ -static inline int16_t mavlink_msg_full_state_get_vx(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[1]; - return (int16_t)r.s; -} - -/** - * @brief Get field vy from full_state message - * - * @return Ground Y Speed (Longitude), expressed as m/s * 100 - */ -static inline int16_t mavlink_msg_full_state_get_vy(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; -} - -/** - * @brief Get field vz from full_state message - * - * @return Ground Z Speed (Altitude), expressed as m/s * 100 - */ -static inline int16_t mavlink_msg_full_state_get_vz(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; -} - -/** - * @brief Get field xacc from full_state message - * - * @return X acceleration (mg) - */ -static inline int16_t mavlink_msg_full_state_get_xacc(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; -} - -/** - * @brief Get field yacc from full_state message - * - * @return Y acceleration (mg) - */ -static inline int16_t mavlink_msg_full_state_get_yacc(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; -} - -/** - * @brief Get field zacc from full_state message - * - * @return Z acceleration (mg) - */ -static inline int16_t mavlink_msg_full_state_get_zacc(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; -} - -/** - * @brief Decode a full_state message into a struct - * - * @param msg The message to decode - * @param full_state C-struct to decode the message contents into - */ -static inline void mavlink_msg_full_state_decode(const mavlink_message_t* msg, mavlink_full_state_t* full_state) -{ - full_state->usec = mavlink_msg_full_state_get_usec(msg); - full_state->roll = mavlink_msg_full_state_get_roll(msg); - full_state->pitch = mavlink_msg_full_state_get_pitch(msg); - full_state->yaw = mavlink_msg_full_state_get_yaw(msg); - full_state->rollspeed = mavlink_msg_full_state_get_rollspeed(msg); - full_state->pitchspeed = mavlink_msg_full_state_get_pitchspeed(msg); - full_state->yawspeed = mavlink_msg_full_state_get_yawspeed(msg); - full_state->lat = mavlink_msg_full_state_get_lat(msg); - full_state->lon = mavlink_msg_full_state_get_lon(msg); - full_state->alt = mavlink_msg_full_state_get_alt(msg); - full_state->vx = mavlink_msg_full_state_get_vx(msg); - full_state->vy = mavlink_msg_full_state_get_vy(msg); - full_state->vz = mavlink_msg_full_state_get_vz(msg); - full_state->xacc = mavlink_msg_full_state_get_xacc(msg); - full_state->yacc = mavlink_msg_full_state_get_yacc(msg); - full_state->zacc = mavlink_msg_full_state_get_zacc(msg); -} diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_position_controller_output.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_position_controller_output.h deleted file mode 100644 index 389d9a2387..0000000000 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_position_controller_output.h +++ /dev/null @@ -1,169 +0,0 @@ -// MESSAGE POSITION_CONTROLLER_OUTPUT PACKING - -#define MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT 61 - -typedef struct __mavlink_position_controller_output_t -{ - uint8_t enabled; ///< 1: enabled, 0: disabled - int8_t x; ///< Position x: -128: -100%, 127: +100% - int8_t y; ///< Position y: -128: -100%, 127: +100% - int8_t z; ///< Position z: -128: -100%, 127: +100% - int8_t yaw; ///< Position yaw: -128: -100%, 127: +100% - -} mavlink_position_controller_output_t; - - - -/** - * @brief Pack a position_controller_output 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 enabled 1: enabled, 0: disabled - * @param x Position x: -128: -100%, 127: +100% - * @param y Position y: -128: -100%, 127: +100% - * @param z Position z: -128: -100%, 127: +100% - * @param yaw Position yaw: -128: -100%, 127: +100% - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_position_controller_output_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t enabled, int8_t x, int8_t y, int8_t z, int8_t yaw) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT; - - i += put_uint8_t_by_index(enabled, i, msg->payload); // 1: enabled, 0: disabled - i += put_int8_t_by_index(x, i, msg->payload); // Position x: -128: -100%, 127: +100% - i += put_int8_t_by_index(y, i, msg->payload); // Position y: -128: -100%, 127: +100% - i += put_int8_t_by_index(z, i, msg->payload); // Position z: -128: -100%, 127: +100% - i += put_int8_t_by_index(yaw, i, msg->payload); // Position yaw: -128: -100%, 127: +100% - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a position_controller_output message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param enabled 1: enabled, 0: disabled - * @param x Position x: -128: -100%, 127: +100% - * @param y Position y: -128: -100%, 127: +100% - * @param z Position z: -128: -100%, 127: +100% - * @param yaw Position yaw: -128: -100%, 127: +100% - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_position_controller_output_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t enabled, int8_t x, int8_t y, int8_t z, int8_t yaw) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT; - - i += put_uint8_t_by_index(enabled, i, msg->payload); // 1: enabled, 0: disabled - i += put_int8_t_by_index(x, i, msg->payload); // Position x: -128: -100%, 127: +100% - i += put_int8_t_by_index(y, i, msg->payload); // Position y: -128: -100%, 127: +100% - i += put_int8_t_by_index(z, i, msg->payload); // Position z: -128: -100%, 127: +100% - i += put_int8_t_by_index(yaw, i, msg->payload); // Position yaw: -128: -100%, 127: +100% - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a position_controller_output struct into 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 position_controller_output C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_position_controller_output_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_position_controller_output_t* position_controller_output) -{ - return mavlink_msg_position_controller_output_pack(system_id, component_id, msg, position_controller_output->enabled, position_controller_output->x, position_controller_output->y, position_controller_output->z, position_controller_output->yaw); -} - -/** - * @brief Send a position_controller_output message - * @param chan MAVLink channel to send the message - * - * @param enabled 1: enabled, 0: disabled - * @param x Position x: -128: -100%, 127: +100% - * @param y Position y: -128: -100%, 127: +100% - * @param z Position z: -128: -100%, 127: +100% - * @param yaw Position yaw: -128: -100%, 127: +100% - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_position_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t x, int8_t y, int8_t z, int8_t yaw) -{ - mavlink_message_t msg; - mavlink_msg_position_controller_output_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, enabled, x, y, z, yaw); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE POSITION_CONTROLLER_OUTPUT UNPACKING - -/** - * @brief Get field enabled from position_controller_output message - * - * @return 1: enabled, 0: disabled - */ -static inline uint8_t mavlink_msg_position_controller_output_get_enabled(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field x from position_controller_output message - * - * @return Position x: -128: -100%, 127: +100% - */ -static inline int8_t mavlink_msg_position_controller_output_get_x(const mavlink_message_t* msg) -{ - return (int8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field y from position_controller_output message - * - * @return Position y: -128: -100%, 127: +100% - */ -static inline int8_t mavlink_msg_position_controller_output_get_y(const mavlink_message_t* msg) -{ - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t))[0]; -} - -/** - * @brief Get field z from position_controller_output message - * - * @return Position z: -128: -100%, 127: +100% - */ -static inline int8_t mavlink_msg_position_controller_output_get_z(const mavlink_message_t* msg) -{ - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t)+sizeof(int8_t))[0]; -} - -/** - * @brief Get field yaw from position_controller_output message - * - * @return Position yaw: -128: -100%, 127: +100% - */ -static inline int8_t mavlink_msg_position_controller_output_get_yaw(const mavlink_message_t* msg) -{ - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t)+sizeof(int8_t)+sizeof(int8_t))[0]; -} - -/** - * @brief Decode a position_controller_output message into a struct - * - * @param msg The message to decode - * @param position_controller_output C-struct to decode the message contents into - */ -static inline void mavlink_msg_position_controller_output_decode(const mavlink_message_t* msg, mavlink_position_controller_output_t* position_controller_output) -{ - position_controller_output->enabled = mavlink_msg_position_controller_output_get_enabled(msg); - position_controller_output->x = mavlink_msg_position_controller_output_get_x(msg); - position_controller_output->y = mavlink_msg_position_controller_output_get_y(msg); - position_controller_output->z = mavlink_msg_position_controller_output_get_z(msg); - position_controller_output->yaw = mavlink_msg_position_controller_output_get_yaw(msg); -} diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_request_dynamic_gyro_calibration.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_request_dynamic_gyro_calibration.h deleted file mode 100644 index 3a3fedb12e..0000000000 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_request_dynamic_gyro_calibration.h +++ /dev/null @@ -1,177 +0,0 @@ -// MESSAGE REQUEST_DYNAMIC_GYRO_CALIBRATION PACKING - -#define MAVLINK_MSG_ID_REQUEST_DYNAMIC_GYRO_CALIBRATION 67 - -typedef struct __mavlink_request_dynamic_gyro_calibration_t -{ - uint8_t target_system; ///< The system which should auto-calibrate - uint8_t target_component; ///< The system component which should auto-calibrate - float mode; ///< The current ground-truth rpm - uint8_t axis; ///< The axis to calibrate: 0 roll, 1 pitch, 2 yaw - uint16_t time; ///< The time to average over in ms - -} mavlink_request_dynamic_gyro_calibration_t; - - - -/** - * @brief Pack a request_dynamic_gyro_calibration 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 The system which should auto-calibrate - * @param target_component The system component which should auto-calibrate - * @param mode The current ground-truth rpm - * @param axis The axis to calibrate: 0 roll, 1 pitch, 2 yaw - * @param time The time to average over in ms - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_request_dynamic_gyro_calibration_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float mode, uint8_t axis, uint16_t time) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_REQUEST_DYNAMIC_GYRO_CALIBRATION; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // The system which should auto-calibrate - i += put_uint8_t_by_index(target_component, i, msg->payload); // The system component which should auto-calibrate - i += put_float_by_index(mode, i, msg->payload); // The current ground-truth rpm - i += put_uint8_t_by_index(axis, i, msg->payload); // The axis to calibrate: 0 roll, 1 pitch, 2 yaw - i += put_uint16_t_by_index(time, i, msg->payload); // The time to average over in ms - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a request_dynamic_gyro_calibration message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system The system which should auto-calibrate - * @param target_component The system component which should auto-calibrate - * @param mode The current ground-truth rpm - * @param axis The axis to calibrate: 0 roll, 1 pitch, 2 yaw - * @param time The time to average over in ms - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_request_dynamic_gyro_calibration_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 mode, uint8_t axis, uint16_t time) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_REQUEST_DYNAMIC_GYRO_CALIBRATION; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // The system which should auto-calibrate - i += put_uint8_t_by_index(target_component, i, msg->payload); // The system component which should auto-calibrate - i += put_float_by_index(mode, i, msg->payload); // The current ground-truth rpm - i += put_uint8_t_by_index(axis, i, msg->payload); // The axis to calibrate: 0 roll, 1 pitch, 2 yaw - i += put_uint16_t_by_index(time, i, msg->payload); // The time to average over in ms - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a request_dynamic_gyro_calibration struct into 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 request_dynamic_gyro_calibration C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_request_dynamic_gyro_calibration_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_request_dynamic_gyro_calibration_t* request_dynamic_gyro_calibration) -{ - return mavlink_msg_request_dynamic_gyro_calibration_pack(system_id, component_id, msg, request_dynamic_gyro_calibration->target_system, request_dynamic_gyro_calibration->target_component, request_dynamic_gyro_calibration->mode, request_dynamic_gyro_calibration->axis, request_dynamic_gyro_calibration->time); -} - -/** - * @brief Send a request_dynamic_gyro_calibration message - * @param chan MAVLink channel to send the message - * - * @param target_system The system which should auto-calibrate - * @param target_component The system component which should auto-calibrate - * @param mode The current ground-truth rpm - * @param axis The axis to calibrate: 0 roll, 1 pitch, 2 yaw - * @param time The time to average over in ms - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_request_dynamic_gyro_calibration_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float mode, uint8_t axis, uint16_t time) -{ - mavlink_message_t msg; - mavlink_msg_request_dynamic_gyro_calibration_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, mode, axis, time); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE REQUEST_DYNAMIC_GYRO_CALIBRATION UNPACKING - -/** - * @brief Get field target_system from request_dynamic_gyro_calibration message - * - * @return The system which should auto-calibrate - */ -static inline uint8_t mavlink_msg_request_dynamic_gyro_calibration_get_target_system(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field target_component from request_dynamic_gyro_calibration message - * - * @return The system component which should auto-calibrate - */ -static inline uint8_t mavlink_msg_request_dynamic_gyro_calibration_get_target_component(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field mode from request_dynamic_gyro_calibration message - * - * @return The current ground-truth rpm - */ -static inline float mavlink_msg_request_dynamic_gyro_calibration_get_mode(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (float)r.f; -} - -/** - * @brief Get field axis from request_dynamic_gyro_calibration message - * - * @return The axis to calibrate: 0 roll, 1 pitch, 2 yaw - */ -static inline uint8_t mavlink_msg_request_dynamic_gyro_calibration_get_axis(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; -} - -/** - * @brief Get field time from request_dynamic_gyro_calibration message - * - * @return The time to average over in ms - */ -static inline uint16_t mavlink_msg_request_dynamic_gyro_calibration_get_time(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Decode a request_dynamic_gyro_calibration message into a struct - * - * @param msg The message to decode - * @param request_dynamic_gyro_calibration C-struct to decode the message contents into - */ -static inline void mavlink_msg_request_dynamic_gyro_calibration_decode(const mavlink_message_t* msg, mavlink_request_dynamic_gyro_calibration_t* request_dynamic_gyro_calibration) -{ - request_dynamic_gyro_calibration->target_system = mavlink_msg_request_dynamic_gyro_calibration_get_target_system(msg); - request_dynamic_gyro_calibration->target_component = mavlink_msg_request_dynamic_gyro_calibration_get_target_component(msg); - request_dynamic_gyro_calibration->mode = mavlink_msg_request_dynamic_gyro_calibration_get_mode(msg); - request_dynamic_gyro_calibration->axis = mavlink_msg_request_dynamic_gyro_calibration_get_axis(msg); - request_dynamic_gyro_calibration->time = mavlink_msg_request_dynamic_gyro_calibration_get_time(msg); -} diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_request_static_calibration.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_request_static_calibration.h deleted file mode 100644 index 309d65348b..0000000000 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_request_static_calibration.h +++ /dev/null @@ -1,138 +0,0 @@ -// MESSAGE REQUEST_STATIC_CALIBRATION PACKING - -#define MAVLINK_MSG_ID_REQUEST_STATIC_CALIBRATION 68 - -typedef struct __mavlink_request_static_calibration_t -{ - uint8_t target_system; ///< The system which should auto-calibrate - uint8_t target_component; ///< The system component which should auto-calibrate - uint16_t time; ///< The time to average over in ms - -} mavlink_request_static_calibration_t; - - - -/** - * @brief Pack a request_static_calibration 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 The system which should auto-calibrate - * @param target_component The system component which should auto-calibrate - * @param time The time to average over in ms - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_request_static_calibration_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t time) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_REQUEST_STATIC_CALIBRATION; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // The system which should auto-calibrate - i += put_uint8_t_by_index(target_component, i, msg->payload); // The system component which should auto-calibrate - i += put_uint16_t_by_index(time, i, msg->payload); // The time to average over in ms - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a request_static_calibration message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system The system which should auto-calibrate - * @param target_component The system component which should auto-calibrate - * @param time The time to average over in ms - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_request_static_calibration_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 time) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_REQUEST_STATIC_CALIBRATION; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // The system which should auto-calibrate - i += put_uint8_t_by_index(target_component, i, msg->payload); // The system component which should auto-calibrate - i += put_uint16_t_by_index(time, i, msg->payload); // The time to average over in ms - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a request_static_calibration struct into 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 request_static_calibration C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_request_static_calibration_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_request_static_calibration_t* request_static_calibration) -{ - return mavlink_msg_request_static_calibration_pack(system_id, component_id, msg, request_static_calibration->target_system, request_static_calibration->target_component, request_static_calibration->time); -} - -/** - * @brief Send a request_static_calibration message - * @param chan MAVLink channel to send the message - * - * @param target_system The system which should auto-calibrate - * @param target_component The system component which should auto-calibrate - * @param time The time to average over in ms - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_request_static_calibration_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t time) -{ - mavlink_message_t msg; - mavlink_msg_request_static_calibration_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, time); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE REQUEST_STATIC_CALIBRATION UNPACKING - -/** - * @brief Get field target_system from request_static_calibration message - * - * @return The system which should auto-calibrate - */ -static inline uint8_t mavlink_msg_request_static_calibration_get_target_system(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field target_component from request_static_calibration message - * - * @return The system component which should auto-calibrate - */ -static inline uint8_t mavlink_msg_request_static_calibration_get_target_component(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field time from request_static_calibration message - * - * @return The time to average over in ms - */ -static inline uint16_t mavlink_msg_request_static_calibration_get_time(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Decode a request_static_calibration message into a struct - * - * @param msg The message to decode - * @param request_static_calibration C-struct to decode the message contents into - */ -static inline void mavlink_msg_request_static_calibration_decode(const mavlink_message_t* msg, mavlink_request_static_calibration_t* request_static_calibration) -{ - request_static_calibration->target_system = mavlink_msg_request_static_calibration_get_target_system(msg); - request_static_calibration->target_component = mavlink_msg_request_static_calibration_get_target_component(msg); - request_static_calibration->time = mavlink_msg_request_static_calibration_get_time(msg); -} diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_set_roll_pitch_yaw.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_set_roll_pitch_yaw.h deleted file mode 100644 index 1cc68dff92..0000000000 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_set_roll_pitch_yaw.h +++ /dev/null @@ -1,184 +0,0 @@ -// MESSAGE SET_ROLL_PITCH_YAW PACKING - -#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW 55 - -typedef struct __mavlink_set_roll_pitch_yaw_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - float roll; ///< Desired roll angle in radians - float pitch; ///< Desired pitch angle in radians - float yaw; ///< Desired yaw angle in radians - -} mavlink_set_roll_pitch_yaw_t; - - - -/** - * @brief Pack a set_roll_pitch_yaw 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 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_float_by_index(roll, i, msg->payload); // Desired roll angle in radians - i += put_float_by_index(pitch, i, msg->payload); // Desired pitch angle in radians - i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle in radians - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a set_roll_pitch_yaw message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was 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 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_float_by_index(roll, i, msg->payload); // Desired roll angle in radians - i += put_float_by_index(pitch, i, msg->payload); // Desired pitch angle in radians - i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle in radians - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a set_roll_pitch_yaw struct into 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 set_roll_pitch_yaw C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_t* set_roll_pitch_yaw) -{ - return mavlink_msg_set_roll_pitch_yaw_pack(system_id, component_id, msg, set_roll_pitch_yaw->target_system, set_roll_pitch_yaw->target_component, set_roll_pitch_yaw->roll, set_roll_pitch_yaw->pitch, set_roll_pitch_yaw->yaw); -} - -/** - * @brief Send a set_roll_pitch_yaw 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 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_roll_pitch_yaw_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw) -{ - mavlink_message_t msg; - mavlink_msg_set_roll_pitch_yaw_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, roll, pitch, yaw); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE SET_ROLL_PITCH_YAW UNPACKING - -/** - * @brief Get field target_system from set_roll_pitch_yaw message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_set_roll_pitch_yaw_get_target_system(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field target_component from set_roll_pitch_yaw message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_set_roll_pitch_yaw_get_target_component(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field roll from set_roll_pitch_yaw message - * - * @return Desired roll angle in radians - */ -static inline float mavlink_msg_set_roll_pitch_yaw_get_roll(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (float)r.f; -} - -/** - * @brief Get field pitch from set_roll_pitch_yaw message - * - * @return Desired pitch angle in radians - */ -static inline float mavlink_msg_set_roll_pitch_yaw_get_pitch(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field yaw from set_roll_pitch_yaw message - * - * @return Desired yaw angle in radians - */ -static inline float mavlink_msg_set_roll_pitch_yaw_get_yaw(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Decode a set_roll_pitch_yaw message into a struct - * - * @param msg The message to decode - * @param set_roll_pitch_yaw C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_roll_pitch_yaw_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_t* set_roll_pitch_yaw) -{ - set_roll_pitch_yaw->target_system = mavlink_msg_set_roll_pitch_yaw_get_target_system(msg); - set_roll_pitch_yaw->target_component = mavlink_msg_set_roll_pitch_yaw_get_target_component(msg); - set_roll_pitch_yaw->roll = mavlink_msg_set_roll_pitch_yaw_get_roll(msg); - set_roll_pitch_yaw->pitch = mavlink_msg_set_roll_pitch_yaw_get_pitch(msg); - set_roll_pitch_yaw->yaw = mavlink_msg_set_roll_pitch_yaw_get_yaw(msg); -} diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_set_roll_pitch_yaw_speed.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_set_roll_pitch_yaw_speed.h deleted file mode 100644 index 7855daf765..0000000000 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_set_roll_pitch_yaw_speed.h +++ /dev/null @@ -1,184 +0,0 @@ -// MESSAGE SET_ROLL_PITCH_YAW_SPEED PACKING - -#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED 56 - -typedef struct __mavlink_set_roll_pitch_yaw_speed_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - 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 - -} mavlink_set_roll_pitch_yaw_speed_t; - - - -/** - * @brief Pack a set_roll_pitch_yaw_speed 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 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s - i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s - i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a set_roll_pitch_yaw_speed message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was 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 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s - i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s - i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a set_roll_pitch_yaw_speed struct into 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 set_roll_pitch_yaw_speed C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_t* set_roll_pitch_yaw_speed) -{ - return mavlink_msg_set_roll_pitch_yaw_speed_pack(system_id, component_id, msg, set_roll_pitch_yaw_speed->target_system, set_roll_pitch_yaw_speed->target_component, set_roll_pitch_yaw_speed->roll_speed, set_roll_pitch_yaw_speed->pitch_speed, set_roll_pitch_yaw_speed->yaw_speed); -} - -/** - * @brief Send a set_roll_pitch_yaw_speed 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 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_roll_pitch_yaw_speed_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed) -{ - mavlink_message_t msg; - mavlink_msg_set_roll_pitch_yaw_speed_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, roll_speed, pitch_speed, yaw_speed); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE SET_ROLL_PITCH_YAW_SPEED UNPACKING - -/** - * @brief Get field target_system from set_roll_pitch_yaw_speed message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_get_target_system(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field target_component from set_roll_pitch_yaw_speed message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_get_target_component(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field roll_speed from set_roll_pitch_yaw_speed message - * - * @return Desired roll angular speed in rad/s - */ -static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_roll_speed(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (float)r.f; -} - -/** - * @brief Get field pitch_speed from set_roll_pitch_yaw_speed message - * - * @return Desired pitch angular speed in rad/s - */ -static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_pitch_speed(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field yaw_speed from set_roll_pitch_yaw_speed message - * - * @return Desired yaw angular speed in rad/s - */ -static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_yaw_speed(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Decode a set_roll_pitch_yaw_speed message into a struct - * - * @param msg The message to decode - * @param set_roll_pitch_yaw_speed C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_roll_pitch_yaw_speed_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_speed_t* set_roll_pitch_yaw_speed) -{ - set_roll_pitch_yaw_speed->target_system = mavlink_msg_set_roll_pitch_yaw_speed_get_target_system(msg); - set_roll_pitch_yaw_speed->target_component = mavlink_msg_set_roll_pitch_yaw_speed_get_target_component(msg); - set_roll_pitch_yaw_speed->roll_speed = mavlink_msg_set_roll_pitch_yaw_speed_get_roll_speed(msg); - set_roll_pitch_yaw_speed->pitch_speed = mavlink_msg_set_roll_pitch_yaw_speed_get_pitch_speed(msg); - set_roll_pitch_yaw_speed->yaw_speed = mavlink_msg_set_roll_pitch_yaw_speed_get_yaw_speed(msg); -} diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_set_global_reference.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_set_global_reference.h deleted file mode 100644 index de2589c43d..0000000000 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_set_global_reference.h +++ /dev/null @@ -1,294 +0,0 @@ -// MESSAGE WAYPOINT_SET_GLOBAL_REFERENCE PACKING - -#define MAVLINK_MSG_ID_WAYPOINT_SET_GLOBAL_REFERENCE 48 - -typedef struct __mavlink_waypoint_set_global_reference_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - float global_x; ///< global x position - float global_y; ///< global y position - float global_z; ///< global z position - float global_yaw; ///< global yaw orientation in radians, 0 = NORTH - float local_x; ///< local x position that matches the global x position - float local_y; ///< local y position that matches the global y position - float local_z; ///< local z position that matches the global z position - float local_yaw; ///< local yaw that matches the global yaw orientation - -} mavlink_waypoint_set_global_reference_t; - - - -/** - * @brief Pack a waypoint_set_global_reference 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 global_x global x position - * @param global_y global y position - * @param global_z global z position - * @param global_yaw global yaw orientation in radians, 0 = NORTH - * @param local_x local x position that matches the global x position - * @param local_y local y position that matches the global y position - * @param local_z local z position that matches the global z position - * @param local_yaw local yaw that matches the global yaw orientation - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_set_global_reference_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float global_x, float global_y, float global_z, float global_yaw, float local_x, float local_y, float local_z, float local_yaw) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_GLOBAL_REFERENCE; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_float_by_index(global_x, i, msg->payload); // global x position - i += put_float_by_index(global_y, i, msg->payload); // global y position - i += put_float_by_index(global_z, i, msg->payload); // global z position - i += put_float_by_index(global_yaw, i, msg->payload); // global yaw orientation in radians, 0 = NORTH - i += put_float_by_index(local_x, i, msg->payload); // local x position that matches the global x position - i += put_float_by_index(local_y, i, msg->payload); // local y position that matches the global y position - i += put_float_by_index(local_z, i, msg->payload); // local z position that matches the global z position - i += put_float_by_index(local_yaw, i, msg->payload); // local yaw that matches the global yaw orientation - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a waypoint_set_global_reference message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param global_x global x position - * @param global_y global y position - * @param global_z global z position - * @param global_yaw global yaw orientation in radians, 0 = NORTH - * @param local_x local x position that matches the global x position - * @param local_y local y position that matches the global y position - * @param local_z local z position that matches the global z position - * @param local_yaw local yaw that matches the global yaw orientation - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_set_global_reference_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 global_x, float global_y, float global_z, float global_yaw, float local_x, float local_y, float local_z, float local_yaw) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_GLOBAL_REFERENCE; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_float_by_index(global_x, i, msg->payload); // global x position - i += put_float_by_index(global_y, i, msg->payload); // global y position - i += put_float_by_index(global_z, i, msg->payload); // global z position - i += put_float_by_index(global_yaw, i, msg->payload); // global yaw orientation in radians, 0 = NORTH - i += put_float_by_index(local_x, i, msg->payload); // local x position that matches the global x position - i += put_float_by_index(local_y, i, msg->payload); // local y position that matches the global y position - i += put_float_by_index(local_z, i, msg->payload); // local z position that matches the global z position - i += put_float_by_index(local_yaw, i, msg->payload); // local yaw that matches the global yaw orientation - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a waypoint_set_global_reference struct into 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 waypoint_set_global_reference C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_waypoint_set_global_reference_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_waypoint_set_global_reference_t* waypoint_set_global_reference) -{ - return mavlink_msg_waypoint_set_global_reference_pack(system_id, component_id, msg, waypoint_set_global_reference->target_system, waypoint_set_global_reference->target_component, waypoint_set_global_reference->global_x, waypoint_set_global_reference->global_y, waypoint_set_global_reference->global_z, waypoint_set_global_reference->global_yaw, waypoint_set_global_reference->local_x, waypoint_set_global_reference->local_y, waypoint_set_global_reference->local_z, waypoint_set_global_reference->local_yaw); -} - -/** - * @brief Send a waypoint_set_global_reference message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param global_x global x position - * @param global_y global y position - * @param global_z global z position - * @param global_yaw global yaw orientation in radians, 0 = NORTH - * @param local_x local x position that matches the global x position - * @param local_y local y position that matches the global y position - * @param local_z local z position that matches the global z position - * @param local_yaw local yaw that matches the global yaw orientation - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_waypoint_set_global_reference_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float global_x, float global_y, float global_z, float global_yaw, float local_x, float local_y, float local_z, float local_yaw) -{ - mavlink_message_t msg; - mavlink_msg_waypoint_set_global_reference_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, global_x, global_y, global_z, global_yaw, local_x, local_y, local_z, local_yaw); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE WAYPOINT_SET_GLOBAL_REFERENCE UNPACKING - -/** - * @brief Get field target_system from waypoint_set_global_reference message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_waypoint_set_global_reference_get_target_system(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field target_component from waypoint_set_global_reference message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_waypoint_set_global_reference_get_target_component(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field global_x from waypoint_set_global_reference message - * - * @return global x position - */ -static inline float mavlink_msg_waypoint_set_global_reference_get_global_x(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (float)r.f; -} - -/** - * @brief Get field global_y from waypoint_set_global_reference message - * - * @return global y position - */ -static inline float mavlink_msg_waypoint_set_global_reference_get_global_y(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field global_z from waypoint_set_global_reference message - * - * @return global z position - */ -static inline float mavlink_msg_waypoint_set_global_reference_get_global_z(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field global_yaw from waypoint_set_global_reference message - * - * @return global yaw orientation in radians, 0 = NORTH - */ -static inline float mavlink_msg_waypoint_set_global_reference_get_global_yaw(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field local_x from waypoint_set_global_reference message - * - * @return local x position that matches the global x position - */ -static inline float mavlink_msg_waypoint_set_global_reference_get_local_x(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field local_y from waypoint_set_global_reference message - * - * @return local y position that matches the global y position - */ -static inline float mavlink_msg_waypoint_set_global_reference_get_local_y(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field local_z from waypoint_set_global_reference message - * - * @return local z position that matches the global z position - */ -static inline float mavlink_msg_waypoint_set_global_reference_get_local_z(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field local_yaw from waypoint_set_global_reference message - * - * @return local yaw that matches the global yaw orientation - */ -static inline float mavlink_msg_waypoint_set_global_reference_get_local_yaw(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Decode a waypoint_set_global_reference message into a struct - * - * @param msg The message to decode - * @param waypoint_set_global_reference C-struct to decode the message contents into - */ -static inline void mavlink_msg_waypoint_set_global_reference_decode(const mavlink_message_t* msg, mavlink_waypoint_set_global_reference_t* waypoint_set_global_reference) -{ - waypoint_set_global_reference->target_system = mavlink_msg_waypoint_set_global_reference_get_target_system(msg); - waypoint_set_global_reference->target_component = mavlink_msg_waypoint_set_global_reference_get_target_component(msg); - waypoint_set_global_reference->global_x = mavlink_msg_waypoint_set_global_reference_get_global_x(msg); - waypoint_set_global_reference->global_y = mavlink_msg_waypoint_set_global_reference_get_global_y(msg); - waypoint_set_global_reference->global_z = mavlink_msg_waypoint_set_global_reference_get_global_z(msg); - waypoint_set_global_reference->global_yaw = mavlink_msg_waypoint_set_global_reference_get_global_yaw(msg); - waypoint_set_global_reference->local_x = mavlink_msg_waypoint_set_global_reference_get_local_x(msg); - waypoint_set_global_reference->local_y = mavlink_msg_waypoint_set_global_reference_get_local_y(msg); - waypoint_set_global_reference->local_z = mavlink_msg_waypoint_set_global_reference_get_local_z(msg); - waypoint_set_global_reference->local_yaw = mavlink_msg_waypoint_set_global_reference_get_local_yaw(msg); -} diff --git a/libraries/GCS_MAVLink/include/common/version.h b/libraries/GCS_MAVLink/include/common/version.h index ae365895bf..e5ba6ae624 100644 --- a/libraries/GCS_MAVLink/include/common/version.h +++ b/libraries/GCS_MAVLink/include/common/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Sun Oct 16 13:23:42 2011" +#define MAVLINK_BUILD_DATE "Sat Oct 29 18:06:02 2011" #define MAVLINK_WIRE_PROTOCOL_VERSION "0.9" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101 diff --git a/libraries/GCS_MAVLink/include/documentation.dox b/libraries/GCS_MAVLink/include/documentation.dox deleted file mode 100644 index 49de0050e4..0000000000 --- a/libraries/GCS_MAVLink/include/documentation.dox +++ /dev/null @@ -1,41 +0,0 @@ -/** - * @file - * @brief MAVLink communication protocol - * - * @author Lorenz Meier - * - */ - - -/** - * @mainpage MAVLink API Documentation - * - * @section intro_sec Introduction - * - * This API documentation covers the MAVLink - * protocol developed PIXHAWK project. - * In case you have generated this documentation locally, the most recent version (generated on every commit) - * is also publicly available on the internet. - * - * @sa http://pixhawk.ethz.ch/api/qgroundcontrol/ - Groundstation code base - * @sa http://pixhawk.ethz.ch/api/mavlink - (this) MAVLink communication protocol - * @sa http://pixhawk.ethz.ch/api/imu_autopilot/ - Flight board (ARM MCU) code base - * @sa http://pixhawk.ethz.ch/api/ai_vision - Computer Vision / AI API docs - * - * @section further_sec Further Information - * - * How to run our software and a general overview of the software architecture is documented in the project - * wiki pages. - * - * @sa http://pixhawk.ethz.ch/software/mavlink/ - MAVLink main documentation - * - * See the PIXHAWK website for more information. - * - * @section usage_sec Doxygen Usage - * - * You can exclude files from being parsed into this Doxygen documentation - * by adding them to the EXCLUDE list in the file in embedded/cmake/doc/api/doxy.config.in. - * - * - * - **/ diff --git a/libraries/GCS_MAVLink/include/mavlink_helpers.h b/libraries/GCS_MAVLink/include/mavlink_helpers.h index 9f4f063780..98250e1ac8 100644 --- a/libraries/GCS_MAVLink/include/mavlink_helpers.h +++ b/libraries/GCS_MAVLink/include/mavlink_helpers.h @@ -214,7 +214,13 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa break; case MAVLINK_PARSE_STATE_GOT_STX: - if (status->msg_received || c > MAVLINK_MAX_PAYLOAD_LEN) + if (status->msg_received + /* Support shorter buffers than the + default maximum packet size */ +#if (MAVLINK_MAX_PAYLOAD_LEN < 255) + || c > MAVLINK_MAX_PAYLOAD_LEN +#endif + ) { status->buffer_overrun++; status->parse_error++; diff --git a/libraries/GCS_MAVLink/include/minimal/mavlink.h b/libraries/GCS_MAVLink/include/minimal/mavlink.h deleted file mode 100644 index 38389146ab..0000000000 --- a/libraries/GCS_MAVLink/include/minimal/mavlink.h +++ /dev/null @@ -1,11 +0,0 @@ -/** @file - * @brief MAVLink comm protocol. - * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Friday, August 5 2011, 07:37 UTC - */ -#ifndef MAVLINK_H -#define MAVLINK_H - -#include "minimal.h" - -#endif diff --git a/libraries/GCS_MAVLink/include/minimal/mavlink_msg_heartbeat.h b/libraries/GCS_MAVLink/include/minimal/mavlink_msg_heartbeat.h deleted file mode 100644 index 0e5c4db5ca..0000000000 --- a/libraries/GCS_MAVLink/include/minimal/mavlink_msg_heartbeat.h +++ /dev/null @@ -1,132 +0,0 @@ -// MESSAGE HEARTBEAT PACKING - -#define MAVLINK_MSG_ID_HEARTBEAT 0 - -typedef struct __mavlink_heartbeat_t -{ - uint8_t type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - uint8_t autopilot; ///< Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - uint8_t mavlink_version; ///< MAVLink version - -} mavlink_heartbeat_t; - - - -/** - * @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 Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE 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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - - i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - i += put_uint8_t_by_index(2, i, msg->payload); // MAVLink version - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @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 chan The MAVLink channel this message was 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 Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE 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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - - i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - i += put_uint8_t_by_index(2, i, msg->payload); // MAVLink version - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a heartbeat struct into 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 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); -} - -/** - * @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 Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) -{ - mavlink_message_t msg; - mavlink_msg_heartbeat_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, autopilot); - mavlink_send_uart(chan, &msg); -} - -#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 (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field autopilot from heartbeat message - * - * @return Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - */ -static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @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 (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -/** - * @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) -{ - heartbeat->type = mavlink_msg_heartbeat_get_type(msg); - heartbeat->autopilot = mavlink_msg_heartbeat_get_autopilot(msg); - heartbeat->mavlink_version = mavlink_msg_heartbeat_get_mavlink_version(msg); -} diff --git a/libraries/GCS_MAVLink/include/minimal/minimal.h b/libraries/GCS_MAVLink/include/minimal/minimal.h deleted file mode 100644 index ee09135a68..0000000000 --- a/libraries/GCS_MAVLink/include/minimal/minimal.h +++ /dev/null @@ -1,45 +0,0 @@ -/** @file - * @brief MAVLink comm protocol. - * @see http://qgroundcontrol.org/mavlink/ - * Generated on Friday, August 5 2011, 07:37 UTC - */ -#ifndef MINIMAL_H -#define MINIMAL_H - -#ifdef __cplusplus -extern "C" { -#endif - - -#include "../protocol.h" - -#define MAVLINK_ENABLED_MINIMAL - -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -// ENUM DEFINITIONS - - -// MESSAGE DEFINITIONS - -#include "./mavlink_msg_heartbeat.h" - - -// MESSAGE LENGTHS - -#undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { } - -#ifdef __cplusplus -} -#endif -#endif diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink.h deleted file mode 100644 index 7d19e1ba6a..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink.h +++ /dev/null @@ -1,11 +0,0 @@ -/** @file - * @brief MAVLink comm protocol. - * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Friday, August 5 2011, 07:37 UTC - */ -#ifndef MAVLINK_H -#define MAVLINK_H - -#include "pixhawk.h" - -#endif diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_attitude_control.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_attitude_control.h deleted file mode 100644 index 1a8b806f8a..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_attitude_control.h +++ /dev/null @@ -1,257 +0,0 @@ -// MESSAGE ATTITUDE_CONTROL PACKING - -#define MAVLINK_MSG_ID_ATTITUDE_CONTROL 85 - -typedef struct __mavlink_attitude_control_t -{ - uint8_t target; ///< The system to be controlled - float roll; ///< roll - float pitch; ///< pitch - float yaw; ///< yaw - float thrust; ///< thrust - uint8_t roll_manual; ///< roll control enabled auto:0, manual:1 - uint8_t pitch_manual; ///< pitch auto:0, manual:1 - uint8_t yaw_manual; ///< yaw auto:0, manual:1 - uint8_t thrust_manual; ///< thrust auto:0, manual:1 - -} mavlink_attitude_control_t; - - - -/** - * @brief Pack a attitude_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 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(uint8_t system_id, uint8_t component_id, 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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; - - i += put_uint8_t_by_index(target, i, msg->payload); // The system to be controlled - i += put_float_by_index(roll, i, msg->payload); // roll - i += put_float_by_index(pitch, i, msg->payload); // pitch - i += put_float_by_index(yaw, i, msg->payload); // yaw - i += put_float_by_index(thrust, i, msg->payload); // thrust - i += put_uint8_t_by_index(roll_manual, i, msg->payload); // roll control enabled auto:0, manual:1 - i += put_uint8_t_by_index(pitch_manual, i, msg->payload); // pitch auto:0, manual:1 - i += put_uint8_t_by_index(yaw_manual, i, msg->payload); // yaw auto:0, manual:1 - i += put_uint8_t_by_index(thrust_manual, i, msg->payload); // thrust auto:0, manual:1 - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a attitude_control message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was 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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; - - i += put_uint8_t_by_index(target, i, msg->payload); // The system to be controlled - i += put_float_by_index(roll, i, msg->payload); // roll - i += put_float_by_index(pitch, i, msg->payload); // pitch - i += put_float_by_index(yaw, i, msg->payload); // yaw - i += put_float_by_index(thrust, i, msg->payload); // thrust - i += put_uint8_t_by_index(roll_manual, i, msg->payload); // roll control enabled auto:0, manual:1 - i += put_uint8_t_by_index(pitch_manual, i, msg->payload); // pitch auto:0, manual:1 - i += put_uint8_t_by_index(yaw_manual, i, msg->payload); // yaw auto:0, manual:1 - i += put_uint8_t_by_index(thrust_manual, i, msg->payload); // thrust auto:0, manual:1 - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a attitude_control struct into 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 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 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) -{ - mavlink_message_t msg; - mavlink_msg_attitude_control_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual); - mavlink_send_uart(chan, &msg); -} - -#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 (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field roll from attitude_control message - * - * @return roll - */ -static inline float mavlink_msg_attitude_control_get_roll(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (float)r.f; -} - -/** - * @brief Get field pitch from attitude_control message - * - * @return pitch - */ -static inline float mavlink_msg_attitude_control_get_pitch(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field yaw from attitude_control message - * - * @return yaw - */ -static inline float mavlink_msg_attitude_control_get_yaw(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field thrust from attitude_control message - * - * @return thrust - */ -static inline float mavlink_msg_attitude_control_get_thrust(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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 (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; -} - -/** - * @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 (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[0]; -} - -/** - * @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 (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -/** - * @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 (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -/** - * @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) -{ - attitude_control->target = mavlink_msg_attitude_control_get_target(msg); - 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->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); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_aux_status.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_aux_status.h deleted file mode 100644 index f9dbde8c86..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_aux_status.h +++ /dev/null @@ -1,204 +0,0 @@ -// MESSAGE AUX_STATUS PACKING - -#define MAVLINK_MSG_ID_AUX_STATUS 142 - -typedef struct __mavlink_aux_status_t -{ - uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - uint16_t i2c0_err_count; ///< Number of I2C errors since startup - uint16_t i2c1_err_count; ///< Number of I2C errors since startup - uint16_t spi0_err_count; ///< Number of I2C errors since startup - uint16_t spi1_err_count; ///< Number of I2C errors since startup - uint16_t uart_total_err_count; ///< Number of I2C errors since startup - -} mavlink_aux_status_t; - - - -/** - * @brief Pack a aux_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 load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - * @param i2c0_err_count Number of I2C errors since startup - * @param i2c1_err_count Number of I2C errors since startup - * @param spi0_err_count Number of I2C errors since startup - * @param spi1_err_count Number of I2C errors since startup - * @param uart_total_err_count Number of I2C errors since startup - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_aux_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t load, uint16_t i2c0_err_count, uint16_t i2c1_err_count, uint16_t spi0_err_count, uint16_t spi1_err_count, uint16_t uart_total_err_count) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_AUX_STATUS; - - i += put_uint16_t_by_index(load, i, msg->payload); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - i += put_uint16_t_by_index(i2c0_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(i2c1_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(spi0_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(spi1_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(uart_total_err_count, i, msg->payload); // Number of I2C errors since startup - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a aux_status message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - * @param i2c0_err_count Number of I2C errors since startup - * @param i2c1_err_count Number of I2C errors since startup - * @param spi0_err_count Number of I2C errors since startup - * @param spi1_err_count Number of I2C errors since startup - * @param uart_total_err_count Number of I2C errors since startup - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_aux_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t load, uint16_t i2c0_err_count, uint16_t i2c1_err_count, uint16_t spi0_err_count, uint16_t spi1_err_count, uint16_t uart_total_err_count) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_AUX_STATUS; - - i += put_uint16_t_by_index(load, i, msg->payload); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - i += put_uint16_t_by_index(i2c0_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(i2c1_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(spi0_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(spi1_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(uart_total_err_count, i, msg->payload); // Number of I2C errors since startup - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a aux_status struct into 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 aux_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_aux_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_aux_status_t* aux_status) -{ - return mavlink_msg_aux_status_pack(system_id, component_id, msg, aux_status->load, aux_status->i2c0_err_count, aux_status->i2c1_err_count, aux_status->spi0_err_count, aux_status->spi1_err_count, aux_status->uart_total_err_count); -} - -/** - * @brief Send a aux_status message - * @param chan MAVLink channel to send the message - * - * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - * @param i2c0_err_count Number of I2C errors since startup - * @param i2c1_err_count Number of I2C errors since startup - * @param spi0_err_count Number of I2C errors since startup - * @param spi1_err_count Number of I2C errors since startup - * @param uart_total_err_count Number of I2C errors since startup - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_aux_status_send(mavlink_channel_t chan, uint16_t load, uint16_t i2c0_err_count, uint16_t i2c1_err_count, uint16_t spi0_err_count, uint16_t spi1_err_count, uint16_t uart_total_err_count) -{ - mavlink_message_t msg; - mavlink_msg_aux_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, load, i2c0_err_count, i2c1_err_count, spi0_err_count, spi1_err_count, uart_total_err_count); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE AUX_STATUS UNPACKING - -/** - * @brief Get field load from aux_status message - * - * @return Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - */ -static inline uint16_t mavlink_msg_aux_status_get_load(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field i2c0_err_count from aux_status message - * - * @return Number of I2C errors since startup - */ -static inline uint16_t mavlink_msg_aux_status_get_i2c0_err_count(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field i2c1_err_count from aux_status message - * - * @return Number of I2C errors since startup - */ -static inline uint16_t mavlink_msg_aux_status_get_i2c1_err_count(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field spi0_err_count from aux_status message - * - * @return Number of I2C errors since startup - */ -static inline uint16_t mavlink_msg_aux_status_get_spi0_err_count(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field spi1_err_count from aux_status message - * - * @return Number of I2C errors since startup - */ -static inline uint16_t mavlink_msg_aux_status_get_spi1_err_count(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field uart_total_err_count from aux_status message - * - * @return Number of I2C errors since startup - */ -static inline uint16_t mavlink_msg_aux_status_get_uart_total_err_count(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Decode a aux_status message into a struct - * - * @param msg The message to decode - * @param aux_status C-struct to decode the message contents into - */ -static inline void mavlink_msg_aux_status_decode(const mavlink_message_t* msg, mavlink_aux_status_t* aux_status) -{ - aux_status->load = mavlink_msg_aux_status_get_load(msg); - aux_status->i2c0_err_count = mavlink_msg_aux_status_get_i2c0_err_count(msg); - aux_status->i2c1_err_count = mavlink_msg_aux_status_get_i2c1_err_count(msg); - aux_status->spi0_err_count = mavlink_msg_aux_status_get_spi0_err_count(msg); - aux_status->spi1_err_count = mavlink_msg_aux_status_get_spi1_err_count(msg); - aux_status->uart_total_err_count = mavlink_msg_aux_status_get_uart_total_err_count(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_brief_feature.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_brief_feature.h deleted file mode 100644 index a61e13bcde..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_brief_feature.h +++ /dev/null @@ -1,249 +0,0 @@ -// MESSAGE BRIEF_FEATURE PACKING - -#define MAVLINK_MSG_ID_BRIEF_FEATURE 172 - -typedef struct __mavlink_brief_feature_t -{ - float x; ///< x position in m - float y; ///< y position in m - float z; ///< z position in m - uint8_t orientation_assignment; ///< Orientation assignment 0: false, 1:true - uint16_t size; ///< Size in pixels - uint16_t orientation; ///< Orientation - uint8_t descriptor[32]; ///< Descriptor - float response; ///< Harris operator response at this location - -} mavlink_brief_feature_t; - -#define MAVLINK_MSG_BRIEF_FEATURE_FIELD_DESCRIPTOR_LEN 32 - - -/** - * @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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; - - i += put_float_by_index(x, i, msg->payload); // x position in m - i += put_float_by_index(y, i, msg->payload); // y position in m - i += put_float_by_index(z, i, msg->payload); // z position in m - i += put_uint8_t_by_index(orientation_assignment, i, msg->payload); // Orientation assignment 0: false, 1:true - i += put_uint16_t_by_index(size, i, msg->payload); // Size in pixels - i += put_uint16_t_by_index(orientation, i, msg->payload); // Orientation - i += put_array_by_index((const int8_t*)descriptor, sizeof(uint8_t)*32, i, msg->payload); // Descriptor - i += put_float_by_index(response, i, msg->payload); // Harris operator response at this location - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @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 chan The MAVLink channel this message was 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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; - - i += put_float_by_index(x, i, msg->payload); // x position in m - i += put_float_by_index(y, i, msg->payload); // y position in m - i += put_float_by_index(z, i, msg->payload); // z position in m - i += put_uint8_t_by_index(orientation_assignment, i, msg->payload); // Orientation assignment 0: false, 1:true - i += put_uint16_t_by_index(size, i, msg->payload); // Size in pixels - i += put_uint16_t_by_index(orientation, i, msg->payload); // Orientation - i += put_array_by_index((const int8_t*)descriptor, sizeof(uint8_t)*32, i, msg->payload); // Descriptor - i += put_float_by_index(response, i, msg->payload); // Harris operator response at this location - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a brief_feature struct into 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 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 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) -{ - mavlink_message_t msg; - mavlink_msg_brief_feature_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, x, y, z, orientation_assignment, size, orientation, descriptor, response); - mavlink_send_uart(chan, &msg); -} - -#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) -{ - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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 (uint8_t)(msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; -} - -/** - * @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) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; -} - -/** - * @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) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @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* r_data) -{ - - memcpy(r_data, msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t), sizeof(uint8_t)*32); - return sizeof(uint8_t)*32; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)*32)[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)*32)[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)*32)[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)*32)[3]; - return (float)r.f; -} - -/** - * @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) -{ - 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->orientation_assignment = mavlink_msg_brief_feature_get_orientation_assignment(msg); - brief_feature->size = mavlink_msg_brief_feature_get_size(msg); - brief_feature->orientation = mavlink_msg_brief_feature_get_orientation(msg); - mavlink_msg_brief_feature_get_descriptor(msg, brief_feature->descriptor); - brief_feature->response = mavlink_msg_brief_feature_get_response(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_control_status.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_control_status.h deleted file mode 100644 index 7bf85b32d9..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_control_status.h +++ /dev/null @@ -1,203 +0,0 @@ -// MESSAGE CONTROL_STATUS PACKING - -#define MAVLINK_MSG_ID_CONTROL_STATUS 143 - -typedef struct __mavlink_control_status_t -{ - uint8_t position_fix; ///< Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - uint8_t vision_fix; ///< Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - uint8_t gps_fix; ///< GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - uint8_t control_att; ///< 0: Attitude control disabled, 1: enabled - uint8_t control_pos_xy; ///< 0: X, Y position control disabled, 1: enabled - uint8_t control_pos_z; ///< 0: Z position control disabled, 1: enabled - uint8_t control_pos_yaw; ///< 0: Yaw angle control disabled, 1: enabled - -} mavlink_control_status_t; - - - -/** - * @brief Pack a control_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 position_fix Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - * @param vision_fix Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - * @param gps_fix GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - * @param control_att 0: Attitude control disabled, 1: enabled - * @param control_pos_xy 0: X, Y position control disabled, 1: enabled - * @param control_pos_z 0: Z position control disabled, 1: enabled - * @param control_pos_yaw 0: Yaw angle control disabled, 1: enabled - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_control_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t position_fix, uint8_t vision_fix, uint8_t gps_fix, uint8_t control_att, uint8_t control_pos_xy, uint8_t control_pos_z, uint8_t control_pos_yaw) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_CONTROL_STATUS; - - i += put_uint8_t_by_index(position_fix, i, msg->payload); // Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - i += put_uint8_t_by_index(vision_fix, i, msg->payload); // Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - i += put_uint8_t_by_index(gps_fix, i, msg->payload); // GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - i += put_uint8_t_by_index(control_att, i, msg->payload); // 0: Attitude control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_xy, i, msg->payload); // 0: X, Y position control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_z, i, msg->payload); // 0: Z position control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_yaw, i, msg->payload); // 0: Yaw angle control disabled, 1: enabled - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a control_status message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param position_fix Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - * @param vision_fix Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - * @param gps_fix GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - * @param control_att 0: Attitude control disabled, 1: enabled - * @param control_pos_xy 0: X, Y position control disabled, 1: enabled - * @param control_pos_z 0: Z position control disabled, 1: enabled - * @param control_pos_yaw 0: Yaw angle control disabled, 1: enabled - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_control_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t position_fix, uint8_t vision_fix, uint8_t gps_fix, uint8_t control_att, uint8_t control_pos_xy, uint8_t control_pos_z, uint8_t control_pos_yaw) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_CONTROL_STATUS; - - i += put_uint8_t_by_index(position_fix, i, msg->payload); // Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - i += put_uint8_t_by_index(vision_fix, i, msg->payload); // Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - i += put_uint8_t_by_index(gps_fix, i, msg->payload); // GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - i += put_uint8_t_by_index(control_att, i, msg->payload); // 0: Attitude control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_xy, i, msg->payload); // 0: X, Y position control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_z, i, msg->payload); // 0: Z position control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_yaw, i, msg->payload); // 0: Yaw angle control disabled, 1: enabled - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a control_status struct into 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 control_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_control_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_control_status_t* control_status) -{ - return mavlink_msg_control_status_pack(system_id, component_id, msg, control_status->position_fix, control_status->vision_fix, control_status->gps_fix, control_status->control_att, control_status->control_pos_xy, control_status->control_pos_z, control_status->control_pos_yaw); -} - -/** - * @brief Send a control_status message - * @param chan MAVLink channel to send the message - * - * @param position_fix Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - * @param vision_fix Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - * @param gps_fix GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - * @param control_att 0: Attitude control disabled, 1: enabled - * @param control_pos_xy 0: X, Y position control disabled, 1: enabled - * @param control_pos_z 0: Z position control disabled, 1: enabled - * @param control_pos_yaw 0: Yaw angle control disabled, 1: enabled - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_control_status_send(mavlink_channel_t chan, uint8_t position_fix, uint8_t vision_fix, uint8_t gps_fix, uint8_t control_att, uint8_t control_pos_xy, uint8_t control_pos_z, uint8_t control_pos_yaw) -{ - mavlink_message_t msg; - mavlink_msg_control_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, position_fix, vision_fix, gps_fix, control_att, control_pos_xy, control_pos_z, control_pos_yaw); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE CONTROL_STATUS UNPACKING - -/** - * @brief Get field position_fix from control_status message - * - * @return Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - */ -static inline uint8_t mavlink_msg_control_status_get_position_fix(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field vision_fix from control_status message - * - * @return Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - */ -static inline uint8_t mavlink_msg_control_status_get_vision_fix(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field gps_fix from control_status message - * - * @return GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - */ -static inline uint8_t mavlink_msg_control_status_get_gps_fix(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field control_att from control_status message - * - * @return 0: Attitude control disabled, 1: enabled - */ -static inline uint8_t mavlink_msg_control_status_get_control_att(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field control_pos_xy from control_status message - * - * @return 0: X, Y position control disabled, 1: enabled - */ -static inline uint8_t mavlink_msg_control_status_get_control_pos_xy(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field control_pos_z from control_status message - * - * @return 0: Z position control disabled, 1: enabled - */ -static inline uint8_t mavlink_msg_control_status_get_control_pos_z(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field control_pos_yaw from control_status message - * - * @return 0: Yaw angle control disabled, 1: enabled - */ -static inline uint8_t mavlink_msg_control_status_get_control_pos_yaw(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -/** - * @brief Decode a control_status message into a struct - * - * @param msg The message to decode - * @param control_status C-struct to decode the message contents into - */ -static inline void mavlink_msg_control_status_decode(const mavlink_message_t* msg, mavlink_control_status_t* control_status) -{ - control_status->position_fix = mavlink_msg_control_status_get_position_fix(msg); - control_status->vision_fix = mavlink_msg_control_status_get_vision_fix(msg); - control_status->gps_fix = mavlink_msg_control_status_get_gps_fix(msg); - control_status->control_att = mavlink_msg_control_status_get_control_att(msg); - control_status->control_pos_xy = mavlink_msg_control_status_get_control_pos_xy(msg); - control_status->control_pos_z = mavlink_msg_control_status_get_control_pos_z(msg); - control_status->control_pos_yaw = mavlink_msg_control_status_get_control_pos_yaw(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_data_transmission_handshake.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_data_transmission_handshake.h deleted file mode 100644 index e9e69f7c9f..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_data_transmission_handshake.h +++ /dev/null @@ -1,174 +0,0 @@ -// MESSAGE DATA_TRANSMISSION_HANDSHAKE PACKING - -#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE 170 - -typedef struct __mavlink_data_transmission_handshake_t -{ - uint8_t type; ///< type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - uint32_t size; ///< total data size in bytes (set on ACK only) - uint8_t packets; ///< number of packets beeing sent (set on ACK only) - uint8_t payload; ///< payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - uint8_t jpg_quality; ///< JPEG quality out of [1,100] - -} mavlink_data_transmission_handshake_t; - - - -/** - * @brief Pack a data_transmission_handshake 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 requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - * @param size total data size in bytes (set on ACK only) - * @param packets number of packets beeing sent (set on ACK only) - * @param payload payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - * @param jpg_quality JPEG quality out of [1,100] - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint32_t size, uint8_t packets, uint8_t payload, uint8_t jpg_quality) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; - - i += put_uint8_t_by_index(type, i, msg->payload); // type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - i += put_uint32_t_by_index(size, i, msg->payload); // total data size in bytes (set on ACK only) - i += put_uint8_t_by_index(packets, i, msg->payload); // number of packets beeing sent (set on ACK only) - i += put_uint8_t_by_index(payload, i, msg->payload); // payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - i += put_uint8_t_by_index(jpg_quality, i, msg->payload); // JPEG quality out of [1,100] - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a data_transmission_handshake message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param type type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - * @param size total data size in bytes (set on ACK only) - * @param packets number of packets beeing sent (set on ACK only) - * @param payload payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - * @param jpg_quality JPEG quality out of [1,100] - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data_transmission_handshake_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint32_t size, uint8_t packets, uint8_t payload, uint8_t jpg_quality) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; - - i += put_uint8_t_by_index(type, i, msg->payload); // type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - i += put_uint32_t_by_index(size, i, msg->payload); // total data size in bytes (set on ACK only) - i += put_uint8_t_by_index(packets, i, msg->payload); // number of packets beeing sent (set on ACK only) - i += put_uint8_t_by_index(payload, i, msg->payload); // payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - i += put_uint8_t_by_index(jpg_quality, i, msg->payload); // JPEG quality out of [1,100] - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a data_transmission_handshake struct into 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 data_transmission_handshake C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data_transmission_handshake_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data_transmission_handshake_t* data_transmission_handshake) -{ - return mavlink_msg_data_transmission_handshake_pack(system_id, component_id, msg, data_transmission_handshake->type, data_transmission_handshake->size, data_transmission_handshake->packets, data_transmission_handshake->payload, data_transmission_handshake->jpg_quality); -} - -/** - * @brief Send a data_transmission_handshake message - * @param chan MAVLink channel to send the message - * - * @param type type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - * @param size total data size in bytes (set on ACK only) - * @param packets number of packets beeing sent (set on ACK only) - * @param payload payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - * @param jpg_quality JPEG quality out of [1,100] - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_t chan, uint8_t type, uint32_t size, uint8_t packets, uint8_t payload, uint8_t jpg_quality) -{ - mavlink_message_t msg; - mavlink_msg_data_transmission_handshake_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, size, packets, payload, jpg_quality); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE DATA_TRANSMISSION_HANDSHAKE UNPACKING - -/** - * @brief Get field type from data_transmission_handshake message - * - * @return type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - */ -static inline uint8_t mavlink_msg_data_transmission_handshake_get_type(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field size from data_transmission_handshake message - * - * @return total data size in bytes (set on ACK only) - */ -static inline uint32_t mavlink_msg_data_transmission_handshake_get_size(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (uint32_t)r.i; -} - -/** - * @brief Get field packets from data_transmission_handshake message - * - * @return number of packets beeing sent (set on ACK only) - */ -static inline uint8_t mavlink_msg_data_transmission_handshake_get_packets(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint32_t))[0]; -} - -/** - * @brief Get field payload from data_transmission_handshake message - * - * @return payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - */ -static inline uint8_t mavlink_msg_data_transmission_handshake_get_payload(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field jpg_quality from data_transmission_handshake message - * - * @return JPEG quality out of [1,100] - */ -static inline uint8_t mavlink_msg_data_transmission_handshake_get_jpg_quality(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -/** - * @brief Decode a data_transmission_handshake message into a struct - * - * @param msg The message to decode - * @param data_transmission_handshake C-struct to decode the message contents into - */ -static inline void mavlink_msg_data_transmission_handshake_decode(const mavlink_message_t* msg, mavlink_data_transmission_handshake_t* data_transmission_handshake) -{ - data_transmission_handshake->type = mavlink_msg_data_transmission_handshake_get_type(msg); - data_transmission_handshake->size = mavlink_msg_data_transmission_handshake_get_size(msg); - data_transmission_handshake->packets = mavlink_msg_data_transmission_handshake_get_packets(msg); - data_transmission_handshake->payload = mavlink_msg_data_transmission_handshake_get_payload(msg); - data_transmission_handshake->jpg_quality = mavlink_msg_data_transmission_handshake_get_jpg_quality(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_debug_vect.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_debug_vect.h deleted file mode 100644 index aff7d2c103..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_debug_vect.h +++ /dev/null @@ -1,156 +0,0 @@ -// MESSAGE DEBUG_VECT PACKING - -#define MAVLINK_MSG_ID_DEBUG_VECT 90 - -typedef struct __mavlink_debug_vect_t -{ - int8_t name[10]; ///< Name - uint64_t usec; ///< Timestamp - float x; ///< x - float y; ///< y - float z; ///< z - -} mavlink_debug_vect_t; - -#define MAVLINK_MSG_DEBUG_VECT_FIELD_NAME_LEN 10 - - -/** - * @brief Send a debug_vect message - * - * @param name Name - * @param usec Timestamp - * @param x x - * @param y y - * @param z z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const int8_t* name, uint64_t usec, float x, float y, float z) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; - - i += put_array_by_index(name, 10, i, msg->payload); //Name - i += put_uint64_t_by_index(usec, i, msg->payload); //Timestamp - i += put_float_by_index(x, i, msg->payload); //x - i += put_float_by_index(y, i, msg->payload); //y - i += put_float_by_index(z, i, msg->payload); //z - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -static inline uint16_t mavlink_msg_debug_vect_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const int8_t* name, uint64_t usec, float x, float y, float z) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; - - i += put_array_by_index(name, 10, i, msg->payload); //Name - i += put_uint64_t_by_index(usec, i, msg->payload); //Timestamp - i += put_float_by_index(x, i, msg->payload); //x - i += put_float_by_index(y, i, msg->payload); //y - i += put_float_by_index(z, i, msg->payload); //z - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_debug_vect_t* debug_vect) -{ - return mavlink_msg_debug_vect_pack(system_id, component_id, msg, debug_vect->name, debug_vect->usec, debug_vect->x, debug_vect->y, debug_vect->z); -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const int8_t* name, uint64_t usec, float x, float y, float z) -{ - mavlink_message_t msg; - mavlink_msg_debug_vect_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, name, usec, x, y, z); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE DEBUG_VECT UNPACKING - -/** - * @brief Get field name from debug_vect message - * - * @return Name - */ -static inline uint16_t mavlink_msg_debug_vect_get_name(const mavlink_message_t* msg, int8_t* r_data) -{ - - memcpy(r_data, msg->payload, 10); - return 10; -} - -/** - * @brief Get field usec from debug_vect message - * - * @return Timestamp - */ -static inline uint64_t mavlink_msg_debug_vect_get_usec(const mavlink_message_t* msg) -{ - generic_64bit r; - r.b[7] = (msg->payload+10)[0]; - r.b[6] = (msg->payload+10)[1]; - r.b[5] = (msg->payload+10)[2]; - r.b[4] = (msg->payload+10)[3]; - r.b[3] = (msg->payload+10)[4]; - r.b[2] = (msg->payload+10)[5]; - r.b[1] = (msg->payload+10)[6]; - r.b[0] = (msg->payload+10)[7]; - return (uint64_t)r.ll; -} - -/** - * @brief Get field x from debug_vect message - * - * @return x - */ -static inline float mavlink_msg_debug_vect_get_x(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+10+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+10+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+10+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+10+sizeof(uint64_t))[3]; - return (float)r.f; -} - -/** - * @brief Get field y from debug_vect message - * - * @return y - */ -static inline float mavlink_msg_debug_vect_get_y(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+10+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+10+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+10+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+10+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field z from debug_vect message - * - * @return z - */ -static inline float mavlink_msg_debug_vect_get_z(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -static inline void mavlink_msg_debug_vect_decode(const mavlink_message_t* msg, mavlink_debug_vect_t* debug_vect) -{ - mavlink_msg_debug_vect_get_name(msg, debug_vect->name); - debug_vect->usec = mavlink_msg_debug_vect_get_usec(msg); - debug_vect->x = mavlink_msg_debug_vect_get_x(msg); - debug_vect->y = mavlink_msg_debug_vect_get_y(msg); - debug_vect->z = mavlink_msg_debug_vect_get_z(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_encapsulated_data.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_encapsulated_data.h deleted file mode 100644 index c2c040ae65..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_encapsulated_data.h +++ /dev/null @@ -1,124 +0,0 @@ -// MESSAGE ENCAPSULATED_DATA PACKING - -#define MAVLINK_MSG_ID_ENCAPSULATED_DATA 171 - -typedef struct __mavlink_encapsulated_data_t -{ - uint16_t seqnr; ///< sequence number (starting with 0 on every transmission) - uint8_t data[253]; ///< image data bytes - -} mavlink_encapsulated_data_t; - -#define MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN 253 - - -/** - * @brief Pack a encapsulated_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 seqnr sequence number (starting with 0 on every transmission) - * @param data image data bytes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_encapsulated_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t seqnr, const uint8_t* data) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; - - i += put_uint16_t_by_index(seqnr, i, msg->payload); // sequence number (starting with 0 on every transmission) - i += put_array_by_index((const int8_t*)data, sizeof(uint8_t)*253, i, msg->payload); // image data bytes - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a encapsulated_data message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param seqnr sequence number (starting with 0 on every transmission) - * @param data image data bytes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_encapsulated_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t seqnr, const uint8_t* data) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; - - i += put_uint16_t_by_index(seqnr, i, msg->payload); // sequence number (starting with 0 on every transmission) - i += put_array_by_index((const int8_t*)data, sizeof(uint8_t)*253, i, msg->payload); // image data bytes - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a encapsulated_data struct into 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 encapsulated_data C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_encapsulated_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_encapsulated_data_t* encapsulated_data) -{ - return mavlink_msg_encapsulated_data_pack(system_id, component_id, msg, encapsulated_data->seqnr, encapsulated_data->data); -} - -/** - * @brief Send a encapsulated_data message - * @param chan MAVLink channel to send the message - * - * @param seqnr sequence number (starting with 0 on every transmission) - * @param data image data bytes - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, uint16_t seqnr, const uint8_t* data) -{ - mavlink_message_t msg; - mavlink_msg_encapsulated_data_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, seqnr, data); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE ENCAPSULATED_DATA UNPACKING - -/** - * @brief Get field seqnr from encapsulated_data message - * - * @return sequence number (starting with 0 on every transmission) - */ -static inline uint16_t mavlink_msg_encapsulated_data_get_seqnr(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field data from encapsulated_data message - * - * @return image data bytes - */ -static inline uint16_t mavlink_msg_encapsulated_data_get_data(const mavlink_message_t* msg, uint8_t* r_data) -{ - - memcpy(r_data, msg->payload+sizeof(uint16_t), sizeof(uint8_t)*253); - return sizeof(uint8_t)*253; -} - -/** - * @brief Decode a encapsulated_data message into a struct - * - * @param msg The message to decode - * @param encapsulated_data C-struct to decode the message contents into - */ -static inline void mavlink_msg_encapsulated_data_decode(const mavlink_message_t* msg, mavlink_encapsulated_data_t* encapsulated_data) -{ - encapsulated_data->seqnr = mavlink_msg_encapsulated_data_get_seqnr(msg); - mavlink_msg_encapsulated_data_get_data(msg, encapsulated_data->data); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_encapsulated_image.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_encapsulated_image.h deleted file mode 100644 index 1878c847e2..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_encapsulated_image.h +++ /dev/null @@ -1,76 +0,0 @@ -// MESSAGE ENCAPSULATED_IMAGE PACKING - -#define MAVLINK_MSG_ID_ENCAPSULATED_IMAGE 171 - -typedef struct __mavlink_encapsulated_image_t -{ - uint8_t seqnr; ///< sequence number (starting with 0 on every transmission) - uint8_t data[254]; ///< image data bytes - -} mavlink_encapsulated_image_t; - -#define MAVLINK_MSG_ENCAPSULATED_IMAGE_FIELD_DATA_LEN 254 - - -/** - * @brief Send a encapsulated_image message - * - * @param seqnr sequence number (starting with 0 on every transmission) - * @param data image data bytes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_encapsulated_image_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t seqnr, const uint8_t* data) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_IMAGE; - - i += put_uint8_t_by_index(seqnr, i, msg->payload); //sequence number (starting with 0 on every transmission) - i += put_array_by_index((int8_t*)data, sizeof(uint8_t)*254, i, msg->payload); //image data bytes - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -static inline uint16_t mavlink_msg_encapsulated_image_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_encapsulated_image_t* encapsulated_image) -{ - return mavlink_msg_encapsulated_image_pack(system_id, component_id, msg, encapsulated_image->seqnr, encapsulated_image->data); -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_encapsulated_image_send(mavlink_channel_t chan, uint8_t seqnr, const uint8_t* data) -{ - mavlink_message_t msg; - mavlink_msg_encapsulated_image_pack(mavlink_system.sysid, mavlink_system.compid, &msg, seqnr, data); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE ENCAPSULATED_IMAGE UNPACKING - -/** - * @brief Get field seqnr from encapsulated_image message - * - * @return sequence number (starting with 0 on every transmission) - */ -static inline uint8_t mavlink_msg_encapsulated_image_get_seqnr(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field data from encapsulated_image message - * - * @return image data bytes - */ -static inline uint16_t mavlink_msg_encapsulated_image_get_data(const mavlink_message_t* msg, uint8_t* r_data) -{ - - memcpy(r_data, msg->payload+sizeof(uint8_t), sizeof(uint8_t)*254); - return sizeof(uint8_t)*254; -} - -static inline void mavlink_msg_encapsulated_image_decode(const mavlink_message_t* msg, mavlink_encapsulated_image_t* encapsulated_image) -{ - encapsulated_image->seqnr = mavlink_msg_encapsulated_image_get_seqnr(msg); - mavlink_msg_encapsulated_image_get_data(msg, encapsulated_image->data); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_get_image_ack.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_get_image_ack.h deleted file mode 100644 index df9229b156..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_get_image_ack.h +++ /dev/null @@ -1,104 +0,0 @@ -// MESSAGE GET_IMAGE_ACK PACKING - -#define MAVLINK_MSG_ID_GET_IMAGE_ACK 170 - -typedef struct __mavlink_get_image_ack_t -{ - uint16_t size; ///< image size in bytes (65000 byte max) - uint8_t packets; ///< number of packets beeing sent - uint8_t payload; ///< image payload size (normally 254 byte) - uint8_t quality; ///< JPEG quality out of [0,100] - -} mavlink_get_image_ack_t; - - - -/** - * @brief Send a get_image_ack message - * - * @param size image size in bytes (65000 byte max) - * @param packets number of packets beeing sent - * @param payload image payload size (normally 254 byte) - * @param quality JPEG quality out of [0,100] - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_get_image_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t size, uint8_t packets, uint8_t payload, uint8_t quality) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_GET_IMAGE_ACK; - - i += put_uint16_t_by_index(size, i, msg->payload); //image size in bytes (65000 byte max) - i += put_uint8_t_by_index(packets, i, msg->payload); //number of packets beeing sent - i += put_uint8_t_by_index(payload, i, msg->payload); //image payload size (normally 254 byte) - i += put_uint8_t_by_index(quality, i, msg->payload); //JPEG quality out of [0,100] - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -static inline uint16_t mavlink_msg_get_image_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_get_image_ack_t* get_image_ack) -{ - return mavlink_msg_get_image_ack_pack(system_id, component_id, msg, get_image_ack->size, get_image_ack->packets, get_image_ack->payload, get_image_ack->quality); -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_get_image_ack_send(mavlink_channel_t chan, uint16_t size, uint8_t packets, uint8_t payload, uint8_t quality) -{ - mavlink_message_t msg; - mavlink_msg_get_image_ack_pack(mavlink_system.sysid, mavlink_system.compid, &msg, size, packets, payload, quality); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE GET_IMAGE_ACK UNPACKING - -/** - * @brief Get field size from get_image_ack message - * - * @return image size in bytes (65000 byte max) - */ -static inline uint16_t mavlink_msg_get_image_ack_get_size(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field packets from get_image_ack message - * - * @return number of packets beeing sent - */ -static inline uint8_t mavlink_msg_get_image_ack_get_packets(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint16_t))[0]; -} - -/** - * @brief Get field payload from get_image_ack message - * - * @return image payload size (normally 254 byte) - */ -static inline uint8_t mavlink_msg_get_image_ack_get_payload(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint16_t)+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field quality from get_image_ack message - * - * @return JPEG quality out of [0,100] - */ -static inline uint8_t mavlink_msg_get_image_ack_get_quality(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -static inline void mavlink_msg_get_image_ack_decode(const mavlink_message_t* msg, mavlink_get_image_ack_t* get_image_ack) -{ - get_image_ack->size = mavlink_msg_get_image_ack_get_size(msg); - get_image_ack->packets = mavlink_msg_get_image_ack_get_packets(msg); - get_image_ack->payload = mavlink_msg_get_image_ack_get_payload(msg); - get_image_ack->quality = mavlink_msg_get_image_ack_get_quality(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_image_available.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_image_available.h deleted file mode 100644 index b40e23df7b..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_image_available.h +++ /dev/null @@ -1,586 +0,0 @@ -// MESSAGE IMAGE_AVAILABLE PACKING - -#define MAVLINK_MSG_ID_IMAGE_AVAILABLE 103 - -typedef struct __mavlink_image_available_t -{ - uint64_t cam_id; ///< Camera id - uint8_t cam_no; ///< Camera # (starts with 0) - 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 - uint16_t width; ///< Image width - uint16_t height; ///< Image height - uint16_t depth; ///< Image depth - uint8_t channels; ///< Image channels - 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 - -} mavlink_image_available_t; - - - -/** - * @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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; - - i += put_uint64_t_by_index(cam_id, i, msg->payload); // Camera id - i += put_uint8_t_by_index(cam_no, i, msg->payload); // Camera # (starts with 0) - i += put_uint64_t_by_index(timestamp, i, msg->payload); // Timestamp - i += put_uint64_t_by_index(valid_until, i, msg->payload); // Until which timestamp this buffer will stay valid - i += put_uint32_t_by_index(img_seq, i, msg->payload); // The image sequence number - i += put_uint32_t_by_index(img_buf_index, i, msg->payload); // Position of the image in the buffer, starts with 0 - i += put_uint16_t_by_index(width, i, msg->payload); // Image width - i += put_uint16_t_by_index(height, i, msg->payload); // Image height - i += put_uint16_t_by_index(depth, i, msg->payload); // Image depth - i += put_uint8_t_by_index(channels, i, msg->payload); // Image channels - i += put_uint32_t_by_index(key, i, msg->payload); // Shared memory area key - i += put_uint32_t_by_index(exposure, i, msg->payload); // Exposure time, in microseconds - i += put_float_by_index(gain, i, msg->payload); // Camera gain - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad - i += put_float_by_index(local_z, i, msg->payload); // Local frame Z coordinate (height over ground) - i += put_float_by_index(lat, i, msg->payload); // GPS X coordinate - i += put_float_by_index(lon, i, msg->payload); // GPS Y coordinate - i += put_float_by_index(alt, i, msg->payload); // Global frame altitude - i += put_float_by_index(ground_x, i, msg->payload); // Ground truth X - i += put_float_by_index(ground_y, i, msg->payload); // Ground truth Y - i += put_float_by_index(ground_z, i, msg->payload); // Ground truth Z - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @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 chan The MAVLink channel this message was 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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; - - i += put_uint64_t_by_index(cam_id, i, msg->payload); // Camera id - i += put_uint8_t_by_index(cam_no, i, msg->payload); // Camera # (starts with 0) - i += put_uint64_t_by_index(timestamp, i, msg->payload); // Timestamp - i += put_uint64_t_by_index(valid_until, i, msg->payload); // Until which timestamp this buffer will stay valid - i += put_uint32_t_by_index(img_seq, i, msg->payload); // The image sequence number - i += put_uint32_t_by_index(img_buf_index, i, msg->payload); // Position of the image in the buffer, starts with 0 - i += put_uint16_t_by_index(width, i, msg->payload); // Image width - i += put_uint16_t_by_index(height, i, msg->payload); // Image height - i += put_uint16_t_by_index(depth, i, msg->payload); // Image depth - i += put_uint8_t_by_index(channels, i, msg->payload); // Image channels - i += put_uint32_t_by_index(key, i, msg->payload); // Shared memory area key - i += put_uint32_t_by_index(exposure, i, msg->payload); // Exposure time, in microseconds - i += put_float_by_index(gain, i, msg->payload); // Camera gain - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad - i += put_float_by_index(local_z, i, msg->payload); // Local frame Z coordinate (height over ground) - i += put_float_by_index(lat, i, msg->payload); // GPS X coordinate - i += put_float_by_index(lon, i, msg->payload); // GPS Y coordinate - i += put_float_by_index(alt, i, msg->payload); // Global frame altitude - i += put_float_by_index(ground_x, i, msg->payload); // Ground truth X - i += put_float_by_index(ground_y, i, msg->payload); // Ground truth Y - i += put_float_by_index(ground_z, i, msg->payload); // Ground truth Z - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a image_available struct into 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 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 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) -{ - mavlink_message_t msg; - mavlink_msg_image_available_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, cam_id, cam_no, timestamp, valid_until, img_seq, img_buf_index, width, height, depth, channels, key, exposure, gain, roll, pitch, yaw, local_z, lat, lon, alt, ground_x, ground_y, ground_z); - mavlink_send_uart(chan, &msg); -} - -#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) -{ - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; -} - -/** - * @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 (uint8_t)(msg->payload+sizeof(uint64_t))[0]; -} - -/** - * @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) -{ - generic_64bit r; - r.b[7] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[0]; - r.b[6] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[1]; - r.b[5] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[2]; - r.b[4] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[3]; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[4]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[5]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[6]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[7]; - return (uint64_t)r.ll; -} - -/** - * @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) -{ - generic_64bit r; - r.b[7] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[0]; - r.b[6] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[1]; - r.b[5] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[2]; - r.b[4] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[3]; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[4]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[5]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[6]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[7]; - return (uint64_t)r.ll; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t))[3]; - return (uint32_t)r.i; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t))[3]; - return (uint32_t)r.i; -} - -/** - * @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) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t))[1]; - return (uint16_t)r.s; -} - -/** - * @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) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @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) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @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 (uint8_t)(msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[3]; - return (uint32_t)r.i; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t))[3]; - return (uint32_t)r.i; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - image_available->cam_id = mavlink_msg_image_available_get_cam_id(msg); - image_available->cam_no = mavlink_msg_image_available_get_cam_no(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->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->channels = mavlink_msg_image_available_get_channels(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); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_image_trigger_control.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_image_trigger_control.h deleted file mode 100644 index 90aa9dcf4e..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_image_trigger_control.h +++ /dev/null @@ -1,101 +0,0 @@ -// MESSAGE IMAGE_TRIGGER_CONTROL PACKING - -#define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL 102 - -typedef struct __mavlink_image_trigger_control_t -{ - uint8_t enable; ///< 0 to disable, 1 to enable - -} mavlink_image_trigger_control_t; - - - -/** - * @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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; - - i += put_uint8_t_by_index(enable, i, msg->payload); // 0 to disable, 1 to enable - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @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 chan The MAVLink channel this message was 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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; - - i += put_uint8_t_by_index(enable, i, msg->payload); // 0 to disable, 1 to enable - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a image_trigger_control struct into 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 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 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) -{ - mavlink_message_t msg; - mavlink_msg_image_trigger_control_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, enable); - mavlink_send_uart(chan, &msg); -} - -#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 (uint8_t)(msg->payload)[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) -{ - image_trigger_control->enable = mavlink_msg_image_trigger_control_get_enable(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_image_triggered.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_image_triggered.h deleted file mode 100644 index 000003f3d7..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_image_triggered.h +++ /dev/null @@ -1,352 +0,0 @@ -// MESSAGE IMAGE_TRIGGERED PACKING - -#define MAVLINK_MSG_ID_IMAGE_TRIGGERED 101 - -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; - - - -/** - * @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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; - - i += put_uint64_t_by_index(timestamp, i, msg->payload); // Timestamp - i += put_uint32_t_by_index(seq, i, msg->payload); // IMU seq - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad - i += put_float_by_index(local_z, i, msg->payload); // Local frame Z coordinate (height over ground) - i += put_float_by_index(lat, i, msg->payload); // GPS X coordinate - i += put_float_by_index(lon, i, msg->payload); // GPS Y coordinate - i += put_float_by_index(alt, i, msg->payload); // Global frame altitude - i += put_float_by_index(ground_x, i, msg->payload); // Ground truth X - i += put_float_by_index(ground_y, i, msg->payload); // Ground truth Y - i += put_float_by_index(ground_z, i, msg->payload); // Ground truth Z - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @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 chan The MAVLink channel this message was 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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; - - i += put_uint64_t_by_index(timestamp, i, msg->payload); // Timestamp - i += put_uint32_t_by_index(seq, i, msg->payload); // IMU seq - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad - i += put_float_by_index(local_z, i, msg->payload); // Local frame Z coordinate (height over ground) - i += put_float_by_index(lat, i, msg->payload); // GPS X coordinate - i += put_float_by_index(lon, i, msg->payload); // GPS Y coordinate - i += put_float_by_index(alt, i, msg->payload); // Global frame altitude - i += put_float_by_index(ground_x, i, msg->payload); // Ground truth X - i += put_float_by_index(ground_y, i, msg->payload); // Ground truth Y - i += put_float_by_index(ground_z, i, msg->payload); // Ground truth Z - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a image_triggered struct into 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 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 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) -{ - mavlink_message_t msg; - mavlink_msg_image_triggered_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, timestamp, seq, roll, pitch, yaw, local_z, lat, lon, alt, ground_x, ground_y, ground_z); - mavlink_send_uart(chan, &msg); -} - -#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) -{ - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (uint32_t)r.i; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - 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); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_manual_control.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_manual_control.h deleted file mode 100644 index b8fb08f6ee..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_manual_control.h +++ /dev/null @@ -1,191 +0,0 @@ -// MESSAGE MANUAL_CONTROL PACKING - -#define MAVLINK_MSG_ID_MANUAL_CONTROL 84 - -typedef struct __mavlink_manual_control_t -{ - uint8_t target; ///< The system to be controlled - float roll; ///< roll - float pitch; ///< pitch - float yaw; ///< yaw - float thrust; ///< thrust - uint8_t roll_manual; ///< roll control enabled auto:0, manual:1 - uint8_t pitch_manual; ///< pitch auto:0, manual:1 - uint8_t yaw_manual; ///< yaw auto:0, manual:1 - uint8_t thrust_manual; ///< thrust auto:0, manual:1 - -} mavlink_manual_control_t; - - - -/** - * @brief Send a manual_control 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 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_t component_id, 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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; - - i += put_uint8_t_by_index(target, i, msg->payload); //The system to be controlled - i += put_float_by_index(roll, i, msg->payload); //roll - i += put_float_by_index(pitch, i, msg->payload); //pitch - i += put_float_by_index(yaw, i, msg->payload); //yaw - i += put_float_by_index(thrust, i, msg->payload); //thrust - i += put_uint8_t_by_index(roll_manual, i, msg->payload); //roll control enabled auto:0, manual:1 - i += put_uint8_t_by_index(pitch_manual, i, msg->payload); //pitch auto:0, manual:1 - i += put_uint8_t_by_index(yaw_manual, i, msg->payload); //yaw auto:0, manual:1 - i += put_uint8_t_by_index(thrust_manual, i, msg->payload); //thrust auto:0, manual:1 - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_manual_control_t* manual_control) -{ - return mavlink_msg_manual_control_pack(system_id, component_id, msg, manual_control->target, manual_control->roll, manual_control->pitch, manual_control->yaw, manual_control->thrust, manual_control->roll_manual, manual_control->pitch_manual, manual_control->yaw_manual, manual_control->thrust_manual); -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_manual_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) -{ - mavlink_message_t msg; - mavlink_msg_manual_control_pack(mavlink_system.sysid, mavlink_system.compid, &msg, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE MANUAL_CONTROL UNPACKING - -/** - * @brief Get field target from manual_control message - * - * @return The system to be controlled - */ -static inline uint8_t mavlink_msg_manual_control_get_target(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field roll from manual_control message - * - * @return roll - */ -static inline float mavlink_msg_manual_control_get_roll(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (float)r.f; -} - -/** - * @brief Get field pitch from manual_control message - * - * @return pitch - */ -static inline float mavlink_msg_manual_control_get_pitch(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field yaw from manual_control message - * - * @return yaw - */ -static inline float mavlink_msg_manual_control_get_yaw(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field thrust from manual_control message - * - * @return thrust - */ -static inline float mavlink_msg_manual_control_get_thrust(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field roll_manual from manual_control message - * - * @return roll control enabled auto:0, manual:1 - */ -static inline uint8_t mavlink_msg_manual_control_get_roll_manual(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; -} - -/** - * @brief Get field pitch_manual from manual_control message - * - * @return pitch auto:0, manual:1 - */ -static inline uint8_t mavlink_msg_manual_control_get_pitch_manual(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field yaw_manual from manual_control message - * - * @return yaw auto:0, manual:1 - */ -static inline uint8_t mavlink_msg_manual_control_get_yaw_manual(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field thrust_manual from manual_control message - * - * @return thrust auto:0, manual:1 - */ -static inline uint8_t mavlink_msg_manual_control_get_thrust_manual(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -static inline void mavlink_msg_manual_control_decode(const mavlink_message_t* msg, mavlink_manual_control_t* manual_control) -{ - manual_control->target = mavlink_msg_manual_control_get_target(msg); - manual_control->roll = mavlink_msg_manual_control_get_roll(msg); - manual_control->pitch = mavlink_msg_manual_control_get_pitch(msg); - manual_control->yaw = mavlink_msg_manual_control_get_yaw(msg); - manual_control->thrust = mavlink_msg_manual_control_get_thrust(msg); - manual_control->roll_manual = mavlink_msg_manual_control_get_roll_manual(msg); - manual_control->pitch_manual = mavlink_msg_manual_control_get_pitch_manual(msg); - manual_control->yaw_manual = mavlink_msg_manual_control_get_yaw_manual(msg); - manual_control->thrust_manual = mavlink_msg_manual_control_get_thrust_manual(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_marker.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_marker.h deleted file mode 100644 index fb275534ae..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_marker.h +++ /dev/null @@ -1,236 +0,0 @@ -// MESSAGE MARKER PACKING - -#define MAVLINK_MSG_ID_MARKER 130 - -typedef struct __mavlink_marker_t -{ - uint16_t id; ///< ID - float x; ///< x position - float y; ///< y position - float z; ///< z position - float roll; ///< roll orientation - float pitch; ///< pitch orientation - float yaw; ///< yaw orientation - -} mavlink_marker_t; - - - -/** - * @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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_MARKER; - - i += put_uint16_t_by_index(id, i, msg->payload); // ID - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(roll, i, msg->payload); // roll orientation - i += put_float_by_index(pitch, i, msg->payload); // pitch orientation - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @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 chan The MAVLink channel this message was 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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_MARKER; - - i += put_uint16_t_by_index(id, i, msg->payload); // ID - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(roll, i, msg->payload); // roll orientation - i += put_float_by_index(pitch, i, msg->payload); // pitch orientation - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a marker struct into 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 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 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) -{ - mavlink_message_t msg; - mavlink_msg_marker_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, id, x, y, z, roll, pitch, yaw); - mavlink_send_uart(chan, &msg); -} - -#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) -{ - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field x from marker message - * - * @return x position - */ -static inline float mavlink_msg_marker_get_x(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t))[3]; - return (float)r.f; -} - -/** - * @brief Get field y from marker message - * - * @return y position - */ -static inline float mavlink_msg_marker_get_y(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field z from marker message - * - * @return z position - */ -static inline float mavlink_msg_marker_get_z(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field roll from marker message - * - * @return roll orientation - */ -static inline float mavlink_msg_marker_get_roll(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field pitch from marker message - * - * @return pitch orientation - */ -static inline float mavlink_msg_marker_get_pitch(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field yaw from marker message - * - * @return yaw orientation - */ -static inline float mavlink_msg_marker_get_yaw(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - marker->id = mavlink_msg_marker_get_id(msg); - 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); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_pattern_detected.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_pattern_detected.h deleted file mode 100644 index 1a2b46596b..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_pattern_detected.h +++ /dev/null @@ -1,160 +0,0 @@ -// MESSAGE PATTERN_DETECTED PACKING - -#define MAVLINK_MSG_ID_PATTERN_DETECTED 160 - -typedef struct __mavlink_pattern_detected_t -{ - uint8_t type; ///< 0: Pattern, 1: Letter - float confidence; ///< Confidence of detection - int8_t file[100]; ///< Pattern file name - uint8_t detected; ///< Accepted as true detection, 0 no, 1 yes - -} mavlink_pattern_detected_t; - -#define MAVLINK_MSG_PATTERN_DETECTED_FIELD_FILE_LEN 100 - - -/** - * @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 int8_t* file, uint8_t detected) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; - - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Pattern, 1: Letter - i += put_float_by_index(confidence, i, msg->payload); // Confidence of detection - i += put_array_by_index(file, 100, i, msg->payload); // Pattern file name - i += put_uint8_t_by_index(detected, i, msg->payload); // Accepted as true detection, 0 no, 1 yes - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @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 chan The MAVLink channel this message was 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 int8_t* file, uint8_t detected) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; - - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Pattern, 1: Letter - i += put_float_by_index(confidence, i, msg->payload); // Confidence of detection - i += put_array_by_index(file, 100, i, msg->payload); // Pattern file name - i += put_uint8_t_by_index(detected, i, msg->payload); // Accepted as true detection, 0 no, 1 yes - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a pattern_detected struct into 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 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 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 int8_t* file, uint8_t detected) -{ - mavlink_message_t msg; - mavlink_msg_pattern_detected_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, confidence, file, detected); - mavlink_send_uart(chan, &msg); -} - -#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 (uint8_t)(msg->payload)[0]; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (float)r.f; -} - -/** - * @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, int8_t* r_data) -{ - - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(float), 100); - return 100; -} - -/** - * @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 (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+100)[0]; -} - -/** - * @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) -{ - pattern_detected->type = mavlink_msg_pattern_detected_get_type(msg); - pattern_detected->confidence = mavlink_msg_pattern_detected_get_confidence(msg); - mavlink_msg_pattern_detected_get_file(msg, pattern_detected->file); - pattern_detected->detected = mavlink_msg_pattern_detected_get_detected(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_point_of_interest.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_point_of_interest.h deleted file mode 100644 index 7e48c0fd91..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_point_of_interest.h +++ /dev/null @@ -1,241 +0,0 @@ -// MESSAGE POINT_OF_INTEREST PACKING - -#define MAVLINK_MSG_ID_POINT_OF_INTEREST 161 - -typedef struct __mavlink_point_of_interest_t -{ - 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 - uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds - float x; ///< X Position - float y; ///< Y Position - float z; ///< Z Position - int8_t name[25]; ///< POI name - -} mavlink_point_of_interest_t; - -#define MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN 25 - - -/** - * @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 int8_t* name) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; - - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - i += put_uint8_t_by_index(color, i, msg->payload); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - i += put_uint8_t_by_index(coordinate_system, i, msg->payload); // 0: global, 1:local - i += put_uint16_t_by_index(timeout, i, msg->payload); // 0: no timeout, >1: timeout in seconds - i += put_float_by_index(x, i, msg->payload); // X Position - i += put_float_by_index(y, i, msg->payload); // Y Position - i += put_float_by_index(z, i, msg->payload); // Z Position - i += put_array_by_index(name, 25, i, msg->payload); // POI name - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @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 chan The MAVLink channel this message was 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 int8_t* name) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; - - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - i += put_uint8_t_by_index(color, i, msg->payload); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - i += put_uint8_t_by_index(coordinate_system, i, msg->payload); // 0: global, 1:local - i += put_uint16_t_by_index(timeout, i, msg->payload); // 0: no timeout, >1: timeout in seconds - i += put_float_by_index(x, i, msg->payload); // X Position - i += put_float_by_index(y, i, msg->payload); // Y Position - i += put_float_by_index(z, i, msg->payload); // Z Position - i += put_array_by_index(name, 25, i, msg->payload); // POI name - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a point_of_interest struct into 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 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 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 int8_t* name) -{ - mavlink_message_t msg; - mavlink_msg_point_of_interest_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, color, coordinate_system, timeout, x, y, z, name); - mavlink_send_uart(chan, &msg); -} - -#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 (uint8_t)(msg->payload)[0]; -} - -/** - * @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 (uint8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @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 (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -/** - * @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) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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, int8_t* r_data) -{ - - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float), 25); - return 25; -} - -/** - * @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) -{ - 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); - point_of_interest->timeout = mavlink_msg_point_of_interest_get_timeout(msg); - 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); - mavlink_msg_point_of_interest_get_name(msg, point_of_interest->name); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_point_of_interest_connection.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_point_of_interest_connection.h deleted file mode 100644 index fb30517463..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_point_of_interest_connection.h +++ /dev/null @@ -1,307 +0,0 @@ -// MESSAGE POINT_OF_INTEREST_CONNECTION PACKING - -#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION 162 - -typedef struct __mavlink_point_of_interest_connection_t -{ - 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 - uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds - float xp1; ///< X1 Position - float yp1; ///< Y1 Position - float zp1; ///< Z1 Position - float xp2; ///< X2 Position - float yp2; ///< Y2 Position - float zp2; ///< Z2 Position - int8_t name[25]; ///< POI connection name - -} mavlink_point_of_interest_connection_t; - -#define MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN 25 - - -/** - * @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 int8_t* name) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; - - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - i += put_uint8_t_by_index(color, i, msg->payload); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - i += put_uint8_t_by_index(coordinate_system, i, msg->payload); // 0: global, 1:local - i += put_uint16_t_by_index(timeout, i, msg->payload); // 0: no timeout, >1: timeout in seconds - i += put_float_by_index(xp1, i, msg->payload); // X1 Position - i += put_float_by_index(yp1, i, msg->payload); // Y1 Position - i += put_float_by_index(zp1, i, msg->payload); // Z1 Position - i += put_float_by_index(xp2, i, msg->payload); // X2 Position - i += put_float_by_index(yp2, i, msg->payload); // Y2 Position - i += put_float_by_index(zp2, i, msg->payload); // Z2 Position - i += put_array_by_index(name, 25, i, msg->payload); // POI connection name - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @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 chan The MAVLink channel this message was 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 int8_t* name) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; - - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - i += put_uint8_t_by_index(color, i, msg->payload); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - i += put_uint8_t_by_index(coordinate_system, i, msg->payload); // 0: global, 1:local - i += put_uint16_t_by_index(timeout, i, msg->payload); // 0: no timeout, >1: timeout in seconds - i += put_float_by_index(xp1, i, msg->payload); // X1 Position - i += put_float_by_index(yp1, i, msg->payload); // Y1 Position - i += put_float_by_index(zp1, i, msg->payload); // Z1 Position - i += put_float_by_index(xp2, i, msg->payload); // X2 Position - i += put_float_by_index(yp2, i, msg->payload); // Y2 Position - i += put_float_by_index(zp2, i, msg->payload); // Z2 Position - i += put_array_by_index(name, 25, i, msg->payload); // POI connection name - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a point_of_interest_connection struct into 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 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 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 int8_t* name) -{ - mavlink_message_t msg; - mavlink_msg_point_of_interest_connection_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, color, coordinate_system, timeout, xp1, yp1, zp1, xp2, yp2, zp2, name); - mavlink_send_uart(chan, &msg); -} - -#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 (uint8_t)(msg->payload)[0]; -} - -/** - * @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 (uint8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @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 (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -/** - * @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) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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, int8_t* r_data) -{ - - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float), 25); - return 25; -} - -/** - * @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) -{ - 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); - point_of_interest_connection->timeout = mavlink_msg_point_of_interest_connection_get_timeout(msg); - 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); - mavlink_msg_point_of_interest_connection_get_name(msg, point_of_interest_connection->name); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_position_control_offset_set.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_position_control_offset_set.h deleted file mode 100644 index 30f076bba8..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_position_control_offset_set.h +++ /dev/null @@ -1,206 +0,0 @@ -// MESSAGE POSITION_CONTROL_OFFSET_SET PACKING - -#define MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET 154 - -typedef struct __mavlink_position_control_offset_set_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - float x; ///< x position offset - float y; ///< y position offset - float z; ///< z position offset - float yaw; ///< yaw orientation offset in radians, 0 = NORTH - -} mavlink_position_control_offset_set_t; - - - -/** - * @brief Pack a position_control_offset_set message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param 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_position_control_offset_set_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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_float_by_index(x, i, msg->payload); // x position offset - i += put_float_by_index(y, i, msg->payload); // y position offset - i += put_float_by_index(z, i, msg->payload); // z position offset - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation offset in radians, 0 = NORTH - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a position_control_offset_set message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was 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_position_control_offset_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, float x, float y, float z, float yaw) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_float_by_index(x, i, msg->payload); // x position offset - i += put_float_by_index(y, i, msg->payload); // y position offset - i += put_float_by_index(z, i, msg->payload); // z position offset - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation offset in radians, 0 = NORTH - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a position_control_offset_set struct into 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 position_control_offset_set C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_position_control_offset_set_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_position_control_offset_set_t* position_control_offset_set) -{ - return mavlink_msg_position_control_offset_set_pack(system_id, component_id, msg, position_control_offset_set->target_system, position_control_offset_set->target_component, position_control_offset_set->x, position_control_offset_set->y, position_control_offset_set->z, position_control_offset_set->yaw); -} - -/** - * @brief Send a position_control_offset_set 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_position_control_offset_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) -{ - mavlink_message_t msg; - mavlink_msg_position_control_offset_set_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, x, y, z, yaw); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE POSITION_CONTROL_OFFSET_SET UNPACKING - -/** - * @brief Get field target_system from position_control_offset_set message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_position_control_offset_set_get_target_system(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field target_component from position_control_offset_set message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_position_control_offset_set_get_target_component(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field x from position_control_offset_set message - * - * @return x position offset - */ -static inline float mavlink_msg_position_control_offset_set_get_x(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (float)r.f; -} - -/** - * @brief Get field y from position_control_offset_set message - * - * @return y position offset - */ -static inline float mavlink_msg_position_control_offset_set_get_y(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field z from position_control_offset_set message - * - * @return z position offset - */ -static inline float mavlink_msg_position_control_offset_set_get_z(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field yaw from position_control_offset_set message - * - * @return yaw orientation offset in radians, 0 = NORTH - */ -static inline float mavlink_msg_position_control_offset_set_get_yaw(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Decode a position_control_offset_set message into a struct - * - * @param msg The message to decode - * @param position_control_offset_set C-struct to decode the message contents into - */ -static inline void mavlink_msg_position_control_offset_set_decode(const mavlink_message_t* msg, mavlink_position_control_offset_set_t* position_control_offset_set) -{ - position_control_offset_set->target_system = mavlink_msg_position_control_offset_set_get_target_system(msg); - position_control_offset_set->target_component = mavlink_msg_position_control_offset_set_get_target_component(msg); - position_control_offset_set->x = mavlink_msg_position_control_offset_set_get_x(msg); - position_control_offset_set->y = mavlink_msg_position_control_offset_set_get_y(msg); - position_control_offset_set->z = mavlink_msg_position_control_offset_set_get_z(msg); - position_control_offset_set->yaw = mavlink_msg_position_control_offset_set_get_yaw(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_position_control_setpoint.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_position_control_setpoint.h deleted file mode 100644 index 59bcb131ce..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_position_control_setpoint.h +++ /dev/null @@ -1,192 +0,0 @@ -// MESSAGE POSITION_CONTROL_SETPOINT PACKING - -#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT 121 - -typedef struct __mavlink_position_control_setpoint_t -{ - uint16_t id; ///< ID of waypoint, 0 for plain position - float x; ///< x position - float y; ///< y position - float z; ///< z position - float yaw; ///< yaw orientation in radians, 0 = NORTH - -} mavlink_position_control_setpoint_t; - - - -/** - * @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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; - - i += put_uint16_t_by_index(id, i, msg->payload); // ID of waypoint, 0 for plain position - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation in radians, 0 = NORTH - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @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 chan The MAVLink channel this message was 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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; - - i += put_uint16_t_by_index(id, i, msg->payload); // ID of waypoint, 0 for plain position - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation in radians, 0 = NORTH - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a position_control_setpoint struct into 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 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 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) -{ - mavlink_message_t msg; - mavlink_msg_position_control_setpoint_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, id, x, y, z, yaw); - mavlink_send_uart(chan, &msg); -} - -#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) -{ - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @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) -{ - position_control_setpoint->id = mavlink_msg_position_control_setpoint_get_id(msg); - 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); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h deleted file mode 100644 index 75150e66b9..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h +++ /dev/null @@ -1,226 +0,0 @@ -// MESSAGE POSITION_CONTROL_SETPOINT_SET PACKING - -#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET 120 - -typedef struct __mavlink_position_control_setpoint_set_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint16_t id; ///< ID of waypoint, 0 for plain position - float x; ///< x position - float y; ///< y position - float z; ///< z position - float yaw; ///< yaw orientation in radians, 0 = NORTH - -} mavlink_position_control_setpoint_set_t; - - - -/** - * @brief Pack a position_control_setpoint_set message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param 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_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t id, float x, float y, float z, float yaw) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(id, i, msg->payload); // ID of waypoint, 0 for plain position - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation in radians, 0 = NORTH - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a position_control_setpoint_set message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @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_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, uint16_t id, float x, float y, float z, float yaw) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(id, i, msg->payload); // ID of waypoint, 0 for plain position - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation in radians, 0 = NORTH - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a position_control_setpoint_set struct into 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 position_control_setpoint_set C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_position_control_setpoint_set_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_position_control_setpoint_set_t* position_control_setpoint_set) -{ - return mavlink_msg_position_control_setpoint_set_pack(system_id, component_id, msg, position_control_setpoint_set->target_system, position_control_setpoint_set->target_component, position_control_setpoint_set->id, position_control_setpoint_set->x, position_control_setpoint_set->y, position_control_setpoint_set->z, position_control_setpoint_set->yaw); -} - -/** - * @brief Send a position_control_setpoint_set message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @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_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t id, float x, float y, float z, float yaw) -{ - mavlink_message_t msg; - mavlink_msg_position_control_setpoint_set_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, id, x, y, z, yaw); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE POSITION_CONTROL_SETPOINT_SET UNPACKING - -/** - * @brief Get field target_system from position_control_setpoint_set message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_position_control_setpoint_set_get_target_system(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field target_component from position_control_setpoint_set message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_position_control_setpoint_set_get_target_component(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field id from position_control_setpoint_set message - * - * @return ID of waypoint, 0 for plain position - */ -static inline uint16_t mavlink_msg_position_control_setpoint_set_get_id(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field x from position_control_setpoint_set message - * - * @return x position - */ -static inline float mavlink_msg_position_control_setpoint_set_get_x(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[3]; - return (float)r.f; -} - -/** - * @brief Get field y from position_control_setpoint_set message - * - * @return y position - */ -static inline float mavlink_msg_position_control_setpoint_set_get_y(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field z from position_control_setpoint_set message - * - * @return z position - */ -static inline float mavlink_msg_position_control_setpoint_set_get_z(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field yaw from position_control_setpoint_set message - * - * @return yaw orientation in radians, 0 = NORTH - */ -static inline float mavlink_msg_position_control_setpoint_set_get_yaw(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Decode a position_control_setpoint_set message into a struct - * - * @param msg The message to decode - * @param position_control_setpoint_set C-struct to decode the message contents into - */ -static inline void mavlink_msg_position_control_setpoint_set_decode(const mavlink_message_t* msg, mavlink_position_control_setpoint_set_t* position_control_setpoint_set) -{ - position_control_setpoint_set->target_system = mavlink_msg_position_control_setpoint_set_get_target_system(msg); - position_control_setpoint_set->target_component = mavlink_msg_position_control_setpoint_set_get_target_component(msg); - position_control_setpoint_set->id = mavlink_msg_position_control_setpoint_set_get_id(msg); - position_control_setpoint_set->x = mavlink_msg_position_control_setpoint_set_get_x(msg); - position_control_setpoint_set->y = mavlink_msg_position_control_setpoint_set_get_y(msg); - position_control_setpoint_set->z = mavlink_msg_position_control_setpoint_set_get_z(msg); - position_control_setpoint_set->yaw = mavlink_msg_position_control_setpoint_set_get_yaw(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_raw_aux.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_raw_aux.h deleted file mode 100644 index 61adee2970..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_raw_aux.h +++ /dev/null @@ -1,226 +0,0 @@ -// MESSAGE RAW_AUX PACKING - -#define MAVLINK_MSG_ID_RAW_AUX 141 - -typedef struct __mavlink_raw_aux_t -{ - 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) - int32_t baro; ///< Barometric pressure (hecto Pascal) - -} mavlink_raw_aux_t; - - - -/** - * @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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_RAW_AUX; - - i += put_uint16_t_by_index(adc1, i, msg->payload); // ADC1 (J405 ADC3, LPC2148 AD0.6) - i += put_uint16_t_by_index(adc2, i, msg->payload); // ADC2 (J405 ADC5, LPC2148 AD0.2) - i += put_uint16_t_by_index(adc3, i, msg->payload); // ADC3 (J405 ADC6, LPC2148 AD0.1) - i += put_uint16_t_by_index(adc4, i, msg->payload); // ADC4 (J405 ADC7, LPC2148 AD1.3) - i += put_uint16_t_by_index(vbat, i, msg->payload); // Battery voltage - i += put_int16_t_by_index(temp, i, msg->payload); // Temperature (degrees celcius) - i += put_int32_t_by_index(baro, i, msg->payload); // Barometric pressure (hecto Pascal) - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @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 chan The MAVLink channel this message was 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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_RAW_AUX; - - i += put_uint16_t_by_index(adc1, i, msg->payload); // ADC1 (J405 ADC3, LPC2148 AD0.6) - i += put_uint16_t_by_index(adc2, i, msg->payload); // ADC2 (J405 ADC5, LPC2148 AD0.2) - i += put_uint16_t_by_index(adc3, i, msg->payload); // ADC3 (J405 ADC6, LPC2148 AD0.1) - i += put_uint16_t_by_index(adc4, i, msg->payload); // ADC4 (J405 ADC7, LPC2148 AD1.3) - i += put_uint16_t_by_index(vbat, i, msg->payload); // Battery voltage - i += put_int16_t_by_index(temp, i, msg->payload); // Temperature (degrees celcius) - i += put_int32_t_by_index(baro, i, msg->payload); // Barometric pressure (hecto Pascal) - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a raw_aux struct into 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 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 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) -{ - mavlink_message_t msg; - mavlink_msg_raw_aux_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, adc1, adc2, adc3, adc4, vbat, temp, baro); - mavlink_send_uart(chan, &msg); -} - -#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) -{ - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; -} - -/** - * @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) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @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) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @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) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @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) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @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) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (int16_t)r.s; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(int16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(int16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(int16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(int16_t))[3]; - return (int32_t)r.i; -} - -/** - * @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) -{ - 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); - raw_aux->baro = mavlink_msg_raw_aux_get_baro(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_request_data_stream.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_request_data_stream.h deleted file mode 100644 index 045dbc70b6..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_request_data_stream.h +++ /dev/null @@ -1,118 +0,0 @@ -// MESSAGE REQUEST_DATA_STREAM PACKING - -#define MAVLINK_MSG_ID_REQUEST_DATA_STREAM 81 - -typedef struct __mavlink_request_data_stream_t -{ - uint8_t target_system; ///< The target requested to send the message stream. - uint8_t target_component; ///< The target requested to send the message stream. - uint8_t req_stream_id; ///< The ID of the requested message type - uint16_t req_message_rate; ///< The requested interval between two messages of this type - uint8_t start_stop; ///< 1 to start sending, 0 to stop sending. - -} mavlink_request_data_stream_t; - - - -/** - * @brief Send a request_data_stream message - * - * @param target_system The target requested to send the message stream. - * @param target_component The target requested to send the message stream. - * @param req_stream_id The ID of the requested message type - * @param req_message_rate The requested interval between two messages of this type - * @param start_stop 1 to start sending, 0 to stop sending. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; - - i += put_uint8_t_by_index(target_system, i, msg->payload); //The target requested to send the message stream. - i += put_uint8_t_by_index(target_component, i, msg->payload); //The target requested to send the message stream. - i += put_uint8_t_by_index(req_stream_id, i, msg->payload); //The ID of the requested message type - i += put_uint16_t_by_index(req_message_rate, i, msg->payload); //The requested interval between two messages of this type - i += put_uint8_t_by_index(start_stop, i, msg->payload); //1 to start sending, 0 to stop sending. - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_request_data_stream_t* request_data_stream) -{ - return mavlink_msg_request_data_stream_pack(system_id, component_id, msg, request_data_stream->target_system, request_data_stream->target_component, request_data_stream->req_stream_id, request_data_stream->req_message_rate, request_data_stream->start_stop); -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_request_data_stream_send(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) -{ - mavlink_message_t msg; - mavlink_msg_request_data_stream_pack(mavlink_system.sysid, mavlink_system.compid, &msg, target_system, target_component, req_stream_id, req_message_rate, start_stop); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE REQUEST_DATA_STREAM UNPACKING - -/** - * @brief Get field target_system from request_data_stream message - * - * @return The target requested to send the message stream. - */ -static inline uint8_t mavlink_msg_request_data_stream_get_target_system(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field target_component from request_data_stream message - * - * @return The target requested to send the message stream. - */ -static inline uint8_t mavlink_msg_request_data_stream_get_target_component(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field req_stream_id from request_data_stream message - * - * @return The ID of the requested message type - */ -static inline uint8_t mavlink_msg_request_data_stream_get_req_stream_id(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field req_message_rate from request_data_stream message - * - * @return The requested interval between two messages of this type - */ -static inline uint16_t mavlink_msg_request_data_stream_get_req_message_rate(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field start_stop from request_data_stream message - * - * @return 1 to start sending, 0 to stop sending. - */ -static inline uint8_t mavlink_msg_request_data_stream_get_start_stop(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; -} - -static inline void mavlink_msg_request_data_stream_decode(const mavlink_message_t* msg, mavlink_request_data_stream_t* request_data_stream) -{ - request_data_stream->target_system = mavlink_msg_request_data_stream_get_target_system(msg); - request_data_stream->target_component = mavlink_msg_request_data_stream_get_target_component(msg); - request_data_stream->req_stream_id = mavlink_msg_request_data_stream_get_req_stream_id(msg); - request_data_stream->req_message_rate = mavlink_msg_request_data_stream_get_req_message_rate(msg); - request_data_stream->start_stop = mavlink_msg_request_data_stream_get_start_stop(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_request_dynamic_gyro_calibration.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_request_dynamic_gyro_calibration.h deleted file mode 100644 index 8a8094b987..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_request_dynamic_gyro_calibration.h +++ /dev/null @@ -1,123 +0,0 @@ -// MESSAGE REQUEST_DYNAMIC_GYRO_CALIBRATION PACKING - -#define MAVLINK_MSG_ID_REQUEST_DYNAMIC_GYRO_CALIBRATION 82 - -typedef struct __mavlink_request_dynamic_gyro_calibration_t -{ - uint8_t target_system; ///< The system which should auto-calibrate - uint8_t target_component; ///< The system component which should auto-calibrate - float mode; ///< The current ground-truth rpm - uint8_t axis; ///< The axis to calibrate: 0 roll, 1 pitch, 2 yaw - uint16_t time; ///< The time to average over in ms - -} mavlink_request_dynamic_gyro_calibration_t; - - - -/** - * @brief Send a request_dynamic_gyro_calibration message - * - * @param target_system The system which should auto-calibrate - * @param target_component The system component which should auto-calibrate - * @param mode The current ground-truth rpm - * @param axis The axis to calibrate: 0 roll, 1 pitch, 2 yaw - * @param time The time to average over in ms - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_request_dynamic_gyro_calibration_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float mode, uint8_t axis, uint16_t time) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_REQUEST_DYNAMIC_GYRO_CALIBRATION; - - i += put_uint8_t_by_index(target_system, i, msg->payload); //The system which should auto-calibrate - i += put_uint8_t_by_index(target_component, i, msg->payload); //The system component which should auto-calibrate - i += put_float_by_index(mode, i, msg->payload); //The current ground-truth rpm - i += put_uint8_t_by_index(axis, i, msg->payload); //The axis to calibrate: 0 roll, 1 pitch, 2 yaw - i += put_uint16_t_by_index(time, i, msg->payload); //The time to average over in ms - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -static inline uint16_t mavlink_msg_request_dynamic_gyro_calibration_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_request_dynamic_gyro_calibration_t* request_dynamic_gyro_calibration) -{ - return mavlink_msg_request_dynamic_gyro_calibration_pack(system_id, component_id, msg, request_dynamic_gyro_calibration->target_system, request_dynamic_gyro_calibration->target_component, request_dynamic_gyro_calibration->mode, request_dynamic_gyro_calibration->axis, request_dynamic_gyro_calibration->time); -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_request_dynamic_gyro_calibration_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float mode, uint8_t axis, uint16_t time) -{ - mavlink_message_t msg; - mavlink_msg_request_dynamic_gyro_calibration_pack(mavlink_system.sysid, mavlink_system.compid, &msg, target_system, target_component, mode, axis, time); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE REQUEST_DYNAMIC_GYRO_CALIBRATION UNPACKING - -/** - * @brief Get field target_system from request_dynamic_gyro_calibration message - * - * @return The system which should auto-calibrate - */ -static inline uint8_t mavlink_msg_request_dynamic_gyro_calibration_get_target_system(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field target_component from request_dynamic_gyro_calibration message - * - * @return The system component which should auto-calibrate - */ -static inline uint8_t mavlink_msg_request_dynamic_gyro_calibration_get_target_component(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field mode from request_dynamic_gyro_calibration message - * - * @return The current ground-truth rpm - */ -static inline float mavlink_msg_request_dynamic_gyro_calibration_get_mode(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (float)r.f; -} - -/** - * @brief Get field axis from request_dynamic_gyro_calibration message - * - * @return The axis to calibrate: 0 roll, 1 pitch, 2 yaw - */ -static inline uint8_t mavlink_msg_request_dynamic_gyro_calibration_get_axis(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; -} - -/** - * @brief Get field time from request_dynamic_gyro_calibration message - * - * @return The time to average over in ms - */ -static inline uint16_t mavlink_msg_request_dynamic_gyro_calibration_get_time(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; -} - -static inline void mavlink_msg_request_dynamic_gyro_calibration_decode(const mavlink_message_t* msg, mavlink_request_dynamic_gyro_calibration_t* request_dynamic_gyro_calibration) -{ - request_dynamic_gyro_calibration->target_system = mavlink_msg_request_dynamic_gyro_calibration_get_target_system(msg); - request_dynamic_gyro_calibration->target_component = mavlink_msg_request_dynamic_gyro_calibration_get_target_component(msg); - request_dynamic_gyro_calibration->mode = mavlink_msg_request_dynamic_gyro_calibration_get_mode(msg); - request_dynamic_gyro_calibration->axis = mavlink_msg_request_dynamic_gyro_calibration_get_axis(msg); - request_dynamic_gyro_calibration->time = mavlink_msg_request_dynamic_gyro_calibration_get_time(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_request_static_calibration.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_request_static_calibration.h deleted file mode 100644 index b97062e75b..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_request_static_calibration.h +++ /dev/null @@ -1,90 +0,0 @@ -// MESSAGE REQUEST_STATIC_CALIBRATION PACKING - -#define MAVLINK_MSG_ID_REQUEST_STATIC_CALIBRATION 83 - -typedef struct __mavlink_request_static_calibration_t -{ - uint8_t target_system; ///< The system which should auto-calibrate - uint8_t target_component; ///< The system component which should auto-calibrate - uint16_t time; ///< The time to average over in ms - -} mavlink_request_static_calibration_t; - - - -/** - * @brief Send a request_static_calibration message - * - * @param target_system The system which should auto-calibrate - * @param target_component The system component which should auto-calibrate - * @param time The time to average over in ms - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_request_static_calibration_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t time) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_REQUEST_STATIC_CALIBRATION; - - i += put_uint8_t_by_index(target_system, i, msg->payload); //The system which should auto-calibrate - i += put_uint8_t_by_index(target_component, i, msg->payload); //The system component which should auto-calibrate - i += put_uint16_t_by_index(time, i, msg->payload); //The time to average over in ms - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -static inline uint16_t mavlink_msg_request_static_calibration_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_request_static_calibration_t* request_static_calibration) -{ - return mavlink_msg_request_static_calibration_pack(system_id, component_id, msg, request_static_calibration->target_system, request_static_calibration->target_component, request_static_calibration->time); -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_request_static_calibration_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t time) -{ - mavlink_message_t msg; - mavlink_msg_request_static_calibration_pack(mavlink_system.sysid, mavlink_system.compid, &msg, target_system, target_component, time); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE REQUEST_STATIC_CALIBRATION UNPACKING - -/** - * @brief Get field target_system from request_static_calibration message - * - * @return The system which should auto-calibrate - */ -static inline uint8_t mavlink_msg_request_static_calibration_get_target_system(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field target_component from request_static_calibration message - * - * @return The system component which should auto-calibrate - */ -static inline uint8_t mavlink_msg_request_static_calibration_get_target_component(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field time from request_static_calibration message - * - * @return The time to average over in ms - */ -static inline uint16_t mavlink_msg_request_static_calibration_get_time(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; -} - -static inline void mavlink_msg_request_static_calibration_decode(const mavlink_message_t* msg, mavlink_request_static_calibration_t* request_static_calibration) -{ - request_static_calibration->target_system = mavlink_msg_request_static_calibration_get_target_system(msg); - request_static_calibration->target_component = mavlink_msg_request_static_calibration_get_target_component(msg); - request_static_calibration->time = mavlink_msg_request_static_calibration_get_time(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_set_altitude.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_set_altitude.h deleted file mode 100644 index 27cae625ff..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_set_altitude.h +++ /dev/null @@ -1,78 +0,0 @@ -// MESSAGE SET_ALTITUDE PACKING - -#define MAVLINK_MSG_ID_SET_ALTITUDE 80 - -typedef struct __mavlink_set_altitude_t -{ - uint8_t target; ///< The system setting the altitude - uint32_t mode; ///< The new altitude in meters - -} mavlink_set_altitude_t; - - - -/** - * @brief Send a set_altitude message - * - * @param target The system setting the altitude - * @param mode The new altitude in meters - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_altitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, uint32_t mode) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_SET_ALTITUDE; - - i += put_uint8_t_by_index(target, i, msg->payload); //The system setting the altitude - i += put_uint32_t_by_index(mode, i, msg->payload); //The new altitude in meters - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -static inline uint16_t mavlink_msg_set_altitude_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_altitude_t* set_altitude) -{ - return mavlink_msg_set_altitude_pack(system_id, component_id, msg, set_altitude->target, set_altitude->mode); -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_altitude_send(mavlink_channel_t chan, uint8_t target, uint32_t mode) -{ - mavlink_message_t msg; - mavlink_msg_set_altitude_pack(mavlink_system.sysid, mavlink_system.compid, &msg, target, mode); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE SET_ALTITUDE UNPACKING - -/** - * @brief Get field target from set_altitude message - * - * @return The system setting the altitude - */ -static inline uint8_t mavlink_msg_set_altitude_get_target(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field mode from set_altitude message - * - * @return The new altitude in meters - */ -static inline uint32_t mavlink_msg_set_altitude_get_mode(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (uint32_t)r.i; -} - -static inline void mavlink_msg_set_altitude_decode(const mavlink_message_t* msg, mavlink_set_altitude_t* set_altitude) -{ - set_altitude->target = mavlink_msg_set_altitude_get_target(msg); - set_altitude->mode = mavlink_msg_set_altitude_get_mode(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_set_cam_shutter.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_set_cam_shutter.h deleted file mode 100644 index 6a34b7e4bd..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_set_cam_shutter.h +++ /dev/null @@ -1,197 +0,0 @@ -// MESSAGE SET_CAM_SHUTTER PACKING - -#define MAVLINK_MSG_ID_SET_CAM_SHUTTER 100 - -typedef struct __mavlink_set_cam_shutter_t -{ - 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 - uint16_t interval; ///< Shutter interval, in microseconds - uint16_t exposure; ///< Exposure time, in microseconds - float gain; ///< Camera gain - -} mavlink_set_cam_shutter_t; - - - -/** - * @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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; - - i += put_uint8_t_by_index(cam_no, i, msg->payload); // Camera id - i += put_uint8_t_by_index(cam_mode, i, msg->payload); // Camera mode: 0 = auto, 1 = manual - i += put_uint8_t_by_index(trigger_pin, i, msg->payload); // Trigger pin, 0-3 for PtGrey FireFly - i += put_uint16_t_by_index(interval, i, msg->payload); // Shutter interval, in microseconds - i += put_uint16_t_by_index(exposure, i, msg->payload); // Exposure time, in microseconds - i += put_float_by_index(gain, i, msg->payload); // Camera gain - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @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 chan The MAVLink channel this message was 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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; - - i += put_uint8_t_by_index(cam_no, i, msg->payload); // Camera id - i += put_uint8_t_by_index(cam_mode, i, msg->payload); // Camera mode: 0 = auto, 1 = manual - i += put_uint8_t_by_index(trigger_pin, i, msg->payload); // Trigger pin, 0-3 for PtGrey FireFly - i += put_uint16_t_by_index(interval, i, msg->payload); // Shutter interval, in microseconds - i += put_uint16_t_by_index(exposure, i, msg->payload); // Exposure time, in microseconds - i += put_float_by_index(gain, i, msg->payload); // Camera gain - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a set_cam_shutter struct into 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 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 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) -{ - mavlink_message_t msg; - mavlink_msg_set_cam_shutter_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, cam_no, cam_mode, trigger_pin, interval, exposure, gain); - mavlink_send_uart(chan, &msg); -} - -#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 (uint8_t)(msg->payload)[0]; -} - -/** - * @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 (uint8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @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 (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -/** - * @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) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; -} - -/** - * @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) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[3]; - return (float)r.f; -} - -/** - * @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) -{ - 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); - 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->gain = mavlink_msg_set_cam_shutter_get_gain(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_vicon_position_estimate.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_vicon_position_estimate.h deleted file mode 100644 index 6e349eafc5..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_vicon_position_estimate.h +++ /dev/null @@ -1,242 +0,0 @@ -// MESSAGE VICON_POSITION_ESTIMATE PACKING - -#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE 112 - -typedef struct __mavlink_vicon_position_estimate_t -{ - uint64_t usec; ///< Timestamp (milliseconds) - float x; ///< Global X position - float y; ///< Global Y position - float z; ///< Global Z position - float roll; ///< Roll angle in rad - float pitch; ///< Pitch angle in rad - float yaw; ///< Yaw angle in rad - -} mavlink_vicon_position_estimate_t; - - - -/** - * @brief Pack a vicon_position_estimate 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 usec Timestamp (milliseconds) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X position - i += put_float_by_index(y, i, msg->payload); // Global Y position - i += put_float_by_index(z, i, msg->payload); // Global Z position - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a vicon_position_estimate message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (milliseconds) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X position - i += put_float_by_index(y, i, msg->payload); // Global Y position - i += put_float_by_index(z, i, msg->payload); // Global Z position - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a vicon_position_estimate struct into 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 vicon_position_estimate C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vicon_position_estimate_t* vicon_position_estimate) -{ - return mavlink_msg_vicon_position_estimate_pack(system_id, component_id, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw); -} - -/** - * @brief Send a vicon_position_estimate message - * @param chan MAVLink channel to send the message - * - * @param usec Timestamp (milliseconds) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) -{ - mavlink_message_t msg; - mavlink_msg_vicon_position_estimate_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, x, y, z, roll, pitch, yaw); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE VICON_POSITION_ESTIMATE UNPACKING - -/** - * @brief Get field usec from vicon_position_estimate message - * - * @return Timestamp (milliseconds) - */ -static inline uint64_t mavlink_msg_vicon_position_estimate_get_usec(const mavlink_message_t* msg) -{ - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; -} - -/** - * @brief Get field x from vicon_position_estimate message - * - * @return Global X position - */ -static inline float mavlink_msg_vicon_position_estimate_get_x(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (float)r.f; -} - -/** - * @brief Get field y from vicon_position_estimate message - * - * @return Global Y position - */ -static inline float mavlink_msg_vicon_position_estimate_get_y(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field z from vicon_position_estimate message - * - * @return Global Z position - */ -static inline float mavlink_msg_vicon_position_estimate_get_z(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field roll from vicon_position_estimate message - * - * @return Roll angle in rad - */ -static inline float mavlink_msg_vicon_position_estimate_get_roll(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field pitch from vicon_position_estimate message - * - * @return Pitch angle in rad - */ -static inline float mavlink_msg_vicon_position_estimate_get_pitch(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field yaw from vicon_position_estimate message - * - * @return Yaw angle in rad - */ -static inline float mavlink_msg_vicon_position_estimate_get_yaw(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Decode a vicon_position_estimate message into a struct - * - * @param msg The message to decode - * @param vicon_position_estimate C-struct to decode the message contents into - */ -static inline void mavlink_msg_vicon_position_estimate_decode(const mavlink_message_t* msg, mavlink_vicon_position_estimate_t* vicon_position_estimate) -{ - vicon_position_estimate->usec = mavlink_msg_vicon_position_estimate_get_usec(msg); - vicon_position_estimate->x = mavlink_msg_vicon_position_estimate_get_x(msg); - vicon_position_estimate->y = mavlink_msg_vicon_position_estimate_get_y(msg); - vicon_position_estimate->z = mavlink_msg_vicon_position_estimate_get_z(msg); - vicon_position_estimate->roll = mavlink_msg_vicon_position_estimate_get_roll(msg); - vicon_position_estimate->pitch = mavlink_msg_vicon_position_estimate_get_pitch(msg); - vicon_position_estimate->yaw = mavlink_msg_vicon_position_estimate_get_yaw(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_vision_position_estimate.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_vision_position_estimate.h deleted file mode 100644 index 30728191d4..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_vision_position_estimate.h +++ /dev/null @@ -1,242 +0,0 @@ -// MESSAGE VISION_POSITION_ESTIMATE PACKING - -#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE 111 - -typedef struct __mavlink_vision_position_estimate_t -{ - uint64_t usec; ///< Timestamp (milliseconds) - float x; ///< Global X position - float y; ///< Global Y position - float z; ///< Global Z position - float roll; ///< Roll angle in rad - float pitch; ///< Pitch angle in rad - float yaw; ///< Yaw angle in rad - -} mavlink_vision_position_estimate_t; - - - -/** - * @brief Pack a vision_position_estimate 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 usec Timestamp (milliseconds) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X position - i += put_float_by_index(y, i, msg->payload); // Global Y position - i += put_float_by_index(z, i, msg->payload); // Global Z position - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a vision_position_estimate message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (milliseconds) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X position - i += put_float_by_index(y, i, msg->payload); // Global Y position - i += put_float_by_index(z, i, msg->payload); // Global Z position - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a vision_position_estimate struct into 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 vision_position_estimate C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vision_position_estimate_t* vision_position_estimate) -{ - return mavlink_msg_vision_position_estimate_pack(system_id, component_id, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw); -} - -/** - * @brief Send a vision_position_estimate message - * @param chan MAVLink channel to send the message - * - * @param usec Timestamp (milliseconds) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) -{ - mavlink_message_t msg; - mavlink_msg_vision_position_estimate_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, x, y, z, roll, pitch, yaw); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE VISION_POSITION_ESTIMATE UNPACKING - -/** - * @brief Get field usec from vision_position_estimate message - * - * @return Timestamp (milliseconds) - */ -static inline uint64_t mavlink_msg_vision_position_estimate_get_usec(const mavlink_message_t* msg) -{ - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; -} - -/** - * @brief Get field x from vision_position_estimate message - * - * @return Global X position - */ -static inline float mavlink_msg_vision_position_estimate_get_x(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (float)r.f; -} - -/** - * @brief Get field y from vision_position_estimate message - * - * @return Global Y position - */ -static inline float mavlink_msg_vision_position_estimate_get_y(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field z from vision_position_estimate message - * - * @return Global Z position - */ -static inline float mavlink_msg_vision_position_estimate_get_z(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field roll from vision_position_estimate message - * - * @return Roll angle in rad - */ -static inline float mavlink_msg_vision_position_estimate_get_roll(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field pitch from vision_position_estimate message - * - * @return Pitch angle in rad - */ -static inline float mavlink_msg_vision_position_estimate_get_pitch(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field yaw from vision_position_estimate message - * - * @return Yaw angle in rad - */ -static inline float mavlink_msg_vision_position_estimate_get_yaw(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Decode a vision_position_estimate message into a struct - * - * @param msg The message to decode - * @param vision_position_estimate C-struct to decode the message contents into - */ -static inline void mavlink_msg_vision_position_estimate_decode(const mavlink_message_t* msg, mavlink_vision_position_estimate_t* vision_position_estimate) -{ - vision_position_estimate->usec = mavlink_msg_vision_position_estimate_get_usec(msg); - vision_position_estimate->x = mavlink_msg_vision_position_estimate_get_x(msg); - vision_position_estimate->y = mavlink_msg_vision_position_estimate_get_y(msg); - vision_position_estimate->z = mavlink_msg_vision_position_estimate_get_z(msg); - vision_position_estimate->roll = mavlink_msg_vision_position_estimate_get_roll(msg); - vision_position_estimate->pitch = mavlink_msg_vision_position_estimate_get_pitch(msg); - vision_position_estimate->yaw = mavlink_msg_vision_position_estimate_get_yaw(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_vision_speed_estimate.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_vision_speed_estimate.h deleted file mode 100644 index 66224c28e9..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_vision_speed_estimate.h +++ /dev/null @@ -1,176 +0,0 @@ -// MESSAGE VISION_SPEED_ESTIMATE PACKING - -#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE 113 - -typedef struct __mavlink_vision_speed_estimate_t -{ - uint64_t usec; ///< Timestamp (milliseconds) - float x; ///< Global X speed - float y; ///< Global Y speed - float z; ///< Global Z speed - -} mavlink_vision_speed_estimate_t; - - - -/** - * @brief Pack a vision_speed_estimate 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 usec Timestamp (milliseconds) - * @param x Global X speed - * @param y Global Y speed - * @param z Global Z speed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float x, float y, float z) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X speed - i += put_float_by_index(y, i, msg->payload); // Global Y speed - i += put_float_by_index(z, i, msg->payload); // Global Z speed - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a vision_speed_estimate message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (milliseconds) - * @param x Global X speed - * @param y Global Y speed - * @param z Global Z speed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float x, float y, float z) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X speed - i += put_float_by_index(y, i, msg->payload); // Global Y speed - i += put_float_by_index(z, i, msg->payload); // Global Z speed - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a vision_speed_estimate struct into 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 vision_speed_estimate C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vision_speed_estimate_t* vision_speed_estimate) -{ - return mavlink_msg_vision_speed_estimate_pack(system_id, component_id, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z); -} - -/** - * @brief Send a vision_speed_estimate message - * @param chan MAVLink channel to send the message - * - * @param usec Timestamp (milliseconds) - * @param x Global X speed - * @param y Global Y speed - * @param z Global Z speed - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z) -{ - mavlink_message_t msg; - mavlink_msg_vision_speed_estimate_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, x, y, z); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE VISION_SPEED_ESTIMATE UNPACKING - -/** - * @brief Get field usec from vision_speed_estimate message - * - * @return Timestamp (milliseconds) - */ -static inline uint64_t mavlink_msg_vision_speed_estimate_get_usec(const mavlink_message_t* msg) -{ - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; -} - -/** - * @brief Get field x from vision_speed_estimate message - * - * @return Global X speed - */ -static inline float mavlink_msg_vision_speed_estimate_get_x(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (float)r.f; -} - -/** - * @brief Get field y from vision_speed_estimate message - * - * @return Global Y speed - */ -static inline float mavlink_msg_vision_speed_estimate_get_y(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field z from vision_speed_estimate message - * - * @return Global Z speed - */ -static inline float mavlink_msg_vision_speed_estimate_get_z(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Decode a vision_speed_estimate message into a struct - * - * @param msg The message to decode - * @param vision_speed_estimate C-struct to decode the message contents into - */ -static inline void mavlink_msg_vision_speed_estimate_decode(const mavlink_message_t* msg, mavlink_vision_speed_estimate_t* vision_speed_estimate) -{ - vision_speed_estimate->usec = mavlink_msg_vision_speed_estimate_get_usec(msg); - vision_speed_estimate->x = mavlink_msg_vision_speed_estimate_get_x(msg); - vision_speed_estimate->y = mavlink_msg_vision_speed_estimate_get_y(msg); - vision_speed_estimate->z = mavlink_msg_vision_speed_estimate_get_z(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_watchdog_command.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_watchdog_command.h deleted file mode 100644 index 25ea0f5f1d..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_watchdog_command.h +++ /dev/null @@ -1,158 +0,0 @@ -// MESSAGE WATCHDOG_COMMAND PACKING - -#define MAVLINK_MSG_ID_WATCHDOG_COMMAND 153 - -typedef struct __mavlink_watchdog_command_t -{ - uint8_t target_system_id; ///< Target system ID - uint16_t watchdog_id; ///< Watchdog ID - uint16_t process_id; ///< Process ID - uint8_t command_id; ///< Command ID - -} mavlink_watchdog_command_t; - - - -/** - * @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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; - - i += put_uint8_t_by_index(target_system_id, i, msg->payload); // Target system ID - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_uint8_t_by_index(command_id, i, msg->payload); // Command ID - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @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 chan The MAVLink channel this message was 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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; - - i += put_uint8_t_by_index(target_system_id, i, msg->payload); // Target system ID - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_uint8_t_by_index(command_id, i, msg->payload); // Command ID - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a watchdog_command struct into 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 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 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) -{ - mavlink_message_t msg; - mavlink_msg_watchdog_command_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system_id, watchdog_id, process_id, command_id); - mavlink_send_uart(chan, &msg); -} - -#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 (uint8_t)(msg->payload)[0]; -} - -/** - * @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) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t))[1]; - return (uint16_t)r.s; -} - -/** - * @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) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @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 (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; -} - -/** - * @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) -{ - watchdog_command->target_system_id = mavlink_msg_watchdog_command_get_target_system_id(msg); - 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->command_id = mavlink_msg_watchdog_command_get_command_id(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h deleted file mode 100644 index db693bc554..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h +++ /dev/null @@ -1,124 +0,0 @@ -// MESSAGE WATCHDOG_HEARTBEAT PACKING - -#define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT 150 - -typedef struct __mavlink_watchdog_heartbeat_t -{ - uint16_t watchdog_id; ///< Watchdog ID - uint16_t process_count; ///< Number of processes - -} mavlink_watchdog_heartbeat_t; - - - -/** - * @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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; - - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_count, i, msg->payload); // Number of processes - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @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 chan The MAVLink channel this message was 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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; - - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_count, i, msg->payload); // Number of processes - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a watchdog_heartbeat struct into 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 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 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) -{ - mavlink_message_t msg; - mavlink_msg_watchdog_heartbeat_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, watchdog_id, process_count); - mavlink_send_uart(chan, &msg); -} - -#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) -{ - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; -} - -/** - * @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) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @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) -{ - watchdog_heartbeat->watchdog_id = mavlink_msg_watchdog_heartbeat_get_watchdog_id(msg); - watchdog_heartbeat->process_count = mavlink_msg_watchdog_heartbeat_get_process_count(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_watchdog_process_info.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_watchdog_process_info.h deleted file mode 100644 index a312a14679..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_watchdog_process_info.h +++ /dev/null @@ -1,186 +0,0 @@ -// MESSAGE WATCHDOG_PROCESS_INFO PACKING - -#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO 151 - -typedef struct __mavlink_watchdog_process_info_t -{ - uint16_t watchdog_id; ///< Watchdog ID - uint16_t process_id; ///< Process ID - int8_t name[100]; ///< Process name - int8_t arguments[147]; ///< Process arguments - int32_t timeout; ///< Timeout (seconds) - -} mavlink_watchdog_process_info_t; - -#define MAVLINK_MSG_WATCHDOG_PROCESS_INFO_FIELD_NAME_LEN 100 -#define MAVLINK_MSG_WATCHDOG_PROCESS_INFO_FIELD_ARGUMENTS_LEN 147 - - -/** - * @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 int8_t* name, const int8_t* arguments, int32_t timeout) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; - - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_array_by_index(name, 100, i, msg->payload); // Process name - i += put_array_by_index(arguments, 147, i, msg->payload); // Process arguments - i += put_int32_t_by_index(timeout, i, msg->payload); // Timeout (seconds) - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @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 chan The MAVLink channel this message was 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 int8_t* name, const int8_t* arguments, int32_t timeout) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; - - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_array_by_index(name, 100, i, msg->payload); // Process name - i += put_array_by_index(arguments, 147, i, msg->payload); // Process arguments - i += put_int32_t_by_index(timeout, i, msg->payload); // Timeout (seconds) - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a watchdog_process_info struct into 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 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 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 int8_t* name, const int8_t* arguments, int32_t timeout) -{ - mavlink_message_t msg; - mavlink_msg_watchdog_process_info_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, watchdog_id, process_id, name, arguments, timeout); - mavlink_send_uart(chan, &msg); -} - -#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) -{ - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; -} - -/** - * @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) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @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, int8_t* r_data) -{ - - memcpy(r_data, msg->payload+sizeof(uint16_t)+sizeof(uint16_t), 100); - return 100; -} - -/** - * @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, int8_t* r_data) -{ - - memcpy(r_data, msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+100, 147); - return 147; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+100+147)[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+100+147)[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+100+147)[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+100+147)[3]; - return (int32_t)r.i; -} - -/** - * @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) -{ - 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); - watchdog_process_info->timeout = mavlink_msg_watchdog_process_info_get_timeout(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_watchdog_process_status.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_watchdog_process_status.h deleted file mode 100644 index 9afa6ca14a..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_watchdog_process_status.h +++ /dev/null @@ -1,200 +0,0 @@ -// MESSAGE WATCHDOG_PROCESS_STATUS PACKING - -#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS 152 - -typedef struct __mavlink_watchdog_process_status_t -{ - uint16_t watchdog_id; ///< Watchdog ID - uint16_t process_id; ///< Process ID - uint8_t state; ///< Is running / finished / suspended / crashed - uint8_t muted; ///< Is muted - int32_t pid; ///< PID - uint16_t crashes; ///< Number of crashes - -} mavlink_watchdog_process_status_t; - - - -/** - * @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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; - - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_uint8_t_by_index(state, i, msg->payload); // Is running / finished / suspended / crashed - i += put_uint8_t_by_index(muted, i, msg->payload); // Is muted - i += put_int32_t_by_index(pid, i, msg->payload); // PID - i += put_uint16_t_by_index(crashes, i, msg->payload); // Number of crashes - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @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 chan The MAVLink channel this message was 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) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; - - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_uint8_t_by_index(state, i, msg->payload); // Is running / finished / suspended / crashed - i += put_uint8_t_by_index(muted, i, msg->payload); // Is muted - i += put_int32_t_by_index(pid, i, msg->payload); // PID - i += put_uint16_t_by_index(crashes, i, msg->payload); // Number of crashes - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a watchdog_process_status struct into 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 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 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) -{ - mavlink_message_t msg; - mavlink_msg_watchdog_process_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, watchdog_id, process_id, state, muted, pid, crashes); - mavlink_send_uart(chan, &msg); -} - -#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) -{ - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; -} - -/** - * @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) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @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 (uint8_t)(msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[0]; -} - -/** - * @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 (uint8_t)(msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[0]; -} - -/** - * @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) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (int32_t)r.i; -} - -/** - * @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) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t))[1]; - return (uint16_t)r.s; -} - -/** - * @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) -{ - 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->state = mavlink_msg_watchdog_process_status_get_state(msg); - watchdog_process_status->muted = mavlink_msg_watchdog_process_status_get_muted(msg); - watchdog_process_status->pid = mavlink_msg_watchdog_process_status_get_pid(msg); - watchdog_process_status->crashes = mavlink_msg_watchdog_process_status_get_crashes(msg); -} diff --git a/libraries/GCS_MAVLink/include/pixhawk/pixhawk.h b/libraries/GCS_MAVLink/include/pixhawk/pixhawk.h deleted file mode 100644 index 318877e873..0000000000 --- a/libraries/GCS_MAVLink/include/pixhawk/pixhawk.h +++ /dev/null @@ -1,79 +0,0 @@ -/** @file - * @brief MAVLink comm protocol. - * @see http://qgroundcontrol.org/mavlink/ - * Generated on Friday, August 5 2011, 07:37 UTC - */ -#ifndef PIXHAWK_H -#define PIXHAWK_H - -#ifdef __cplusplus -extern "C" { -#endif - - -#include "../protocol.h" - -#define MAVLINK_ENABLED_PIXHAWK - - -#include "../common/common.h" -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 0 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 0 -#endif - -// ENUM DEFINITIONS - -/** @brief Content Types for data transmission handshake */ -enum DATA_TYPES -{ - DATA_TYPE_JPEG_IMAGE=1, - DATA_TYPE_RAW_IMAGE=2, - DATA_TYPE_KINECT=3, - DATA_TYPES_ENUM_END -}; - - -// MESSAGE DEFINITIONS - -#include "./mavlink_msg_attitude_control.h" -#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_vision_position_estimate.h" -#include "./mavlink_msg_vicon_position_estimate.h" -#include "./mavlink_msg_vision_speed_estimate.h" -#include "./mavlink_msg_position_control_setpoint_set.h" -#include "./mavlink_msg_position_control_offset_set.h" -#include "./mavlink_msg_position_control_setpoint.h" -#include "./mavlink_msg_marker.h" -#include "./mavlink_msg_raw_aux.h" -#include "./mavlink_msg_aux_status.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_data_transmission_handshake.h" -#include "./mavlink_msg_encapsulated_data.h" -#include "./mavlink_msg_brief_feature.h" - - -// MESSAGE LENGTHS - -#undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 14, 14, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 11, 52, 1, 92, 0, 0, 0, 0, 0, 0, 0, 32, 32, 20, 0, 0, 0, 0, 0, 0, 20, 18, 0, 0, 0, 0, 0, 0, 0, 0, 26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 12, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 18, 0, 0, 0, 0, 0, 106, 42, 54, 0, 0, 0, 0, 0, 0, 0, 8, 255, 53, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } - -#ifdef __cplusplus -} -#endif -#endif diff --git a/libraries/GCS_MAVLink/include/slugs/mavlink.h b/libraries/GCS_MAVLink/include/slugs/mavlink.h deleted file mode 100644 index 29ce7ed555..0000000000 --- a/libraries/GCS_MAVLink/include/slugs/mavlink.h +++ /dev/null @@ -1,11 +0,0 @@ -/** @file - * @brief MAVLink comm protocol. - * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Friday, August 5 2011, 07:37 UTC - */ -#ifndef MAVLINK_H -#define MAVLINK_H - -#include "slugs.h" - -#endif diff --git a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_air_data.h b/libraries/GCS_MAVLink/include/slugs/mavlink_msg_air_data.h deleted file mode 100644 index 2b212ae1ba..0000000000 --- a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_air_data.h +++ /dev/null @@ -1,148 +0,0 @@ -// MESSAGE AIR_DATA PACKING - -#define MAVLINK_MSG_ID_AIR_DATA 171 - -typedef struct __mavlink_air_data_t -{ - float dynamicPressure; ///< Dynamic pressure (Pa) - float staticPressure; ///< Static pressure (Pa) - uint16_t temperature; ///< Board temperature - -} mavlink_air_data_t; - - - -/** - * @brief Pack a air_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 dynamicPressure Dynamic pressure (Pa) - * @param staticPressure Static pressure (Pa) - * @param temperature Board temperature - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_air_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float dynamicPressure, float staticPressure, uint16_t temperature) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_AIR_DATA; - - i += put_float_by_index(dynamicPressure, i, msg->payload); // Dynamic pressure (Pa) - i += put_float_by_index(staticPressure, i, msg->payload); // Static pressure (Pa) - i += put_uint16_t_by_index(temperature, i, msg->payload); // Board temperature - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a air_data message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param dynamicPressure Dynamic pressure (Pa) - * @param staticPressure Static pressure (Pa) - * @param temperature Board temperature - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_air_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float dynamicPressure, float staticPressure, uint16_t temperature) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_AIR_DATA; - - i += put_float_by_index(dynamicPressure, i, msg->payload); // Dynamic pressure (Pa) - i += put_float_by_index(staticPressure, i, msg->payload); // Static pressure (Pa) - i += put_uint16_t_by_index(temperature, i, msg->payload); // Board temperature - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a air_data struct into 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 air_data C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_air_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_air_data_t* air_data) -{ - return mavlink_msg_air_data_pack(system_id, component_id, msg, air_data->dynamicPressure, air_data->staticPressure, air_data->temperature); -} - -/** - * @brief Send a air_data message - * @param chan MAVLink channel to send the message - * - * @param dynamicPressure Dynamic pressure (Pa) - * @param staticPressure Static pressure (Pa) - * @param temperature Board temperature - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_air_data_send(mavlink_channel_t chan, float dynamicPressure, float staticPressure, uint16_t temperature) -{ - mavlink_message_t msg; - mavlink_msg_air_data_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, dynamicPressure, staticPressure, temperature); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE AIR_DATA UNPACKING - -/** - * @brief Get field dynamicPressure from air_data message - * - * @return Dynamic pressure (Pa) - */ -static inline float mavlink_msg_air_data_get_dynamicPressure(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; -} - -/** - * @brief Get field staticPressure from air_data message - * - * @return Static pressure (Pa) - */ -static inline float mavlink_msg_air_data_get_staticPressure(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field temperature from air_data message - * - * @return Board temperature - */ -static inline uint16_t mavlink_msg_air_data_get_temperature(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Decode a air_data message into a struct - * - * @param msg The message to decode - * @param air_data C-struct to decode the message contents into - */ -static inline void mavlink_msg_air_data_decode(const mavlink_message_t* msg, mavlink_air_data_t* air_data) -{ - air_data->dynamicPressure = mavlink_msg_air_data_get_dynamicPressure(msg); - air_data->staticPressure = mavlink_msg_air_data_get_staticPressure(msg); - air_data->temperature = mavlink_msg_air_data_get_temperature(msg); -} diff --git a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_cpu_load.h b/libraries/GCS_MAVLink/include/slugs/mavlink_msg_cpu_load.h deleted file mode 100644 index 265aa3fcee..0000000000 --- a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_cpu_load.h +++ /dev/null @@ -1,138 +0,0 @@ -// MESSAGE CPU_LOAD PACKING - -#define MAVLINK_MSG_ID_CPU_LOAD 170 - -typedef struct __mavlink_cpu_load_t -{ - uint8_t sensLoad; ///< Sensor DSC Load - uint8_t ctrlLoad; ///< Control DSC Load - uint16_t batVolt; ///< Battery Voltage in millivolts - -} mavlink_cpu_load_t; - - - -/** - * @brief Pack a cpu_load 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 sensLoad Sensor DSC Load - * @param ctrlLoad Control DSC Load - * @param batVolt Battery Voltage in millivolts - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_cpu_load_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t sensLoad, uint8_t ctrlLoad, uint16_t batVolt) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; - - i += put_uint8_t_by_index(sensLoad, i, msg->payload); // Sensor DSC Load - i += put_uint8_t_by_index(ctrlLoad, i, msg->payload); // Control DSC Load - i += put_uint16_t_by_index(batVolt, i, msg->payload); // Battery Voltage in millivolts - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a cpu_load message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param sensLoad Sensor DSC Load - * @param ctrlLoad Control DSC Load - * @param batVolt Battery Voltage in millivolts - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_cpu_load_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t sensLoad, uint8_t ctrlLoad, uint16_t batVolt) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; - - i += put_uint8_t_by_index(sensLoad, i, msg->payload); // Sensor DSC Load - i += put_uint8_t_by_index(ctrlLoad, i, msg->payload); // Control DSC Load - i += put_uint16_t_by_index(batVolt, i, msg->payload); // Battery Voltage in millivolts - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a cpu_load struct into 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 cpu_load C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_cpu_load_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cpu_load_t* cpu_load) -{ - return mavlink_msg_cpu_load_pack(system_id, component_id, msg, cpu_load->sensLoad, cpu_load->ctrlLoad, cpu_load->batVolt); -} - -/** - * @brief Send a cpu_load message - * @param chan MAVLink channel to send the message - * - * @param sensLoad Sensor DSC Load - * @param ctrlLoad Control DSC Load - * @param batVolt Battery Voltage in millivolts - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_cpu_load_send(mavlink_channel_t chan, uint8_t sensLoad, uint8_t ctrlLoad, uint16_t batVolt) -{ - mavlink_message_t msg; - mavlink_msg_cpu_load_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, sensLoad, ctrlLoad, batVolt); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE CPU_LOAD UNPACKING - -/** - * @brief Get field sensLoad from cpu_load message - * - * @return Sensor DSC Load - */ -static inline uint8_t mavlink_msg_cpu_load_get_sensLoad(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field ctrlLoad from cpu_load message - * - * @return Control DSC Load - */ -static inline uint8_t mavlink_msg_cpu_load_get_ctrlLoad(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field batVolt from cpu_load message - * - * @return Battery Voltage in millivolts - */ -static inline uint16_t mavlink_msg_cpu_load_get_batVolt(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Decode a cpu_load message into a struct - * - * @param msg The message to decode - * @param cpu_load C-struct to decode the message contents into - */ -static inline void mavlink_msg_cpu_load_decode(const mavlink_message_t* msg, mavlink_cpu_load_t* cpu_load) -{ - cpu_load->sensLoad = mavlink_msg_cpu_load_get_sensLoad(msg); - cpu_load->ctrlLoad = mavlink_msg_cpu_load_get_ctrlLoad(msg); - cpu_load->batVolt = mavlink_msg_cpu_load_get_batVolt(msg); -} diff --git a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_ctrl_srfc_pt.h b/libraries/GCS_MAVLink/include/slugs/mavlink_msg_ctrl_srfc_pt.h deleted file mode 100644 index 9221dc4f00..0000000000 --- a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_ctrl_srfc_pt.h +++ /dev/null @@ -1,121 +0,0 @@ -// MESSAGE CTRL_SRFC_PT PACKING - -#define MAVLINK_MSG_ID_CTRL_SRFC_PT 181 - -typedef struct __mavlink_ctrl_srfc_pt_t -{ - uint8_t target; ///< The system setting the commands - uint16_t bitfieldPt; ///< Bitfield containing the PT configuration - -} mavlink_ctrl_srfc_pt_t; - - - -/** - * @brief Pack a ctrl_srfc_pt 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 The system setting the commands - * @param bitfieldPt Bitfield containing the PT configuration - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, uint16_t bitfieldPt) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; - - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the commands - i += put_uint16_t_by_index(bitfieldPt, i, msg->payload); // Bitfield containing the PT configuration - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a ctrl_srfc_pt message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target The system setting the commands - * @param bitfieldPt Bitfield containing the PT configuration - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint16_t bitfieldPt) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; - - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the commands - i += put_uint16_t_by_index(bitfieldPt, i, msg->payload); // Bitfield containing the PT configuration - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a ctrl_srfc_pt struct into 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 ctrl_srfc_pt C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ctrl_srfc_pt_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ctrl_srfc_pt_t* ctrl_srfc_pt) -{ - return mavlink_msg_ctrl_srfc_pt_pack(system_id, component_id, msg, ctrl_srfc_pt->target, ctrl_srfc_pt->bitfieldPt); -} - -/** - * @brief Send a ctrl_srfc_pt message - * @param chan MAVLink channel to send the message - * - * @param target The system setting the commands - * @param bitfieldPt Bitfield containing the PT configuration - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_ctrl_srfc_pt_send(mavlink_channel_t chan, uint8_t target, uint16_t bitfieldPt) -{ - mavlink_message_t msg; - mavlink_msg_ctrl_srfc_pt_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, bitfieldPt); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE CTRL_SRFC_PT UNPACKING - -/** - * @brief Get field target from ctrl_srfc_pt message - * - * @return The system setting the commands - */ -static inline uint8_t mavlink_msg_ctrl_srfc_pt_get_target(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field bitfieldPt from ctrl_srfc_pt message - * - * @return Bitfield containing the PT configuration - */ -static inline uint16_t mavlink_msg_ctrl_srfc_pt_get_bitfieldPt(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Decode a ctrl_srfc_pt message into a struct - * - * @param msg The message to decode - * @param ctrl_srfc_pt C-struct to decode the message contents into - */ -static inline void mavlink_msg_ctrl_srfc_pt_decode(const mavlink_message_t* msg, mavlink_ctrl_srfc_pt_t* ctrl_srfc_pt) -{ - ctrl_srfc_pt->target = mavlink_msg_ctrl_srfc_pt_get_target(msg); - ctrl_srfc_pt->bitfieldPt = mavlink_msg_ctrl_srfc_pt_get_bitfieldPt(msg); -} diff --git a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_data_log.h b/libraries/GCS_MAVLink/include/slugs/mavlink_msg_data_log.h deleted file mode 100644 index cbcc0daba3..0000000000 --- a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_data_log.h +++ /dev/null @@ -1,216 +0,0 @@ -// MESSAGE DATA_LOG PACKING - -#define MAVLINK_MSG_ID_DATA_LOG 177 - -typedef struct __mavlink_data_log_t -{ - float fl_1; ///< Log value 1 - float fl_2; ///< Log value 2 - float fl_3; ///< Log value 3 - float fl_4; ///< Log value 4 - float fl_5; ///< Log value 5 - float fl_6; ///< Log value 6 - -} mavlink_data_log_t; - - - -/** - * @brief Pack a data_log 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 fl_1 Log value 1 - * @param fl_2 Log value 2 - * @param fl_3 Log value 3 - * @param fl_4 Log value 4 - * @param fl_5 Log value 5 - * @param fl_6 Log value 6 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data_log_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float fl_1, float fl_2, float fl_3, float fl_4, float fl_5, float fl_6) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_DATA_LOG; - - i += put_float_by_index(fl_1, i, msg->payload); // Log value 1 - i += put_float_by_index(fl_2, i, msg->payload); // Log value 2 - i += put_float_by_index(fl_3, i, msg->payload); // Log value 3 - i += put_float_by_index(fl_4, i, msg->payload); // Log value 4 - i += put_float_by_index(fl_5, i, msg->payload); // Log value 5 - i += put_float_by_index(fl_6, i, msg->payload); // Log value 6 - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a data_log message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param fl_1 Log value 1 - * @param fl_2 Log value 2 - * @param fl_3 Log value 3 - * @param fl_4 Log value 4 - * @param fl_5 Log value 5 - * @param fl_6 Log value 6 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data_log_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float fl_1, float fl_2, float fl_3, float fl_4, float fl_5, float fl_6) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_DATA_LOG; - - i += put_float_by_index(fl_1, i, msg->payload); // Log value 1 - i += put_float_by_index(fl_2, i, msg->payload); // Log value 2 - i += put_float_by_index(fl_3, i, msg->payload); // Log value 3 - i += put_float_by_index(fl_4, i, msg->payload); // Log value 4 - i += put_float_by_index(fl_5, i, msg->payload); // Log value 5 - i += put_float_by_index(fl_6, i, msg->payload); // Log value 6 - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a data_log struct into 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 data_log C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data_log_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data_log_t* data_log) -{ - return mavlink_msg_data_log_pack(system_id, component_id, msg, data_log->fl_1, data_log->fl_2, data_log->fl_3, data_log->fl_4, data_log->fl_5, data_log->fl_6); -} - -/** - * @brief Send a data_log message - * @param chan MAVLink channel to send the message - * - * @param fl_1 Log value 1 - * @param fl_2 Log value 2 - * @param fl_3 Log value 3 - * @param fl_4 Log value 4 - * @param fl_5 Log value 5 - * @param fl_6 Log value 6 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_data_log_send(mavlink_channel_t chan, float fl_1, float fl_2, float fl_3, float fl_4, float fl_5, float fl_6) -{ - mavlink_message_t msg; - mavlink_msg_data_log_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, fl_1, fl_2, fl_3, fl_4, fl_5, fl_6); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE DATA_LOG UNPACKING - -/** - * @brief Get field fl_1 from data_log message - * - * @return Log value 1 - */ -static inline float mavlink_msg_data_log_get_fl_1(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; -} - -/** - * @brief Get field fl_2 from data_log message - * - * @return Log value 2 - */ -static inline float mavlink_msg_data_log_get_fl_2(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field fl_3 from data_log message - * - * @return Log value 3 - */ -static inline float mavlink_msg_data_log_get_fl_3(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field fl_4 from data_log message - * - * @return Log value 4 - */ -static inline float mavlink_msg_data_log_get_fl_4(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field fl_5 from data_log message - * - * @return Log value 5 - */ -static inline float mavlink_msg_data_log_get_fl_5(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field fl_6 from data_log message - * - * @return Log value 6 - */ -static inline float mavlink_msg_data_log_get_fl_6(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Decode a data_log message into a struct - * - * @param msg The message to decode - * @param data_log C-struct to decode the message contents into - */ -static inline void mavlink_msg_data_log_decode(const mavlink_message_t* msg, mavlink_data_log_t* data_log) -{ - data_log->fl_1 = mavlink_msg_data_log_get_fl_1(msg); - data_log->fl_2 = mavlink_msg_data_log_get_fl_2(msg); - data_log->fl_3 = mavlink_msg_data_log_get_fl_3(msg); - data_log->fl_4 = mavlink_msg_data_log_get_fl_4(msg); - data_log->fl_5 = mavlink_msg_data_log_get_fl_5(msg); - data_log->fl_6 = mavlink_msg_data_log_get_fl_6(msg); -} diff --git a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_diagnostic.h b/libraries/GCS_MAVLink/include/slugs/mavlink_msg_diagnostic.h deleted file mode 100644 index f3798f59bb..0000000000 --- a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_diagnostic.h +++ /dev/null @@ -1,210 +0,0 @@ -// MESSAGE DIAGNOSTIC PACKING - -#define MAVLINK_MSG_ID_DIAGNOSTIC 173 - -typedef struct __mavlink_diagnostic_t -{ - float diagFl1; ///< Diagnostic float 1 - float diagFl2; ///< Diagnostic float 2 - float diagFl3; ///< Diagnostic float 3 - int16_t diagSh1; ///< Diagnostic short 1 - int16_t diagSh2; ///< Diagnostic short 2 - int16_t diagSh3; ///< Diagnostic short 3 - -} mavlink_diagnostic_t; - - - -/** - * @brief Pack a diagnostic 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 diagFl1 Diagnostic float 1 - * @param diagFl2 Diagnostic float 2 - * @param diagFl3 Diagnostic float 3 - * @param diagSh1 Diagnostic short 1 - * @param diagSh2 Diagnostic short 2 - * @param diagSh3 Diagnostic short 3 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_diagnostic_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float diagFl1, float diagFl2, float diagFl3, int16_t diagSh1, int16_t diagSh2, int16_t diagSh3) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_DIAGNOSTIC; - - i += put_float_by_index(diagFl1, i, msg->payload); // Diagnostic float 1 - i += put_float_by_index(diagFl2, i, msg->payload); // Diagnostic float 2 - i += put_float_by_index(diagFl3, i, msg->payload); // Diagnostic float 3 - i += put_int16_t_by_index(diagSh1, i, msg->payload); // Diagnostic short 1 - i += put_int16_t_by_index(diagSh2, i, msg->payload); // Diagnostic short 2 - i += put_int16_t_by_index(diagSh3, i, msg->payload); // Diagnostic short 3 - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a diagnostic message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param diagFl1 Diagnostic float 1 - * @param diagFl2 Diagnostic float 2 - * @param diagFl3 Diagnostic float 3 - * @param diagSh1 Diagnostic short 1 - * @param diagSh2 Diagnostic short 2 - * @param diagSh3 Diagnostic short 3 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_diagnostic_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float diagFl1, float diagFl2, float diagFl3, int16_t diagSh1, int16_t diagSh2, int16_t diagSh3) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_DIAGNOSTIC; - - i += put_float_by_index(diagFl1, i, msg->payload); // Diagnostic float 1 - i += put_float_by_index(diagFl2, i, msg->payload); // Diagnostic float 2 - i += put_float_by_index(diagFl3, i, msg->payload); // Diagnostic float 3 - i += put_int16_t_by_index(diagSh1, i, msg->payload); // Diagnostic short 1 - i += put_int16_t_by_index(diagSh2, i, msg->payload); // Diagnostic short 2 - i += put_int16_t_by_index(diagSh3, i, msg->payload); // Diagnostic short 3 - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a diagnostic struct into 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 diagnostic C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_diagnostic_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_diagnostic_t* diagnostic) -{ - return mavlink_msg_diagnostic_pack(system_id, component_id, msg, diagnostic->diagFl1, diagnostic->diagFl2, diagnostic->diagFl3, diagnostic->diagSh1, diagnostic->diagSh2, diagnostic->diagSh3); -} - -/** - * @brief Send a diagnostic message - * @param chan MAVLink channel to send the message - * - * @param diagFl1 Diagnostic float 1 - * @param diagFl2 Diagnostic float 2 - * @param diagFl3 Diagnostic float 3 - * @param diagSh1 Diagnostic short 1 - * @param diagSh2 Diagnostic short 2 - * @param diagSh3 Diagnostic short 3 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_diagnostic_send(mavlink_channel_t chan, float diagFl1, float diagFl2, float diagFl3, int16_t diagSh1, int16_t diagSh2, int16_t diagSh3) -{ - mavlink_message_t msg; - mavlink_msg_diagnostic_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, diagFl1, diagFl2, diagFl3, diagSh1, diagSh2, diagSh3); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE DIAGNOSTIC UNPACKING - -/** - * @brief Get field diagFl1 from diagnostic message - * - * @return Diagnostic float 1 - */ -static inline float mavlink_msg_diagnostic_get_diagFl1(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; -} - -/** - * @brief Get field diagFl2 from diagnostic message - * - * @return Diagnostic float 2 - */ -static inline float mavlink_msg_diagnostic_get_diagFl2(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field diagFl3 from diagnostic message - * - * @return Diagnostic float 3 - */ -static inline float mavlink_msg_diagnostic_get_diagFl3(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field diagSh1 from diagnostic message - * - * @return Diagnostic short 1 - */ -static inline int16_t mavlink_msg_diagnostic_get_diagSh1(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - return (int16_t)r.s; -} - -/** - * @brief Get field diagSh2 from diagnostic message - * - * @return Diagnostic short 2 - */ -static inline int16_t mavlink_msg_diagnostic_get_diagSh2(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int16_t))[1]; - return (int16_t)r.s; -} - -/** - * @brief Get field diagSh3 from diagnostic message - * - * @return Diagnostic short 3 - */ -static inline int16_t mavlink_msg_diagnostic_get_diagSh3(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; -} - -/** - * @brief Decode a diagnostic message into a struct - * - * @param msg The message to decode - * @param diagnostic C-struct to decode the message contents into - */ -static inline void mavlink_msg_diagnostic_decode(const mavlink_message_t* msg, mavlink_diagnostic_t* diagnostic) -{ - diagnostic->diagFl1 = mavlink_msg_diagnostic_get_diagFl1(msg); - diagnostic->diagFl2 = mavlink_msg_diagnostic_get_diagFl2(msg); - diagnostic->diagFl3 = mavlink_msg_diagnostic_get_diagFl3(msg); - diagnostic->diagSh1 = mavlink_msg_diagnostic_get_diagSh1(msg); - diagnostic->diagSh2 = mavlink_msg_diagnostic_get_diagSh2(msg); - diagnostic->diagSh3 = mavlink_msg_diagnostic_get_diagSh3(msg); -} diff --git a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_filtered_data.h b/libraries/GCS_MAVLink/include/slugs/mavlink_msg_filtered_data.h deleted file mode 100644 index 0384935c9d..0000000000 --- a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_filtered_data.h +++ /dev/null @@ -1,216 +0,0 @@ -// MESSAGE FILTERED_DATA PACKING - -#define MAVLINK_MSG_ID_FILTERED_DATA 178 - -typedef struct __mavlink_filtered_data_t -{ - float aX; ///< Accelerometer X value (m/s^2) - float aY; ///< Accelerometer Y value (m/s^2) - float aZ; ///< Accelerometer Z value (m/s^2) - float gX; ///< Gyro X value (rad/s) - float gY; ///< Gyro Y value (rad/s) - float gZ; ///< Gyro Z value (rad/s) - float mX; ///< Magnetometer X (normalized to 1) - float mY; ///< Magnetometer Y (normalized to 1) - float mZ; ///< Magnetometer Z (normalized to 1) - -} mavlink_filtered_data_t; - - - -/** - * @brief Send a filtered_data message - * - * @param aX Accelerometer X value (m/s^2) - * @param aY Accelerometer Y value (m/s^2) - * @param aZ Accelerometer Z value (m/s^2) - * @param gX Gyro X value (rad/s) - * @param gY Gyro Y value (rad/s) - * @param gZ Gyro Z value (rad/s) - * @param mX Magnetometer X (normalized to 1) - * @param mY Magnetometer Y (normalized to 1) - * @param mZ Magnetometer Z (normalized to 1) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_filtered_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float aX, float aY, float aZ, float gX, float gY, float gZ, float mX, float mY, float mZ) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_FILTERED_DATA; - - i += put_float_by_index(aX, i, msg->payload); //Accelerometer X value (m/s^2) - i += put_float_by_index(aY, i, msg->payload); //Accelerometer Y value (m/s^2) - i += put_float_by_index(aZ, i, msg->payload); //Accelerometer Z value (m/s^2) - i += put_float_by_index(gX, i, msg->payload); //Gyro X value (rad/s) - i += put_float_by_index(gY, i, msg->payload); //Gyro Y value (rad/s) - i += put_float_by_index(gZ, i, msg->payload); //Gyro Z value (rad/s) - i += put_float_by_index(mX, i, msg->payload); //Magnetometer X (normalized to 1) - i += put_float_by_index(mY, i, msg->payload); //Magnetometer Y (normalized to 1) - i += put_float_by_index(mZ, i, msg->payload); //Magnetometer Z (normalized to 1) - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -static inline uint16_t mavlink_msg_filtered_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_filtered_data_t* filtered_data) -{ - return mavlink_msg_filtered_data_pack(system_id, component_id, msg, filtered_data->aX, filtered_data->aY, filtered_data->aZ, filtered_data->gX, filtered_data->gY, filtered_data->gZ, filtered_data->mX, filtered_data->mY, filtered_data->mZ); -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_filtered_data_send(mavlink_channel_t chan, float aX, float aY, float aZ, float gX, float gY, float gZ, float mX, float mY, float mZ) -{ - mavlink_message_t msg; - mavlink_msg_filtered_data_pack(mavlink_system.sysid, mavlink_system.compid, &msg, aX, aY, aZ, gX, gY, gZ, mX, mY, mZ); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE FILTERED_DATA UNPACKING - -/** - * @brief Get field aX from filtered_data message - * - * @return Accelerometer X value (m/s^2) - */ -static inline float mavlink_msg_filtered_data_get_aX(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; -} - -/** - * @brief Get field aY from filtered_data message - * - * @return Accelerometer Y value (m/s^2) - */ -static inline float mavlink_msg_filtered_data_get_aY(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field aZ from filtered_data message - * - * @return Accelerometer Z value (m/s^2) - */ -static inline float mavlink_msg_filtered_data_get_aZ(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field gX from filtered_data message - * - * @return Gyro X value (rad/s) - */ -static inline float mavlink_msg_filtered_data_get_gX(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field gY from filtered_data message - * - * @return Gyro Y value (rad/s) - */ -static inline float mavlink_msg_filtered_data_get_gY(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field gZ from filtered_data message - * - * @return Gyro Z value (rad/s) - */ -static inline float mavlink_msg_filtered_data_get_gZ(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field mX from filtered_data message - * - * @return Magnetometer X (normalized to 1) - */ -static inline float mavlink_msg_filtered_data_get_mX(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field mY from filtered_data message - * - * @return Magnetometer Y (normalized to 1) - */ -static inline float mavlink_msg_filtered_data_get_mY(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field mZ from filtered_data message - * - * @return Magnetometer Z (normalized to 1) - */ -static inline float mavlink_msg_filtered_data_get_mZ(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -static inline void mavlink_msg_filtered_data_decode(const mavlink_message_t* msg, mavlink_filtered_data_t* filtered_data) -{ - filtered_data->aX = mavlink_msg_filtered_data_get_aX(msg); - filtered_data->aY = mavlink_msg_filtered_data_get_aY(msg); - filtered_data->aZ = mavlink_msg_filtered_data_get_aZ(msg); - filtered_data->gX = mavlink_msg_filtered_data_get_gX(msg); - filtered_data->gY = mavlink_msg_filtered_data_get_gY(msg); - filtered_data->gZ = mavlink_msg_filtered_data_get_gZ(msg); - filtered_data->mX = mavlink_msg_filtered_data_get_mX(msg); - filtered_data->mY = mavlink_msg_filtered_data_get_mY(msg); - filtered_data->mZ = mavlink_msg_filtered_data_get_mZ(msg); -} diff --git a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_gps_date_time.h b/libraries/GCS_MAVLink/include/slugs/mavlink_msg_gps_date_time.h deleted file mode 100644 index 199cbb9ec2..0000000000 --- a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_gps_date_time.h +++ /dev/null @@ -1,203 +0,0 @@ -// MESSAGE GPS_DATE_TIME PACKING - -#define MAVLINK_MSG_ID_GPS_DATE_TIME 179 - -typedef struct __mavlink_gps_date_time_t -{ - uint8_t year; ///< Year reported by Gps - uint8_t month; ///< Month reported by Gps - uint8_t day; ///< Day reported by Gps - uint8_t hour; ///< Hour reported by Gps - uint8_t min; ///< Min reported by Gps - uint8_t sec; ///< Sec reported by Gps - uint8_t visSat; ///< Visible sattelites reported by Gps - -} mavlink_gps_date_time_t; - - - -/** - * @brief Pack a gps_date_time 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 year Year reported by Gps - * @param month Month reported by Gps - * @param day Day reported by Gps - * @param hour Hour reported by Gps - * @param min Min reported by Gps - * @param sec Sec reported by Gps - * @param visSat Visible sattelites reported by Gps - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps_date_time_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec, uint8_t visSat) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; - - i += put_uint8_t_by_index(year, i, msg->payload); // Year reported by Gps - i += put_uint8_t_by_index(month, i, msg->payload); // Month reported by Gps - i += put_uint8_t_by_index(day, i, msg->payload); // Day reported by Gps - i += put_uint8_t_by_index(hour, i, msg->payload); // Hour reported by Gps - i += put_uint8_t_by_index(min, i, msg->payload); // Min reported by Gps - i += put_uint8_t_by_index(sec, i, msg->payload); // Sec reported by Gps - i += put_uint8_t_by_index(visSat, i, msg->payload); // Visible sattelites reported by Gps - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a gps_date_time message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param year Year reported by Gps - * @param month Month reported by Gps - * @param day Day reported by Gps - * @param hour Hour reported by Gps - * @param min Min reported by Gps - * @param sec Sec reported by Gps - * @param visSat Visible sattelites reported by Gps - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps_date_time_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec, uint8_t visSat) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; - - i += put_uint8_t_by_index(year, i, msg->payload); // Year reported by Gps - i += put_uint8_t_by_index(month, i, msg->payload); // Month reported by Gps - i += put_uint8_t_by_index(day, i, msg->payload); // Day reported by Gps - i += put_uint8_t_by_index(hour, i, msg->payload); // Hour reported by Gps - i += put_uint8_t_by_index(min, i, msg->payload); // Min reported by Gps - i += put_uint8_t_by_index(sec, i, msg->payload); // Sec reported by Gps - i += put_uint8_t_by_index(visSat, i, msg->payload); // Visible sattelites reported by Gps - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a gps_date_time struct into 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 gps_date_time C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_gps_date_time_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_date_time_t* gps_date_time) -{ - return mavlink_msg_gps_date_time_pack(system_id, component_id, msg, gps_date_time->year, gps_date_time->month, gps_date_time->day, gps_date_time->hour, gps_date_time->min, gps_date_time->sec, gps_date_time->visSat); -} - -/** - * @brief Send a gps_date_time message - * @param chan MAVLink channel to send the message - * - * @param year Year reported by Gps - * @param month Month reported by Gps - * @param day Day reported by Gps - * @param hour Hour reported by Gps - * @param min Min reported by Gps - * @param sec Sec reported by Gps - * @param visSat Visible sattelites reported by Gps - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_gps_date_time_send(mavlink_channel_t chan, uint8_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec, uint8_t visSat) -{ - mavlink_message_t msg; - mavlink_msg_gps_date_time_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, year, month, day, hour, min, sec, visSat); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE GPS_DATE_TIME UNPACKING - -/** - * @brief Get field year from gps_date_time message - * - * @return Year reported by Gps - */ -static inline uint8_t mavlink_msg_gps_date_time_get_year(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field month from gps_date_time message - * - * @return Month reported by Gps - */ -static inline uint8_t mavlink_msg_gps_date_time_get_month(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field day from gps_date_time message - * - * @return Day reported by Gps - */ -static inline uint8_t mavlink_msg_gps_date_time_get_day(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field hour from gps_date_time message - * - * @return Hour reported by Gps - */ -static inline uint8_t mavlink_msg_gps_date_time_get_hour(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field min from gps_date_time message - * - * @return Min reported by Gps - */ -static inline uint8_t mavlink_msg_gps_date_time_get_min(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field sec from gps_date_time message - * - * @return Sec reported by Gps - */ -static inline uint8_t mavlink_msg_gps_date_time_get_sec(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field visSat from gps_date_time message - * - * @return Visible sattelites reported by Gps - */ -static inline uint8_t mavlink_msg_gps_date_time_get_visSat(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -/** - * @brief Decode a gps_date_time message into a struct - * - * @param msg The message to decode - * @param gps_date_time C-struct to decode the message contents into - */ -static inline void mavlink_msg_gps_date_time_decode(const mavlink_message_t* msg, mavlink_gps_date_time_t* gps_date_time) -{ - gps_date_time->year = mavlink_msg_gps_date_time_get_year(msg); - gps_date_time->month = mavlink_msg_gps_date_time_get_month(msg); - gps_date_time->day = mavlink_msg_gps_date_time_get_day(msg); - gps_date_time->hour = mavlink_msg_gps_date_time_get_hour(msg); - gps_date_time->min = mavlink_msg_gps_date_time_get_min(msg); - gps_date_time->sec = mavlink_msg_gps_date_time_get_sec(msg); - gps_date_time->visSat = mavlink_msg_gps_date_time_get_visSat(msg); -} diff --git a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_mid_lvl_cmds.h b/libraries/GCS_MAVLink/include/slugs/mavlink_msg_mid_lvl_cmds.h deleted file mode 100644 index a0d78b1ceb..0000000000 --- a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_mid_lvl_cmds.h +++ /dev/null @@ -1,167 +0,0 @@ -// MESSAGE MID_LVL_CMDS PACKING - -#define MAVLINK_MSG_ID_MID_LVL_CMDS 180 - -typedef struct __mavlink_mid_lvl_cmds_t -{ - uint8_t target; ///< The system setting the commands - float hCommand; ///< Commanded Airspeed - float uCommand; ///< Log value 2 - float rCommand; ///< Log value 3 - -} mavlink_mid_lvl_cmds_t; - - - -/** - * @brief Pack a mid_lvl_cmds 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 The system setting the commands - * @param hCommand Commanded Airspeed - * @param uCommand Log value 2 - * @param rCommand Log value 3 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mid_lvl_cmds_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, float hCommand, float uCommand, float rCommand) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; - - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the commands - i += put_float_by_index(hCommand, i, msg->payload); // Commanded Airspeed - i += put_float_by_index(uCommand, i, msg->payload); // Log value 2 - i += put_float_by_index(rCommand, i, msg->payload); // Log value 3 - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a mid_lvl_cmds message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target The system setting the commands - * @param hCommand Commanded Airspeed - * @param uCommand Log value 2 - * @param rCommand Log value 3 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mid_lvl_cmds_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, float hCommand, float uCommand, float rCommand) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; - - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the commands - i += put_float_by_index(hCommand, i, msg->payload); // Commanded Airspeed - i += put_float_by_index(uCommand, i, msg->payload); // Log value 2 - i += put_float_by_index(rCommand, i, msg->payload); // Log value 3 - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a mid_lvl_cmds struct into 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 mid_lvl_cmds C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mid_lvl_cmds_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mid_lvl_cmds_t* mid_lvl_cmds) -{ - return mavlink_msg_mid_lvl_cmds_pack(system_id, component_id, msg, mid_lvl_cmds->target, mid_lvl_cmds->hCommand, mid_lvl_cmds->uCommand, mid_lvl_cmds->rCommand); -} - -/** - * @brief Send a mid_lvl_cmds message - * @param chan MAVLink channel to send the message - * - * @param target The system setting the commands - * @param hCommand Commanded Airspeed - * @param uCommand Log value 2 - * @param rCommand Log value 3 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_mid_lvl_cmds_send(mavlink_channel_t chan, uint8_t target, float hCommand, float uCommand, float rCommand) -{ - mavlink_message_t msg; - mavlink_msg_mid_lvl_cmds_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, hCommand, uCommand, rCommand); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE MID_LVL_CMDS UNPACKING - -/** - * @brief Get field target from mid_lvl_cmds message - * - * @return The system setting the commands - */ -static inline uint8_t mavlink_msg_mid_lvl_cmds_get_target(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field hCommand from mid_lvl_cmds message - * - * @return Commanded Airspeed - */ -static inline float mavlink_msg_mid_lvl_cmds_get_hCommand(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (float)r.f; -} - -/** - * @brief Get field uCommand from mid_lvl_cmds message - * - * @return Log value 2 - */ -static inline float mavlink_msg_mid_lvl_cmds_get_uCommand(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field rCommand from mid_lvl_cmds message - * - * @return Log value 3 - */ -static inline float mavlink_msg_mid_lvl_cmds_get_rCommand(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Decode a mid_lvl_cmds message into a struct - * - * @param msg The message to decode - * @param mid_lvl_cmds C-struct to decode the message contents into - */ -static inline void mavlink_msg_mid_lvl_cmds_decode(const mavlink_message_t* msg, mavlink_mid_lvl_cmds_t* mid_lvl_cmds) -{ - mid_lvl_cmds->target = mavlink_msg_mid_lvl_cmds_get_target(msg); - mid_lvl_cmds->hCommand = mavlink_msg_mid_lvl_cmds_get_hCommand(msg); - mid_lvl_cmds->uCommand = mavlink_msg_mid_lvl_cmds_get_uCommand(msg); - mid_lvl_cmds->rCommand = mavlink_msg_mid_lvl_cmds_get_rCommand(msg); -} diff --git a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_pid.h b/libraries/GCS_MAVLink/include/slugs/mavlink_msg_pid.h deleted file mode 100644 index a75a475372..0000000000 --- a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_pid.h +++ /dev/null @@ -1,130 +0,0 @@ -// MESSAGE PID PACKING - -#define MAVLINK_MSG_ID_PID 182 - -typedef struct __mavlink_pid_t -{ - uint8_t target; ///< The system setting the PID values - float pVal; ///< Proportional gain - float iVal; ///< Integral gain - float dVal; ///< Derivative gain - uint8_t idx; ///< PID loop index - -} mavlink_pid_t; - - - -/** - * @brief Send a pid message - * - * @param target The system setting the PID values - * @param pVal Proportional gain - * @param iVal Integral gain - * @param dVal Derivative gain - * @param idx PID loop index - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_pid_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, float pVal, float iVal, float dVal, uint8_t idx) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_PID; - - i += put_uint8_t_by_index(target, i, msg->payload); //The system setting the PID values - i += put_float_by_index(pVal, i, msg->payload); //Proportional gain - i += put_float_by_index(iVal, i, msg->payload); //Integral gain - i += put_float_by_index(dVal, i, msg->payload); //Derivative gain - i += put_uint8_t_by_index(idx, i, msg->payload); //PID loop index - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -static inline uint16_t mavlink_msg_pid_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_pid_t* pid) -{ - return mavlink_msg_pid_pack(system_id, component_id, msg, pid->target, pid->pVal, pid->iVal, pid->dVal, pid->idx); -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_pid_send(mavlink_channel_t chan, uint8_t target, float pVal, float iVal, float dVal, uint8_t idx) -{ - mavlink_message_t msg; - mavlink_msg_pid_pack(mavlink_system.sysid, mavlink_system.compid, &msg, target, pVal, iVal, dVal, idx); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE PID UNPACKING - -/** - * @brief Get field target from pid message - * - * @return The system setting the PID values - */ -static inline uint8_t mavlink_msg_pid_get_target(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field pVal from pid message - * - * @return Proportional gain - */ -static inline float mavlink_msg_pid_get_pVal(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (float)r.f; -} - -/** - * @brief Get field iVal from pid message - * - * @return Integral gain - */ -static inline float mavlink_msg_pid_get_iVal(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field dVal from pid message - * - * @return Derivative gain - */ -static inline float mavlink_msg_pid_get_dVal(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field idx from pid message - * - * @return PID loop index - */ -static inline uint8_t mavlink_msg_pid_get_idx(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; -} - -static inline void mavlink_msg_pid_decode(const mavlink_message_t* msg, mavlink_pid_t* pid) -{ - pid->target = mavlink_msg_pid_get_target(msg); - pid->pVal = mavlink_msg_pid_get_pVal(msg); - pid->iVal = mavlink_msg_pid_get_iVal(msg); - pid->dVal = mavlink_msg_pid_get_dVal(msg); - pid->idx = mavlink_msg_pid_get_idx(msg); -} diff --git a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_pilot_console.h b/libraries/GCS_MAVLink/include/slugs/mavlink_msg_pilot_console.h deleted file mode 100644 index a46a099277..0000000000 --- a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_pilot_console.h +++ /dev/null @@ -1,145 +0,0 @@ -// MESSAGE PILOT_CONSOLE PACKING - -#define MAVLINK_MSG_ID_PILOT_CONSOLE 174 - -typedef struct __mavlink_pilot_console_t -{ - uint16_t dt; ///< Pilot's console throttle command - uint16_t dla; ///< Pilot's console left aileron command - uint16_t dra; ///< Pilot's console right aileron command - uint16_t dr; ///< Pilot's console rudder command - uint16_t de; ///< Pilot's console elevator command - -} mavlink_pilot_console_t; - - - -/** - * @brief Send a pilot_console message - * - * @param dt Pilot's console throttle command - * @param dla Pilot's console left aileron command - * @param dra Pilot's console right aileron command - * @param dr Pilot's console rudder command - * @param de Pilot's console elevator command - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_pilot_console_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t dt, uint16_t dla, uint16_t dra, uint16_t dr, uint16_t de) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_PILOT_CONSOLE; - - i += put_uint16_t_by_index(dt, i, msg->payload); //Pilot's console throttle command - i += put_uint16_t_by_index(dla, i, msg->payload); //Pilot's console left aileron command - i += put_uint16_t_by_index(dra, i, msg->payload); //Pilot's console right aileron command - i += put_uint16_t_by_index(dr, i, msg->payload); //Pilot's console rudder command - i += put_uint16_t_by_index(de, i, msg->payload); //Pilot's console elevator command - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -static inline uint16_t mavlink_msg_pilot_console_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint16_t dt, uint16_t dla, uint16_t dra, uint16_t dr, uint16_t de) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_PILOT_CONSOLE; - - i += put_uint8_t_by_index(target, i, msg->payload); //The system reporting the diagnostic - i += put_uint16_t_by_index(dt, i, msg->payload); //Pilot's console throttle command - i += put_uint16_t_by_index(dla, i, msg->payload); //Pilot's console left aileron command - i += put_uint16_t_by_index(dra, i, msg->payload); //Pilot's console right aileron command - i += put_uint16_t_by_index(dr, i, msg->payload); //Pilot's console rudder command - i += put_uint16_t_by_index(de, i, msg->payload); //Pilot's console elevator command - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -static inline uint16_t mavlink_msg_pilot_console_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_pilot_console_t* pilot_console) -{ - return mavlink_msg_pilot_console_pack(system_id, component_id, msg, pilot_console->dt, pilot_console->dla, pilot_console->dra, pilot_console->dr, pilot_console->de); -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_pilot_console_send(mavlink_channel_t chan, uint16_t dt, uint16_t dla, uint16_t dra, uint16_t dr, uint16_t de) -{ - mavlink_message_t msg; - mavlink_msg_pilot_console_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, dt, dla, dra, dr, de); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE PILOT_CONSOLE UNPACKING - -/** - * @brief Get field dt from pilot_console message - * - * @return Pilot's console throttle command - */ -static inline uint16_t mavlink_msg_pilot_console_get_dt(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field dla from pilot_console message - * - * @return Pilot's console left aileron command - */ -static inline uint16_t mavlink_msg_pilot_console_get_dla(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field dra from pilot_console message - * - * @return Pilot's console right aileron command - */ -static inline uint16_t mavlink_msg_pilot_console_get_dra(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field dr from pilot_console message - * - * @return Pilot's console rudder command - */ -static inline uint16_t mavlink_msg_pilot_console_get_dr(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field de from pilot_console message - * - * @return Pilot's console elevator command - */ -static inline uint16_t mavlink_msg_pilot_console_get_de(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -static inline void mavlink_msg_pilot_console_decode(const mavlink_message_t* msg, mavlink_pilot_console_t* pilot_console) -{ - pilot_console->dt = mavlink_msg_pilot_console_get_dt(msg); - pilot_console->dla = mavlink_msg_pilot_console_get_dla(msg); - pilot_console->dra = mavlink_msg_pilot_console_get_dra(msg); - pilot_console->dr = mavlink_msg_pilot_console_get_dr(msg); - pilot_console->de = mavlink_msg_pilot_console_get_de(msg); -} diff --git a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_pwm_commands.h b/libraries/GCS_MAVLink/include/slugs/mavlink_msg_pwm_commands.h deleted file mode 100644 index 6c5b08d187..0000000000 --- a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_pwm_commands.h +++ /dev/null @@ -1,235 +0,0 @@ -// MESSAGE PWM_COMMANDS PACKING - -#define MAVLINK_MSG_ID_PWM_COMMANDS 175 - -typedef struct __mavlink_pwm_commands_t -{ - uint16_t dt_c; ///< AutoPilot's throttle command - uint16_t dla_c; ///< AutoPilot's left aileron command - uint16_t dra_c; ///< AutoPilot's right aileron command - uint16_t dr_c; ///< AutoPilot's rudder command - uint16_t dle_c; ///< AutoPilot's left elevator command - uint16_t dre_c; ///< AutoPilot's right elevator command - uint16_t dlf_c; ///< AutoPilot's left flap command - uint16_t drf_c; ///< AutoPilot's right flap command - uint16_t aux1; ///< AutoPilot's aux1 command - uint16_t aux2; ///< AutoPilot's aux2 command - -} mavlink_pwm_commands_t; - - - -/** - * @brief Send a pwm_commands message - * - * @param dt_c AutoPilot's throttle command - * @param dla_c AutoPilot's left aileron command - * @param dra_c AutoPilot's right aileron command - * @param dr_c AutoPilot's rudder command - * @param dle_c AutoPilot's left elevator command - * @param dre_c AutoPilot's right elevator command - * @param dlf_c AutoPilot's left flap command - * @param drf_c AutoPilot's right flap command - * @param aux1 AutoPilot's aux1 command - * @param aux2 AutoPilot's aux2 command - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_pwm_commands_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t dt_c, uint16_t dla_c, uint16_t dra_c, uint16_t dr_c, uint16_t dle_c, uint16_t dre_c, uint16_t dlf_c, uint16_t drf_c, uint16_t aux1, uint16_t aux2) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_PWM_COMMANDS; - - i += put_uint16_t_by_index(dt_c, i, msg->payload); //AutoPilot's throttle command - i += put_uint16_t_by_index(dla_c, i, msg->payload); //AutoPilot's left aileron command - i += put_uint16_t_by_index(dra_c, i, msg->payload); //AutoPilot's right aileron command - i += put_uint16_t_by_index(dr_c, i, msg->payload); //AutoPilot's rudder command - i += put_uint16_t_by_index(dle_c, i, msg->payload); //AutoPilot's left elevator command - i += put_uint16_t_by_index(dre_c, i, msg->payload); //AutoPilot's right elevator command - i += put_uint16_t_by_index(dlf_c, i, msg->payload); //AutoPilot's left flap command - i += put_uint16_t_by_index(drf_c, i, msg->payload); //AutoPilot's right flap command - i += put_uint16_t_by_index(aux1, i, msg->payload); //AutoPilot's aux1 command - i += put_uint16_t_by_index(aux2, i, msg->payload); //AutoPilot's aux2 command - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -static inline uint16_t mavlink_msg_pwm_commands_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint16_t dt_c, uint16_t dla_c, uint16_t dra_c, uint16_t dr_c, uint16_t dle_c, uint16_t dre_c, uint16_t dlf_c, uint16_t drf_c, uint16_t aux1, uint16_t aux2) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_PWM_COMMANDS; - - i += put_uint8_t_by_index(target, i, msg->payload); //The system reporting the diagnostic - i += put_uint16_t_by_index(dt_c, i, msg->payload); //AutoPilot's throttle command - i += put_uint16_t_by_index(dla_c, i, msg->payload); //AutoPilot's left aileron command - i += put_uint16_t_by_index(dra_c, i, msg->payload); //AutoPilot's right aileron command - i += put_uint16_t_by_index(dr_c, i, msg->payload); //AutoPilot's rudder command - i += put_uint16_t_by_index(dle_c, i, msg->payload); //AutoPilot's left elevator command - i += put_uint16_t_by_index(dre_c, i, msg->payload); //AutoPilot's right elevator command - i += put_uint16_t_by_index(dlf_c, i, msg->payload); //AutoPilot's left flap command - i += put_uint16_t_by_index(drf_c, i, msg->payload); //AutoPilot's right flap command - i += put_uint16_t_by_index(aux1, i, msg->payload); //AutoPilot's aux1 command - i += put_uint16_t_by_index(aux2, i, msg->payload); //AutoPilot's aux2 command - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -static inline uint16_t mavlink_msg_pwm_commands_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_pwm_commands_t* pwm_commands) -{ - return mavlink_msg_pwm_commands_pack(system_id, component_id, msg, pwm_commands->dt_c, pwm_commands->dla_c, pwm_commands->dra_c, pwm_commands->dr_c, pwm_commands->dle_c, pwm_commands->dre_c, pwm_commands->dlf_c, pwm_commands->drf_c, pwm_commands->aux1, pwm_commands->aux2); -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_pwm_commands_send(mavlink_channel_t chan, uint16_t dt_c, uint16_t dla_c, uint16_t dra_c, uint16_t dr_c, uint16_t dle_c, uint16_t dre_c, uint16_t dlf_c, uint16_t drf_c, uint16_t aux1, uint16_t aux2) -{ - mavlink_message_t msg; - mavlink_msg_pwm_commands_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, dt_c, dla_c, dra_c, dr_c, dle_c, dre_c, dlf_c, drf_c, aux1, aux2); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE PWM_COMMANDS UNPACKING - -/** - * @brief Get field dt_c from pwm_commands message - * - * @return AutoPilot's throttle command - */ -static inline uint16_t mavlink_msg_pwm_commands_get_dt_c(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field dla_c from pwm_commands message - * - * @return AutoPilot's left aileron command - */ -static inline uint16_t mavlink_msg_pwm_commands_get_dla_c(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field dra_c from pwm_commands message - * - * @return AutoPilot's right aileron command - */ -static inline uint16_t mavlink_msg_pwm_commands_get_dra_c(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field dr_c from pwm_commands message - * - * @return AutoPilot's rudder command - */ -static inline uint16_t mavlink_msg_pwm_commands_get_dr_c(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field dle_c from pwm_commands message - * - * @return AutoPilot's left elevator command - */ -static inline uint16_t mavlink_msg_pwm_commands_get_dle_c(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field dre_c from pwm_commands message - * - * @return AutoPilot's right elevator command - */ -static inline uint16_t mavlink_msg_pwm_commands_get_dre_c(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field dlf_c from pwm_commands message - * - * @return AutoPilot's left flap command - */ -static inline uint16_t mavlink_msg_pwm_commands_get_dlf_c(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field drf_c from pwm_commands message - * - * @return AutoPilot's right flap command - */ -static inline uint16_t mavlink_msg_pwm_commands_get_drf_c(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field aux1 from pwm_commands message - * - * @return AutoPilot's aux1 command - */ -static inline uint16_t mavlink_msg_pwm_commands_get_aux1(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Get field aux2 from pwm_commands message - * - * @return AutoPilot's aux2 command - */ -static inline uint16_t mavlink_msg_pwm_commands_get_aux2(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; -} - -static inline void mavlink_msg_pwm_commands_decode(const mavlink_message_t* msg, mavlink_pwm_commands_t* pwm_commands) -{ - pwm_commands->dt_c = mavlink_msg_pwm_commands_get_dt_c(msg); - pwm_commands->dla_c = mavlink_msg_pwm_commands_get_dla_c(msg); - pwm_commands->dra_c = mavlink_msg_pwm_commands_get_dra_c(msg); - pwm_commands->dr_c = mavlink_msg_pwm_commands_get_dr_c(msg); - pwm_commands->dle_c = mavlink_msg_pwm_commands_get_dle_c(msg); - pwm_commands->dre_c = mavlink_msg_pwm_commands_get_dre_c(msg); - pwm_commands->dlf_c = mavlink_msg_pwm_commands_get_dlf_c(msg); - pwm_commands->drf_c = mavlink_msg_pwm_commands_get_drf_c(msg); - pwm_commands->aux1 = mavlink_msg_pwm_commands_get_aux1(msg); - pwm_commands->aux2 = mavlink_msg_pwm_commands_get_aux2(msg); -} diff --git a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_sensor_bias.h b/libraries/GCS_MAVLink/include/slugs/mavlink_msg_sensor_bias.h deleted file mode 100644 index 8b40bdd67a..0000000000 --- a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_sensor_bias.h +++ /dev/null @@ -1,216 +0,0 @@ -// MESSAGE SENSOR_BIAS PACKING - -#define MAVLINK_MSG_ID_SENSOR_BIAS 172 - -typedef struct __mavlink_sensor_bias_t -{ - float axBias; ///< Accelerometer X bias (m/s) - float ayBias; ///< Accelerometer Y bias (m/s) - float azBias; ///< Accelerometer Z bias (m/s) - float gxBias; ///< Gyro X bias (rad/s) - float gyBias; ///< Gyro Y bias (rad/s) - float gzBias; ///< Gyro Z bias (rad/s) - -} mavlink_sensor_bias_t; - - - -/** - * @brief Pack a sensor_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 axBias Accelerometer X bias (m/s) - * @param ayBias Accelerometer Y bias (m/s) - * @param azBias Accelerometer Z bias (m/s) - * @param gxBias Gyro X bias (rad/s) - * @param gyBias Gyro Y bias (rad/s) - * @param gzBias Gyro Z bias (rad/s) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_sensor_bias_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float axBias, float ayBias, float azBias, float gxBias, float gyBias, float gzBias) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS; - - i += put_float_by_index(axBias, i, msg->payload); // Accelerometer X bias (m/s) - i += put_float_by_index(ayBias, i, msg->payload); // Accelerometer Y bias (m/s) - i += put_float_by_index(azBias, i, msg->payload); // Accelerometer Z bias (m/s) - i += put_float_by_index(gxBias, i, msg->payload); // Gyro X bias (rad/s) - i += put_float_by_index(gyBias, i, msg->payload); // Gyro Y bias (rad/s) - i += put_float_by_index(gzBias, i, msg->payload); // Gyro Z bias (rad/s) - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a sensor_bias message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param axBias Accelerometer X bias (m/s) - * @param ayBias Accelerometer Y bias (m/s) - * @param azBias Accelerometer Z bias (m/s) - * @param gxBias Gyro X bias (rad/s) - * @param gyBias Gyro Y bias (rad/s) - * @param gzBias Gyro Z bias (rad/s) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_sensor_bias_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float axBias, float ayBias, float azBias, float gxBias, float gyBias, float gzBias) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS; - - i += put_float_by_index(axBias, i, msg->payload); // Accelerometer X bias (m/s) - i += put_float_by_index(ayBias, i, msg->payload); // Accelerometer Y bias (m/s) - i += put_float_by_index(azBias, i, msg->payload); // Accelerometer Z bias (m/s) - i += put_float_by_index(gxBias, i, msg->payload); // Gyro X bias (rad/s) - i += put_float_by_index(gyBias, i, msg->payload); // Gyro Y bias (rad/s) - i += put_float_by_index(gzBias, i, msg->payload); // Gyro Z bias (rad/s) - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a sensor_bias struct into 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 sensor_bias C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_sensor_bias_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sensor_bias_t* sensor_bias) -{ - return mavlink_msg_sensor_bias_pack(system_id, component_id, msg, sensor_bias->axBias, sensor_bias->ayBias, sensor_bias->azBias, sensor_bias->gxBias, sensor_bias->gyBias, sensor_bias->gzBias); -} - -/** - * @brief Send a sensor_bias message - * @param chan MAVLink channel to send the message - * - * @param axBias Accelerometer X bias (m/s) - * @param ayBias Accelerometer Y bias (m/s) - * @param azBias Accelerometer Z bias (m/s) - * @param gxBias Gyro X bias (rad/s) - * @param gyBias Gyro Y bias (rad/s) - * @param gzBias Gyro Z bias (rad/s) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_sensor_bias_send(mavlink_channel_t chan, float axBias, float ayBias, float azBias, float gxBias, float gyBias, float gzBias) -{ - mavlink_message_t msg; - mavlink_msg_sensor_bias_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, axBias, ayBias, azBias, gxBias, gyBias, gzBias); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE SENSOR_BIAS UNPACKING - -/** - * @brief Get field axBias from sensor_bias message - * - * @return Accelerometer X bias (m/s) - */ -static inline float mavlink_msg_sensor_bias_get_axBias(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; -} - -/** - * @brief Get field ayBias from sensor_bias message - * - * @return Accelerometer Y bias (m/s) - */ -static inline float mavlink_msg_sensor_bias_get_ayBias(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field azBias from sensor_bias message - * - * @return Accelerometer Z bias (m/s) - */ -static inline float mavlink_msg_sensor_bias_get_azBias(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field gxBias from sensor_bias message - * - * @return Gyro X bias (rad/s) - */ -static inline float mavlink_msg_sensor_bias_get_gxBias(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field gyBias from sensor_bias message - * - * @return Gyro Y bias (rad/s) - */ -static inline float mavlink_msg_sensor_bias_get_gyBias(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field gzBias from sensor_bias message - * - * @return Gyro Z bias (rad/s) - */ -static inline float mavlink_msg_sensor_bias_get_gzBias(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Decode a sensor_bias message into a struct - * - * @param msg The message to decode - * @param sensor_bias C-struct to decode the message contents into - */ -static inline void mavlink_msg_sensor_bias_decode(const mavlink_message_t* msg, mavlink_sensor_bias_t* sensor_bias) -{ - sensor_bias->axBias = mavlink_msg_sensor_bias_get_axBias(msg); - sensor_bias->ayBias = mavlink_msg_sensor_bias_get_ayBias(msg); - sensor_bias->azBias = mavlink_msg_sensor_bias_get_azBias(msg); - sensor_bias->gxBias = mavlink_msg_sensor_bias_get_gxBias(msg); - sensor_bias->gyBias = mavlink_msg_sensor_bias_get_gyBias(msg); - sensor_bias->gzBias = mavlink_msg_sensor_bias_get_gzBias(msg); -} diff --git a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_slugs_action.h b/libraries/GCS_MAVLink/include/slugs/mavlink_msg_slugs_action.h deleted file mode 100644 index 0664166567..0000000000 --- a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_slugs_action.h +++ /dev/null @@ -1,138 +0,0 @@ -// MESSAGE SLUGS_ACTION PACKING - -#define MAVLINK_MSG_ID_SLUGS_ACTION 183 - -typedef struct __mavlink_slugs_action_t -{ - uint8_t target; ///< The system reporting the action - uint8_t actionId; ///< Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - uint16_t actionVal; ///< Value associated with the action - -} mavlink_slugs_action_t; - - - -/** - * @brief Pack a slugs_action 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 The system reporting the action - * @param actionId Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - * @param actionVal Value associated with the action - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_slugs_action_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, uint8_t actionId, uint16_t actionVal) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_SLUGS_ACTION; - - i += put_uint8_t_by_index(target, i, msg->payload); // The system reporting the action - i += put_uint8_t_by_index(actionId, i, msg->payload); // Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - i += put_uint16_t_by_index(actionVal, i, msg->payload); // Value associated with the action - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a slugs_action message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target The system reporting the action - * @param actionId Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - * @param actionVal Value associated with the action - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_slugs_action_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint8_t actionId, uint16_t actionVal) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_SLUGS_ACTION; - - i += put_uint8_t_by_index(target, i, msg->payload); // The system reporting the action - i += put_uint8_t_by_index(actionId, i, msg->payload); // Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - i += put_uint16_t_by_index(actionVal, i, msg->payload); // Value associated with the action - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a slugs_action struct into 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 slugs_action C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_slugs_action_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_slugs_action_t* slugs_action) -{ - return mavlink_msg_slugs_action_pack(system_id, component_id, msg, slugs_action->target, slugs_action->actionId, slugs_action->actionVal); -} - -/** - * @brief Send a slugs_action message - * @param chan MAVLink channel to send the message - * - * @param target The system reporting the action - * @param actionId Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - * @param actionVal Value associated with the action - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_slugs_action_send(mavlink_channel_t chan, uint8_t target, uint8_t actionId, uint16_t actionVal) -{ - mavlink_message_t msg; - mavlink_msg_slugs_action_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, actionId, actionVal); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE SLUGS_ACTION UNPACKING - -/** - * @brief Get field target from slugs_action message - * - * @return The system reporting the action - */ -static inline uint8_t mavlink_msg_slugs_action_get_target(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field actionId from slugs_action message - * - * @return Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - */ -static inline uint8_t mavlink_msg_slugs_action_get_actionId(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field actionVal from slugs_action message - * - * @return Value associated with the action - */ -static inline uint16_t mavlink_msg_slugs_action_get_actionVal(const mavlink_message_t* msg) -{ - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; -} - -/** - * @brief Decode a slugs_action message into a struct - * - * @param msg The message to decode - * @param slugs_action C-struct to decode the message contents into - */ -static inline void mavlink_msg_slugs_action_decode(const mavlink_message_t* msg, mavlink_slugs_action_t* slugs_action) -{ - slugs_action->target = mavlink_msg_slugs_action_get_target(msg); - slugs_action->actionId = mavlink_msg_slugs_action_get_actionId(msg); - slugs_action->actionVal = mavlink_msg_slugs_action_get_actionVal(msg); -} diff --git a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_slugs_navigation.h b/libraries/GCS_MAVLink/include/slugs/mavlink_msg_slugs_navigation.h deleted file mode 100644 index f1a54cb3e4..0000000000 --- a/libraries/GCS_MAVLink/include/slugs/mavlink_msg_slugs_navigation.h +++ /dev/null @@ -1,272 +0,0 @@ -// MESSAGE SLUGS_NAVIGATION PACKING - -#define MAVLINK_MSG_ID_SLUGS_NAVIGATION 176 - -typedef struct __mavlink_slugs_navigation_t -{ - float u_m; ///< Measured Airspeed prior to the Nav Filter - float phi_c; ///< Commanded Roll - float theta_c; ///< Commanded Pitch - float psiDot_c; ///< Commanded Turn rate - float ay_body; ///< Y component of the body acceleration - float totalDist; ///< Total Distance to Run on this leg of Navigation - float dist2Go; ///< Remaining distance to Run on this leg of Navigation - uint8_t fromWP; ///< Origin WP - uint8_t toWP; ///< Destination WP - -} mavlink_slugs_navigation_t; - - - -/** - * @brief Pack a slugs_navigation 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 u_m Measured Airspeed prior to the Nav Filter - * @param phi_c Commanded Roll - * @param theta_c Commanded Pitch - * @param psiDot_c Commanded Turn rate - * @param ay_body Y component of the body acceleration - * @param totalDist Total Distance to Run on this leg of Navigation - * @param dist2Go Remaining distance to Run on this leg of Navigation - * @param fromWP Origin WP - * @param toWP Destination WP - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_slugs_navigation_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float u_m, float phi_c, float theta_c, float psiDot_c, float ay_body, float totalDist, float dist2Go, uint8_t fromWP, uint8_t toWP) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; - - i += put_float_by_index(u_m, i, msg->payload); // Measured Airspeed prior to the Nav Filter - i += put_float_by_index(phi_c, i, msg->payload); // Commanded Roll - i += put_float_by_index(theta_c, i, msg->payload); // Commanded Pitch - i += put_float_by_index(psiDot_c, i, msg->payload); // Commanded Turn rate - i += put_float_by_index(ay_body, i, msg->payload); // Y component of the body acceleration - i += put_float_by_index(totalDist, i, msg->payload); // Total Distance to Run on this leg of Navigation - i += put_float_by_index(dist2Go, i, msg->payload); // Remaining distance to Run on this leg of Navigation - i += put_uint8_t_by_index(fromWP, i, msg->payload); // Origin WP - i += put_uint8_t_by_index(toWP, i, msg->payload); // Destination WP - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a slugs_navigation message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param u_m Measured Airspeed prior to the Nav Filter - * @param phi_c Commanded Roll - * @param theta_c Commanded Pitch - * @param psiDot_c Commanded Turn rate - * @param ay_body Y component of the body acceleration - * @param totalDist Total Distance to Run on this leg of Navigation - * @param dist2Go Remaining distance to Run on this leg of Navigation - * @param fromWP Origin WP - * @param toWP Destination WP - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_slugs_navigation_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float u_m, float phi_c, float theta_c, float psiDot_c, float ay_body, float totalDist, float dist2Go, uint8_t fromWP, uint8_t toWP) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; - - i += put_float_by_index(u_m, i, msg->payload); // Measured Airspeed prior to the Nav Filter - i += put_float_by_index(phi_c, i, msg->payload); // Commanded Roll - i += put_float_by_index(theta_c, i, msg->payload); // Commanded Pitch - i += put_float_by_index(psiDot_c, i, msg->payload); // Commanded Turn rate - i += put_float_by_index(ay_body, i, msg->payload); // Y component of the body acceleration - i += put_float_by_index(totalDist, i, msg->payload); // Total Distance to Run on this leg of Navigation - i += put_float_by_index(dist2Go, i, msg->payload); // Remaining distance to Run on this leg of Navigation - i += put_uint8_t_by_index(fromWP, i, msg->payload); // Origin WP - i += put_uint8_t_by_index(toWP, i, msg->payload); // Destination WP - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a slugs_navigation struct into 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 slugs_navigation C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_slugs_navigation_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_slugs_navigation_t* slugs_navigation) -{ - return mavlink_msg_slugs_navigation_pack(system_id, component_id, msg, slugs_navigation->u_m, slugs_navigation->phi_c, slugs_navigation->theta_c, slugs_navigation->psiDot_c, slugs_navigation->ay_body, slugs_navigation->totalDist, slugs_navigation->dist2Go, slugs_navigation->fromWP, slugs_navigation->toWP); -} - -/** - * @brief Send a slugs_navigation message - * @param chan MAVLink channel to send the message - * - * @param u_m Measured Airspeed prior to the Nav Filter - * @param phi_c Commanded Roll - * @param theta_c Commanded Pitch - * @param psiDot_c Commanded Turn rate - * @param ay_body Y component of the body acceleration - * @param totalDist Total Distance to Run on this leg of Navigation - * @param dist2Go Remaining distance to Run on this leg of Navigation - * @param fromWP Origin WP - * @param toWP Destination WP - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_slugs_navigation_send(mavlink_channel_t chan, float u_m, float phi_c, float theta_c, float psiDot_c, float ay_body, float totalDist, float dist2Go, uint8_t fromWP, uint8_t toWP) -{ - mavlink_message_t msg; - mavlink_msg_slugs_navigation_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, u_m, phi_c, theta_c, psiDot_c, ay_body, totalDist, dist2Go, fromWP, toWP); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE SLUGS_NAVIGATION UNPACKING - -/** - * @brief Get field u_m from slugs_navigation message - * - * @return Measured Airspeed prior to the Nav Filter - */ -static inline float mavlink_msg_slugs_navigation_get_u_m(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; -} - -/** - * @brief Get field phi_c from slugs_navigation message - * - * @return Commanded Roll - */ -static inline float mavlink_msg_slugs_navigation_get_phi_c(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field theta_c from slugs_navigation message - * - * @return Commanded Pitch - */ -static inline float mavlink_msg_slugs_navigation_get_theta_c(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field psiDot_c from slugs_navigation message - * - * @return Commanded Turn rate - */ -static inline float mavlink_msg_slugs_navigation_get_psiDot_c(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field ay_body from slugs_navigation message - * - * @return Y component of the body acceleration - */ -static inline float mavlink_msg_slugs_navigation_get_ay_body(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field totalDist from slugs_navigation message - * - * @return Total Distance to Run on this leg of Navigation - */ -static inline float mavlink_msg_slugs_navigation_get_totalDist(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field dist2Go from slugs_navigation message - * - * @return Remaining distance to Run on this leg of Navigation - */ -static inline float mavlink_msg_slugs_navigation_get_dist2Go(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field fromWP from slugs_navigation message - * - * @return Origin WP - */ -static inline uint8_t mavlink_msg_slugs_navigation_get_fromWP(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; -} - -/** - * @brief Get field toWP from slugs_navigation message - * - * @return Destination WP - */ -static inline uint8_t mavlink_msg_slugs_navigation_get_toWP(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[0]; -} - -/** - * @brief Decode a slugs_navigation message into a struct - * - * @param msg The message to decode - * @param slugs_navigation C-struct to decode the message contents into - */ -static inline void mavlink_msg_slugs_navigation_decode(const mavlink_message_t* msg, mavlink_slugs_navigation_t* slugs_navigation) -{ - slugs_navigation->u_m = mavlink_msg_slugs_navigation_get_u_m(msg); - slugs_navigation->phi_c = mavlink_msg_slugs_navigation_get_phi_c(msg); - slugs_navigation->theta_c = mavlink_msg_slugs_navigation_get_theta_c(msg); - slugs_navigation->psiDot_c = mavlink_msg_slugs_navigation_get_psiDot_c(msg); - slugs_navigation->ay_body = mavlink_msg_slugs_navigation_get_ay_body(msg); - slugs_navigation->totalDist = mavlink_msg_slugs_navigation_get_totalDist(msg); - slugs_navigation->dist2Go = mavlink_msg_slugs_navigation_get_dist2Go(msg); - slugs_navigation->fromWP = mavlink_msg_slugs_navigation_get_fromWP(msg); - slugs_navigation->toWP = mavlink_msg_slugs_navigation_get_toWP(msg); -} diff --git a/libraries/GCS_MAVLink/include/slugs/slugs.h b/libraries/GCS_MAVLink/include/slugs/slugs.h deleted file mode 100644 index b04cb7922f..0000000000 --- a/libraries/GCS_MAVLink/include/slugs/slugs.h +++ /dev/null @@ -1,56 +0,0 @@ -/** @file - * @brief MAVLink comm protocol. - * @see http://qgroundcontrol.org/mavlink/ - * Generated on Friday, August 5 2011, 07:37 UTC - */ -#ifndef SLUGS_H -#define SLUGS_H - -#ifdef __cplusplus -extern "C" { -#endif - - -#include "../protocol.h" - -#define MAVLINK_ENABLED_SLUGS - - -#include "../common/common.h" -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 0 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 0 -#endif - -// ENUM DEFINITIONS - - -// MESSAGE DEFINITIONS - -#include "./mavlink_msg_cpu_load.h" -#include "./mavlink_msg_air_data.h" -#include "./mavlink_msg_sensor_bias.h" -#include "./mavlink_msg_diagnostic.h" -#include "./mavlink_msg_slugs_navigation.h" -#include "./mavlink_msg_data_log.h" -#include "./mavlink_msg_gps_date_time.h" -#include "./mavlink_msg_mid_lvl_cmds.h" -#include "./mavlink_msg_ctrl_srfc_pt.h" -#include "./mavlink_msg_slugs_action.h" - - -// MESSAGE LENGTHS - -#undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 14, 14, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 10, 24, 18, 0, 0, 30, 24, 0, 7, 13, 3, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } - -#ifdef __cplusplus -} -#endif -#endif diff --git a/libraries/GCS_MAVLink/include/ualberta/mavlink.h b/libraries/GCS_MAVLink/include/ualberta/mavlink.h deleted file mode 100644 index e12f74a8df..0000000000 --- a/libraries/GCS_MAVLink/include/ualberta/mavlink.h +++ /dev/null @@ -1,11 +0,0 @@ -/** @file - * @brief MAVLink comm protocol. - * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Friday, August 5 2011, 07:37 UTC - */ -#ifndef MAVLINK_H -#define MAVLINK_H - -#include "ualberta.h" - -#endif diff --git a/libraries/GCS_MAVLink/include/ualberta/mavlink_msg_nav_filter_bias.h b/libraries/GCS_MAVLink/include/ualberta/mavlink_msg_nav_filter_bias.h deleted file mode 100644 index a011fc1ab4..0000000000 --- a/libraries/GCS_MAVLink/include/ualberta/mavlink_msg_nav_filter_bias.h +++ /dev/null @@ -1,242 +0,0 @@ -// MESSAGE NAV_FILTER_BIAS PACKING - -#define MAVLINK_MSG_ID_NAV_FILTER_BIAS 220 - -typedef struct __mavlink_nav_filter_bias_t -{ - uint64_t usec; ///< Timestamp (microseconds) - float accel_0; ///< b_f[0] - float accel_1; ///< b_f[1] - float accel_2; ///< b_f[2] - float gyro_0; ///< b_f[0] - float gyro_1; ///< b_f[1] - float gyro_2; ///< b_f[2] - -} mavlink_nav_filter_bias_t; - - - -/** - * @brief Pack a nav_filter_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 usec Timestamp (microseconds) - * @param accel_0 b_f[0] - * @param accel_1 b_f[1] - * @param accel_2 b_f[2] - * @param gyro_0 b_f[0] - * @param gyro_1 b_f[1] - * @param gyro_2 b_f[2] - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_nav_filter_bias_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float accel_0, float accel_1, float accel_2, float gyro_0, float gyro_1, float gyro_2) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds) - i += put_float_by_index(accel_0, i, msg->payload); // b_f[0] - i += put_float_by_index(accel_1, i, msg->payload); // b_f[1] - i += put_float_by_index(accel_2, i, msg->payload); // b_f[2] - i += put_float_by_index(gyro_0, i, msg->payload); // b_f[0] - i += put_float_by_index(gyro_1, i, msg->payload); // b_f[1] - i += put_float_by_index(gyro_2, i, msg->payload); // b_f[2] - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a nav_filter_bias message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (microseconds) - * @param accel_0 b_f[0] - * @param accel_1 b_f[1] - * @param accel_2 b_f[2] - * @param gyro_0 b_f[0] - * @param gyro_1 b_f[1] - * @param gyro_2 b_f[2] - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_nav_filter_bias_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float accel_0, float accel_1, float accel_2, float gyro_0, float gyro_1, float gyro_2) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds) - i += put_float_by_index(accel_0, i, msg->payload); // b_f[0] - i += put_float_by_index(accel_1, i, msg->payload); // b_f[1] - i += put_float_by_index(accel_2, i, msg->payload); // b_f[2] - i += put_float_by_index(gyro_0, i, msg->payload); // b_f[0] - i += put_float_by_index(gyro_1, i, msg->payload); // b_f[1] - i += put_float_by_index(gyro_2, i, msg->payload); // b_f[2] - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a nav_filter_bias struct into 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 nav_filter_bias C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_nav_filter_bias_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_nav_filter_bias_t* nav_filter_bias) -{ - return mavlink_msg_nav_filter_bias_pack(system_id, component_id, msg, nav_filter_bias->usec, nav_filter_bias->accel_0, nav_filter_bias->accel_1, nav_filter_bias->accel_2, nav_filter_bias->gyro_0, nav_filter_bias->gyro_1, nav_filter_bias->gyro_2); -} - -/** - * @brief Send a nav_filter_bias message - * @param chan MAVLink channel to send the message - * - * @param usec Timestamp (microseconds) - * @param accel_0 b_f[0] - * @param accel_1 b_f[1] - * @param accel_2 b_f[2] - * @param gyro_0 b_f[0] - * @param gyro_1 b_f[1] - * @param gyro_2 b_f[2] - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_nav_filter_bias_send(mavlink_channel_t chan, uint64_t usec, float accel_0, float accel_1, float accel_2, float gyro_0, float gyro_1, float gyro_2) -{ - mavlink_message_t msg; - mavlink_msg_nav_filter_bias_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, accel_0, accel_1, accel_2, gyro_0, gyro_1, gyro_2); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE NAV_FILTER_BIAS UNPACKING - -/** - * @brief Get field usec from nav_filter_bias message - * - * @return Timestamp (microseconds) - */ -static inline uint64_t mavlink_msg_nav_filter_bias_get_usec(const mavlink_message_t* msg) -{ - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; -} - -/** - * @brief Get field accel_0 from nav_filter_bias message - * - * @return b_f[0] - */ -static inline float mavlink_msg_nav_filter_bias_get_accel_0(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (float)r.f; -} - -/** - * @brief Get field accel_1 from nav_filter_bias message - * - * @return b_f[1] - */ -static inline float mavlink_msg_nav_filter_bias_get_accel_1(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field accel_2 from nav_filter_bias message - * - * @return b_f[2] - */ -static inline float mavlink_msg_nav_filter_bias_get_accel_2(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field gyro_0 from nav_filter_bias message - * - * @return b_f[0] - */ -static inline float mavlink_msg_nav_filter_bias_get_gyro_0(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field gyro_1 from nav_filter_bias message - * - * @return b_f[1] - */ -static inline float mavlink_msg_nav_filter_bias_get_gyro_1(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field gyro_2 from nav_filter_bias message - * - * @return b_f[2] - */ -static inline float mavlink_msg_nav_filter_bias_get_gyro_2(const mavlink_message_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Decode a nav_filter_bias message into a struct - * - * @param msg The message to decode - * @param nav_filter_bias C-struct to decode the message contents into - */ -static inline void mavlink_msg_nav_filter_bias_decode(const mavlink_message_t* msg, mavlink_nav_filter_bias_t* nav_filter_bias) -{ - nav_filter_bias->usec = mavlink_msg_nav_filter_bias_get_usec(msg); - nav_filter_bias->accel_0 = mavlink_msg_nav_filter_bias_get_accel_0(msg); - nav_filter_bias->accel_1 = mavlink_msg_nav_filter_bias_get_accel_1(msg); - nav_filter_bias->accel_2 = mavlink_msg_nav_filter_bias_get_accel_2(msg); - nav_filter_bias->gyro_0 = mavlink_msg_nav_filter_bias_get_gyro_0(msg); - nav_filter_bias->gyro_1 = mavlink_msg_nav_filter_bias_get_gyro_1(msg); - nav_filter_bias->gyro_2 = mavlink_msg_nav_filter_bias_get_gyro_2(msg); -} diff --git a/libraries/GCS_MAVLink/include/ualberta/mavlink_msg_radio_calibration.h b/libraries/GCS_MAVLink/include/ualberta/mavlink_msg_radio_calibration.h deleted file mode 100644 index 5907aba95b..0000000000 --- a/libraries/GCS_MAVLink/include/ualberta/mavlink_msg_radio_calibration.h +++ /dev/null @@ -1,204 +0,0 @@ -// MESSAGE RADIO_CALIBRATION PACKING - -#define MAVLINK_MSG_ID_RADIO_CALIBRATION 221 - -typedef struct __mavlink_radio_calibration_t -{ - uint16_t aileron[3]; ///< Aileron setpoints: left, center, right - uint16_t elevator[3]; ///< Elevator setpoints: nose down, center, nose up - uint16_t rudder[3]; ///< Rudder setpoints: nose left, center, nose right - uint16_t gyro[2]; ///< Tail gyro mode/gain setpoints: heading hold, rate mode - uint16_t pitch[5]; ///< Pitch curve setpoints (every 25%) - uint16_t throttle[5]; ///< Throttle curve setpoints (every 25%) - -} mavlink_radio_calibration_t; - -#define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_AILERON_LEN 3 -#define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_ELEVATOR_LEN 3 -#define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_RUDDER_LEN 3 -#define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_GYRO_LEN 2 -#define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_PITCH_LEN 5 -#define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_THROTTLE_LEN 5 - - -/** - * @brief Pack a radio_calibration 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 aileron Aileron setpoints: left, center, right - * @param elevator Elevator setpoints: nose down, center, nose up - * @param rudder Rudder setpoints: nose left, center, nose right - * @param gyro Tail gyro mode/gain setpoints: heading hold, rate mode - * @param pitch Pitch curve setpoints (every 25%) - * @param throttle Throttle curve setpoints (every 25%) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_radio_calibration_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const uint16_t* aileron, const uint16_t* elevator, const uint16_t* rudder, const uint16_t* gyro, const uint16_t* pitch, const uint16_t* throttle) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; - - i += put_array_by_index((const int8_t*)aileron, sizeof(uint16_t)*3, i, msg->payload); // Aileron setpoints: left, center, right - i += put_array_by_index((const int8_t*)elevator, sizeof(uint16_t)*3, i, msg->payload); // Elevator setpoints: nose down, center, nose up - i += put_array_by_index((const int8_t*)rudder, sizeof(uint16_t)*3, i, msg->payload); // Rudder setpoints: nose left, center, nose right - i += put_array_by_index((const int8_t*)gyro, sizeof(uint16_t)*2, i, msg->payload); // Tail gyro mode/gain setpoints: heading hold, rate mode - i += put_array_by_index((const int8_t*)pitch, sizeof(uint16_t)*5, i, msg->payload); // Pitch curve setpoints (every 25%) - i += put_array_by_index((const int8_t*)throttle, sizeof(uint16_t)*5, i, msg->payload); // Throttle curve setpoints (every 25%) - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a radio_calibration message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param aileron Aileron setpoints: left, center, right - * @param elevator Elevator setpoints: nose down, center, nose up - * @param rudder Rudder setpoints: nose left, center, nose right - * @param gyro Tail gyro mode/gain setpoints: heading hold, rate mode - * @param pitch Pitch curve setpoints (every 25%) - * @param throttle Throttle curve setpoints (every 25%) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_radio_calibration_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const uint16_t* aileron, const uint16_t* elevator, const uint16_t* rudder, const uint16_t* gyro, const uint16_t* pitch, const uint16_t* throttle) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; - - i += put_array_by_index((const int8_t*)aileron, sizeof(uint16_t)*3, i, msg->payload); // Aileron setpoints: left, center, right - i += put_array_by_index((const int8_t*)elevator, sizeof(uint16_t)*3, i, msg->payload); // Elevator setpoints: nose down, center, nose up - i += put_array_by_index((const int8_t*)rudder, sizeof(uint16_t)*3, i, msg->payload); // Rudder setpoints: nose left, center, nose right - i += put_array_by_index((const int8_t*)gyro, sizeof(uint16_t)*2, i, msg->payload); // Tail gyro mode/gain setpoints: heading hold, rate mode - i += put_array_by_index((const int8_t*)pitch, sizeof(uint16_t)*5, i, msg->payload); // Pitch curve setpoints (every 25%) - i += put_array_by_index((const int8_t*)throttle, sizeof(uint16_t)*5, i, msg->payload); // Throttle curve setpoints (every 25%) - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a radio_calibration struct into 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 radio_calibration C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_radio_calibration_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_radio_calibration_t* radio_calibration) -{ - return mavlink_msg_radio_calibration_pack(system_id, component_id, msg, radio_calibration->aileron, radio_calibration->elevator, radio_calibration->rudder, radio_calibration->gyro, radio_calibration->pitch, radio_calibration->throttle); -} - -/** - * @brief Send a radio_calibration message - * @param chan MAVLink channel to send the message - * - * @param aileron Aileron setpoints: left, center, right - * @param elevator Elevator setpoints: nose down, center, nose up - * @param rudder Rudder setpoints: nose left, center, nose right - * @param gyro Tail gyro mode/gain setpoints: heading hold, rate mode - * @param pitch Pitch curve setpoints (every 25%) - * @param throttle Throttle curve setpoints (every 25%) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_radio_calibration_send(mavlink_channel_t chan, const uint16_t* aileron, const uint16_t* elevator, const uint16_t* rudder, const uint16_t* gyro, const uint16_t* pitch, const uint16_t* throttle) -{ - mavlink_message_t msg; - mavlink_msg_radio_calibration_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, aileron, elevator, rudder, gyro, pitch, throttle); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE RADIO_CALIBRATION UNPACKING - -/** - * @brief Get field aileron from radio_calibration message - * - * @return Aileron setpoints: left, center, right - */ -static inline uint16_t mavlink_msg_radio_calibration_get_aileron(const mavlink_message_t* msg, uint16_t* r_data) -{ - - memcpy(r_data, msg->payload, sizeof(uint16_t)*3); - return sizeof(uint16_t)*3; -} - -/** - * @brief Get field elevator from radio_calibration message - * - * @return Elevator setpoints: nose down, center, nose up - */ -static inline uint16_t mavlink_msg_radio_calibration_get_elevator(const mavlink_message_t* msg, uint16_t* r_data) -{ - - memcpy(r_data, msg->payload+sizeof(uint16_t)*3, sizeof(uint16_t)*3); - return sizeof(uint16_t)*3; -} - -/** - * @brief Get field rudder from radio_calibration message - * - * @return Rudder setpoints: nose left, center, nose right - */ -static inline uint16_t mavlink_msg_radio_calibration_get_rudder(const mavlink_message_t* msg, uint16_t* r_data) -{ - - memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3, sizeof(uint16_t)*3); - return sizeof(uint16_t)*3; -} - -/** - * @brief Get field gyro from radio_calibration message - * - * @return Tail gyro mode/gain setpoints: heading hold, rate mode - */ -static inline uint16_t mavlink_msg_radio_calibration_get_gyro(const mavlink_message_t* msg, uint16_t* r_data) -{ - - memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*3, sizeof(uint16_t)*2); - return sizeof(uint16_t)*2; -} - -/** - * @brief Get field pitch from radio_calibration message - * - * @return Pitch curve setpoints (every 25%) - */ -static inline uint16_t mavlink_msg_radio_calibration_get_pitch(const mavlink_message_t* msg, uint16_t* r_data) -{ - - memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*2, sizeof(uint16_t)*5); - return sizeof(uint16_t)*5; -} - -/** - * @brief Get field throttle from radio_calibration message - * - * @return Throttle curve setpoints (every 25%) - */ -static inline uint16_t mavlink_msg_radio_calibration_get_throttle(const mavlink_message_t* msg, uint16_t* r_data) -{ - - memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*2+sizeof(uint16_t)*5, sizeof(uint16_t)*5); - return sizeof(uint16_t)*5; -} - -/** - * @brief Decode a radio_calibration message into a struct - * - * @param msg The message to decode - * @param radio_calibration C-struct to decode the message contents into - */ -static inline void mavlink_msg_radio_calibration_decode(const mavlink_message_t* msg, mavlink_radio_calibration_t* radio_calibration) -{ - mavlink_msg_radio_calibration_get_aileron(msg, radio_calibration->aileron); - mavlink_msg_radio_calibration_get_elevator(msg, radio_calibration->elevator); - mavlink_msg_radio_calibration_get_rudder(msg, radio_calibration->rudder); - mavlink_msg_radio_calibration_get_gyro(msg, radio_calibration->gyro); - mavlink_msg_radio_calibration_get_pitch(msg, radio_calibration->pitch); - mavlink_msg_radio_calibration_get_throttle(msg, radio_calibration->throttle); -} diff --git a/libraries/GCS_MAVLink/include/ualberta/mavlink_msg_request_radio_calibration.h b/libraries/GCS_MAVLink/include/ualberta/mavlink_msg_request_radio_calibration.h deleted file mode 100644 index 6fa5dd08b3..0000000000 --- a/libraries/GCS_MAVLink/include/ualberta/mavlink_msg_request_radio_calibration.h +++ /dev/null @@ -1,59 +0,0 @@ -// MESSAGE REQUEST_RADIO_CALIBRATION PACKING - -#define MAVLINK_MSG_ID_REQUEST_RADIO_CALIBRATION 83 - -typedef struct __mavlink_request_radio_calibration_t -{ - uint8_t unused; ///< Unused field. Included to prevent compile time warnings - -} mavlink_request_radio_calibration_t; - - - -/** - * @brief Send a request_radio_calibration message - * - * @param unused Unused field. Included to prevent compile time warnings - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_request_radio_calibration_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t unused) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_REQUEST_RADIO_CALIBRATION; - - i += put_uint8_t_by_index(unused, i, msg->payload); //Unused field. Included to prevent compile time warnings - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -static inline uint16_t mavlink_msg_request_radio_calibration_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_request_radio_calibration_t* request_radio_calibration) -{ - return mavlink_msg_request_radio_calibration_pack(system_id, component_id, msg, request_radio_calibration->unused); -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_request_radio_calibration_send(mavlink_channel_t chan, uint8_t unused) -{ - mavlink_message_t msg; - mavlink_msg_request_radio_calibration_pack(mavlink_system.sysid, mavlink_system.compid, &msg, unused); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE REQUEST_RADIO_CALIBRATION UNPACKING - -/** - * @brief Get field unused from request_radio_calibration message - * - * @return Unused field. Included to prevent compile time warnings - */ -static inline uint8_t mavlink_msg_request_radio_calibration_get_unused(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -static inline void mavlink_msg_request_radio_calibration_decode(const mavlink_message_t* msg, mavlink_request_radio_calibration_t* request_radio_calibration) -{ - request_radio_calibration->unused = mavlink_msg_request_radio_calibration_get_unused(msg); -} diff --git a/libraries/GCS_MAVLink/include/ualberta/mavlink_msg_request_rc_channels.h b/libraries/GCS_MAVLink/include/ualberta/mavlink_msg_request_rc_channels.h deleted file mode 100644 index 64202ffe26..0000000000 --- a/libraries/GCS_MAVLink/include/ualberta/mavlink_msg_request_rc_channels.h +++ /dev/null @@ -1,101 +0,0 @@ -// MESSAGE REQUEST_RC_CHANNELS PACKING - -#define MAVLINK_MSG_ID_REQUEST_RC_CHANNELS 221 - -typedef struct __mavlink_request_rc_channels_t -{ - uint8_t enabled; ///< True: start sending data; False: stop sending data - -} mavlink_request_rc_channels_t; - - - -/** - * @brief Pack a request_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 enabled True: start sending data; False: stop sending data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_request_rc_channels_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t enabled) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_REQUEST_RC_CHANNELS; - - i += put_uint8_t_by_index(enabled, i, msg->payload); // True: start sending data; False: stop sending data - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a request_rc_channels message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param enabled True: start sending data; False: stop sending data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_request_rc_channels_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t enabled) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_REQUEST_RC_CHANNELS; - - i += put_uint8_t_by_index(enabled, i, msg->payload); // True: start sending data; False: stop sending data - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a request_rc_channels struct into 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 request_rc_channels C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_request_rc_channels_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_request_rc_channels_t* request_rc_channels) -{ - return mavlink_msg_request_rc_channels_pack(system_id, component_id, msg, request_rc_channels->enabled); -} - -/** - * @brief Send a request_rc_channels message - * @param chan MAVLink channel to send the message - * - * @param enabled True: start sending data; False: stop sending data - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_request_rc_channels_send(mavlink_channel_t chan, uint8_t enabled) -{ - mavlink_message_t msg; - mavlink_msg_request_rc_channels_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, enabled); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE REQUEST_RC_CHANNELS UNPACKING - -/** - * @brief Get field enabled from request_rc_channels message - * - * @return True: start sending data; False: stop sending data - */ -static inline uint8_t mavlink_msg_request_rc_channels_get_enabled(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Decode a request_rc_channels message into a struct - * - * @param msg The message to decode - * @param request_rc_channels C-struct to decode the message contents into - */ -static inline void mavlink_msg_request_rc_channels_decode(const mavlink_message_t* msg, mavlink_request_rc_channels_t* request_rc_channels) -{ - request_rc_channels->enabled = mavlink_msg_request_rc_channels_get_enabled(msg); -} diff --git a/libraries/GCS_MAVLink/include/ualberta/mavlink_msg_ualberta_sys_status.h b/libraries/GCS_MAVLink/include/ualberta/mavlink_msg_ualberta_sys_status.h deleted file mode 100644 index 50e8f7d02c..0000000000 --- a/libraries/GCS_MAVLink/include/ualberta/mavlink_msg_ualberta_sys_status.h +++ /dev/null @@ -1,135 +0,0 @@ -// MESSAGE UALBERTA_SYS_STATUS PACKING - -#define MAVLINK_MSG_ID_UALBERTA_SYS_STATUS 222 - -typedef struct __mavlink_ualberta_sys_status_t -{ - uint8_t mode; ///< System mode, see UALBERTA_AUTOPILOT_MODE ENUM - uint8_t nav_mode; ///< Navigation mode, see UALBERTA_NAV_MODE ENUM - uint8_t pilot; ///< Pilot mode, see UALBERTA_PILOT_MODE - -} mavlink_ualberta_sys_status_t; - - - -/** - * @brief Pack a ualberta_sys_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 mode System mode, see UALBERTA_AUTOPILOT_MODE ENUM - * @param nav_mode Navigation mode, see UALBERTA_NAV_MODE ENUM - * @param pilot Pilot mode, see UALBERTA_PILOT_MODE - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ualberta_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t mode, uint8_t nav_mode, uint8_t pilot) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; - - i += put_uint8_t_by_index(mode, i, msg->payload); // System mode, see UALBERTA_AUTOPILOT_MODE ENUM - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode, see UALBERTA_NAV_MODE ENUM - i += put_uint8_t_by_index(pilot, i, msg->payload); // Pilot mode, see UALBERTA_PILOT_MODE - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a ualberta_sys_status message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param mode System mode, see UALBERTA_AUTOPILOT_MODE ENUM - * @param nav_mode Navigation mode, see UALBERTA_NAV_MODE ENUM - * @param pilot Pilot mode, see UALBERTA_PILOT_MODE - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ualberta_sys_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t mode, uint8_t nav_mode, uint8_t pilot) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; - - i += put_uint8_t_by_index(mode, i, msg->payload); // System mode, see UALBERTA_AUTOPILOT_MODE ENUM - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode, see UALBERTA_NAV_MODE ENUM - i += put_uint8_t_by_index(pilot, i, msg->payload); // Pilot mode, see UALBERTA_PILOT_MODE - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a ualberta_sys_status struct into 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 ualberta_sys_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ualberta_sys_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ualberta_sys_status_t* ualberta_sys_status) -{ - return mavlink_msg_ualberta_sys_status_pack(system_id, component_id, msg, ualberta_sys_status->mode, ualberta_sys_status->nav_mode, ualberta_sys_status->pilot); -} - -/** - * @brief Send a ualberta_sys_status message - * @param chan MAVLink channel to send the message - * - * @param mode System mode, see UALBERTA_AUTOPILOT_MODE ENUM - * @param nav_mode Navigation mode, see UALBERTA_NAV_MODE ENUM - * @param pilot Pilot mode, see UALBERTA_PILOT_MODE - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_ualberta_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t pilot) -{ - mavlink_message_t msg; - mavlink_msg_ualberta_sys_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, mode, nav_mode, pilot); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE UALBERTA_SYS_STATUS UNPACKING - -/** - * @brief Get field mode from ualberta_sys_status message - * - * @return System mode, see UALBERTA_AUTOPILOT_MODE ENUM - */ -static inline uint8_t mavlink_msg_ualberta_sys_status_get_mode(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field nav_mode from ualberta_sys_status message - * - * @return Navigation mode, see UALBERTA_NAV_MODE ENUM - */ -static inline uint8_t mavlink_msg_ualberta_sys_status_get_nav_mode(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field pilot from ualberta_sys_status message - * - * @return Pilot mode, see UALBERTA_PILOT_MODE - */ -static inline uint8_t mavlink_msg_ualberta_sys_status_get_pilot(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -/** - * @brief Decode a ualberta_sys_status message into a struct - * - * @param msg The message to decode - * @param ualberta_sys_status C-struct to decode the message contents into - */ -static inline void mavlink_msg_ualberta_sys_status_decode(const mavlink_message_t* msg, mavlink_ualberta_sys_status_t* ualberta_sys_status) -{ - ualberta_sys_status->mode = mavlink_msg_ualberta_sys_status_get_mode(msg); - ualberta_sys_status->nav_mode = mavlink_msg_ualberta_sys_status_get_nav_mode(msg); - ualberta_sys_status->pilot = mavlink_msg_ualberta_sys_status_get_pilot(msg); -} diff --git a/libraries/GCS_MAVLink/include/ualberta/ualberta.h b/libraries/GCS_MAVLink/include/ualberta/ualberta.h deleted file mode 100644 index be77cc2e5d..0000000000 --- a/libraries/GCS_MAVLink/include/ualberta/ualberta.h +++ /dev/null @@ -1,79 +0,0 @@ -/** @file - * @brief MAVLink comm protocol. - * @see http://qgroundcontrol.org/mavlink/ - * Generated on Friday, August 5 2011, 07:37 UTC - */ -#ifndef UALBERTA_H -#define UALBERTA_H - -#ifdef __cplusplus -extern "C" { -#endif - - -#include "../protocol.h" - -#define MAVLINK_ENABLED_UALBERTA - - -#include "../common/common.h" -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 0 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 0 -#endif - -// ENUM DEFINITIONS - -/** @brief Available autopilot modes for ualberta uav */ -enum UALBERTA_AUTOPILOT_MODE -{ - MODE_MANUAL_DIRECT=0, /* */ - MODE_MANUAL_SCALED=1, /* */ - MODE_AUTO_PID_ATT=2, /* */ - MODE_AUTO_PID_VEL=3, /* */ - MODE_AUTO_PID_POS=4, /* */ - UALBERTA_AUTOPILOT_MODE_ENUM_END -}; - -/** @brief Navigation filter mode */ -enum UALBERTA_NAV_MODE -{ - NAV_AHRS_INIT=0, - NAV_AHRS=1, /* */ - NAV_INS_GPS_INIT=2, /* */ - NAV_INS_GPS=3, /* */ - UALBERTA_NAV_MODE_ENUM_END -}; - -/** @brief Mode currently commanded by pilot */ -enum UALBERTA_PILOT_MODE -{ - PILOT_MANUAL=0, /* */ - PILOT_AUTO=1, /* */ - PILOT_ROTO=2, /* */ - UALBERTA_PILOT_MODE_ENUM_END -}; - - -// MESSAGE DEFINITIONS - -#include "./mavlink_msg_nav_filter_bias.h" -#include "./mavlink_msg_radio_calibration.h" -#include "./mavlink_msg_ualberta_sys_status.h" - - -// MESSAGE LENGTHS - -#undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 14, 14, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 32, 42, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } - -#ifdef __cplusplus -} -#endif -#endif diff --git a/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/ardupilotmega.h b/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/ardupilotmega.h index ca15b7a34a..801301e87f 100644 --- a/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/ardupilotmega.h +++ b/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/ardupilotmega.h @@ -12,15 +12,15 @@ 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, 0, 28, 22, 22, 21, 0, 36, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 0, 0, 26, 0, 36, 0, 6, 4, 0, 21, 18, 0, 0, 0, 20, 20, 32, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 18, 32, 32, 20, 32, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 3} +#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, 0, 0, 26, 0, 36, 0, 6, 4, 0, 21, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 18, 32, 32, 20, 32, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 3} #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, 0, 104, 244, 237, 222, 0, 158, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 0, 0, 183, 0, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 160, 168, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 19, 102, 158, 208, 56, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 134, 219, 208, 188, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 247} +#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, 0, 0, 183, 0, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 19, 102, 158, 208, 56, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 247} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {NULL}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SET_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, 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, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, {NULL}, 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, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, {NULL}, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {NULL}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {NULL}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_SHORT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, 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, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, MAVLINK_MESSAGE_INFO_EXTENDED_MESSAGE} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {NULL}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SET_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, 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, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, {NULL}, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {NULL}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {NULL}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_VFR_HUD, {NULL}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, 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, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, 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, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, MAVLINK_MESSAGE_INFO_EXTENDED_MESSAGE} #endif #include "../protocol.h" @@ -43,12 +43,78 @@ extern "C" { // 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_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_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| Empty| 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_ENUM_END=301, /* | */ +}; +#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" #ifdef __cplusplus } diff --git a/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/ardupilotmega_testsuite.h b/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/ardupilotmega_testsuite.h deleted file mode 100644 index c75bab6a9c..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/ardupilotmega_testsuite.h +++ /dev/null @@ -1,22 +0,0 @@ -/** @file - * @brief MAVLink comm protocol testsuite generated from ardupilotmega.xml - * @see http://qgroundcontrol.org/mavlink/ - * Generated on Wed Aug 24 17:14:16 2011 - */ -#ifndef ARDUPILOTMEGA_TESTSUITE_H -#define ARDUPILOTMEGA_TESTSUITE_H - -#ifdef __cplusplus -extern "C" { -#endif - -static void mavtest_generate_outputs(mavlink_channel_t chan) -{ - mavlink_msg_sensor_offsets_send(chan , 19107 , 19211 , 19315 , 17.0 , 963497672 , 963497880 , 101.0 , 129.0 , 157.0 , 185.0 , 213.0 , 241.0 ); - mavlink_msg_set_mag_offsets_send(chan , 151 , 218 , 17235 , 17339 , 17443 ); -} - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // ARDUPILOTMEGA_TESTSUITE_H diff --git a/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_digicam_configure.h b/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_digicam_configure.h new file mode 100644 index 0000000000..89df0d9a4e --- /dev/null +++ b/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_digicam_configure.h @@ -0,0 +1,364 @@ +// 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_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[15]; + _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(msg), buf, 15); +#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(msg), &packet, 15); +#endif + + msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONFIGURE; + return mavlink_finalize_message(msg, system_id, component_id, 15, 84); +} + +/** + * @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 was 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[15]; + _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(msg), buf, 15); +#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(msg), &packet, 15); +#endif + + msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONFIGURE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 15, 84); +} + +/** + * @brief Encode a digicam_configure struct into 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 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 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[15]; + _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); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, 15, 84); +#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; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)&packet, 15, 84); +#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), 15); +#endif +} diff --git a/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_digicam_control.h b/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_digicam_control.h new file mode 100644 index 0000000000..baf63d6c7d --- /dev/null +++ b/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_digicam_control.h @@ -0,0 +1,342 @@ +// 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_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[13]; + _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(msg), buf, 13); +#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(msg), &packet, 13); +#endif + + msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONTROL; + return mavlink_finalize_message(msg, system_id, component_id, 13, 22); +} + +/** + * @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 was 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[13]; + _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(msg), buf, 13); +#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(msg), &packet, 13); +#endif + + msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONTROL; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 13, 22); +} + +/** + * @brief Encode a digicam_control struct into 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 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 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[13]; + _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); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, 13, 22); +#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; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)&packet, 13, 22); +#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), 13); +#endif +} diff --git a/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_mount_configure.h b/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_mount_configure.h new file mode 100644 index 0000000000..84979b3e4d --- /dev/null +++ b/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_mount_configure.h @@ -0,0 +1,254 @@ +// 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_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[6]; + _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(msg), buf, 6); +#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(msg), &packet, 6); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_CONFIGURE; + return mavlink_finalize_message(msg, system_id, component_id, 6, 19); +} + +/** + * @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 was 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[6]; + _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(msg), buf, 6); +#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(msg), &packet, 6); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_CONFIGURE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 6, 19); +} + +/** + * @brief Encode a mount_configure struct into 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 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 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[6]; + _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); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, buf, 6, 19); +#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; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, (const char *)&packet, 6, 19); +#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), 6); +#endif +} diff --git a/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_mount_control.h b/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_mount_control.h new file mode 100644 index 0000000000..37f87fd7ac --- /dev/null +++ b/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_mount_control.h @@ -0,0 +1,254 @@ +// 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_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[15]; + _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(msg), buf, 15); +#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(msg), &packet, 15); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_CONTROL; + return mavlink_finalize_message(msg, system_id, component_id, 15, 21); +} + +/** + * @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 was 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[15]; + _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(msg), buf, 15); +#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(msg), &packet, 15); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_CONTROL; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 15, 21); +} + +/** + * @brief Encode a mount_control struct into 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 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 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[15]; + _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); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, buf, 15, 21); +#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; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, (const char *)&packet, 15, 21); +#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), 15); +#endif +} diff --git a/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_mount_status.h b/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_mount_status.h new file mode 100644 index 0000000000..f43709ba78 --- /dev/null +++ b/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_mount_status.h @@ -0,0 +1,232 @@ +// 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_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[14]; + _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(msg), buf, 14); +#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(msg), &packet, 14); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, 14, 134); +} + +/** + * @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 was 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[14]; + _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(msg), buf, 14); +#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(msg), &packet, 14); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 134); +} + +/** + * @brief Encode a mount_status struct into 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 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 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[14]; + _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); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, buf, 14, 134); +#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; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, (const char *)&packet, 14, 134); +#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), 14); +#endif +} diff --git a/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/testsuite.h b/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/testsuite.h index c7cdc402f7..5be7e2bed4 100644 --- a/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/testsuite.h +++ b/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/testsuite.h @@ -238,12 +238,298 @@ static void mavlink_test_ap_adc(uint8_t system_id, uint8_t component_id, mavlink MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_digicam_configure(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_digicam_configure_t packet_in = { + 17.0, + 17443, + 151, + 218, + 29, + 96, + 163, + 230, + 41, + 108, + 175, + }; + mavlink_digicam_configure_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.extra_value = packet_in.extra_value; + packet1.shutter_speed = packet_in.shutter_speed; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.mode = packet_in.mode; + packet1.aperture = packet_in.aperture; + packet1.iso = packet_in.iso; + packet1.exposure_type = packet_in.exposure_type; + packet1.command_id = packet_in.command_id; + packet1.engine_cut_off = packet_in.engine_cut_off; + packet1.extra_param = packet_in.extra_param; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_digicam_configure_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_digicam_configure_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_digicam_configure_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.mode , packet1.shutter_speed , packet1.aperture , packet1.iso , packet1.exposure_type , packet1.command_id , packet1.engine_cut_off , packet1.extra_param , packet1.extra_value ); + mavlink_msg_digicam_configure_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_digicam_configure_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.mode , packet1.shutter_speed , packet1.aperture , packet1.iso , packet1.exposure_type , packet1.command_id , packet1.engine_cut_off , packet1.extra_param , packet1.extra_value ); + mavlink_msg_digicam_configure_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_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| Empty| 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_ENUM_END=253, /* | */ -}; +#endif /** @brief Data stream IDs. A data stream is not a fixed set of messages, but rather a recommendation to the autopilot software. Individual autopilots may or may not obey the recommended messages. */ +#ifndef HAVE_ENUM_MAV_DATA_STREAM +#define HAVE_ENUM_MAV_DATA_STREAM enum MAV_DATA_STREAM { MAV_DATA_STREAM_ALL=0, /* Enable all data streams | */ @@ -270,11 +258,14 @@ enum MAV_DATA_STREAM MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot | */ MAV_DATA_STREAM_ENUM_END=13, /* | */ }; +#endif /** @brief The ROI (region of interest) for the vehicle. This can be be used by the vehicle for camera/vehicle attitude alignment (see MAV_CMD_NAV_ROI). */ +#ifndef HAVE_ENUM_MAV_ROI +#define HAVE_ENUM_MAV_ROI enum MAV_ROI { MAV_ROI_NONE=0, /* No region of interest. | */ @@ -284,8 +275,11 @@ enum MAV_ROI MAV_ROI_TARGET=4, /* Point toward of given id. | */ MAV_ROI_ENUM_END=5, /* | */ }; +#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 { MAV_CMD_ACK_OK=1, /* Command / mission item is ok. | */ @@ -299,8 +293,11 @@ enum MAV_CMD_ACK 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, /* | */ }; +#endif /** @brief type of a mavlink parameter */ +#ifndef HAVE_ENUM_MAV_VAR +#define HAVE_ENUM_MAV_VAR enum MAV_VAR { MAV_VAR_FLOAT=0, /* 32 bit float | */ @@ -312,8 +309,11 @@ enum MAV_VAR MAV_VAR_INT32=6, /* 32 bit signed integer | */ MAV_VAR_ENUM_END=7, /* | */ }; +#endif /** @brief result from a mavlink command */ +#ifndef HAVE_ENUM_MAV_RESULT +#define HAVE_ENUM_MAV_RESULT enum MAV_RESULT { MAV_RESULT_ACCEPTED=0, /* Command ACCEPTED and EXECUTED | */ @@ -323,8 +323,11 @@ enum MAV_RESULT MAV_RESULT_FAILED=4, /* Command executed, but failed | */ MAV_RESULT_ENUM_END=5, /* | */ }; +#endif /** @brief result in a mavlink mission ack */ +#ifndef HAVE_ENUM_MAV_MISSION_RESULT +#define HAVE_ENUM_MAV_MISSION_RESULT enum MAV_MISSION_RESULT { MAV_MISSION_ACCEPTED=0, /* mission accepted OK | */ @@ -344,6 +347,7 @@ enum MAV_MISSION_RESULT MAV_MISSION_DENIED=14, /* not accepting any mission commands from this communication partner | */ MAV_MISSION_RESULT_ENUM_END=15, /* | */ }; +#endif // MESSAGE DEFINITIONS #include "./mavlink_msg_heartbeat.h" @@ -368,9 +372,11 @@ enum MAV_MISSION_RESULT #include "./mavlink_msg_attitude_quaternion.h" #include "./mavlink_msg_local_position_ned.h" #include "./mavlink_msg_global_position_int.h" -#include "./mavlink_msg_rc_channels_raw.h" #include "./mavlink_msg_rc_channels_scaled.h" +#include "./mavlink_msg_rc_channels_raw.h" #include "./mavlink_msg_servo_output_raw.h" +#include "./mavlink_msg_mission_request_partial_list.h" +#include "./mavlink_msg_mission_write_partial_list.h" #include "./mavlink_msg_mission_item.h" #include "./mavlink_msg_mission_request.h" #include "./mavlink_msg_mission_set_current.h" @@ -399,7 +405,6 @@ enum MAV_MISSION_RESULT #include "./mavlink_msg_manual_control.h" #include "./mavlink_msg_rc_channels_override.h" #include "./mavlink_msg_vfr_hud.h" -#include "./mavlink_msg_command_short.h" #include "./mavlink_msg_command_long.h" #include "./mavlink_msg_command_ack.h" #include "./mavlink_msg_hil_state.h" diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_command_long.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_command_long.h index 44fd8c10bf..7a8bac533f 100644 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_command_long.h +++ b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_command_long.h @@ -11,14 +11,14 @@ typedef struct __mavlink_command_long_t float param5; ///< Parameter 5, as defined by MAV_CMD enum. float param6; ///< Parameter 6, as defined by MAV_CMD enum. float param7; ///< Parameter 7, as defined by MAV_CMD enum. + uint16_t command; ///< Command ID, as defined by MAV_CMD enum. uint8_t target_system; ///< System which should execute the command uint8_t target_component; ///< Component which should execute the command, 0 for all components - uint8_t command; ///< Command ID, as defined by MAV_CMD enum. uint8_t confirmation; ///< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) } mavlink_command_long_t; -#define MAVLINK_MSG_ID_COMMAND_LONG_LEN 32 -#define MAVLINK_MSG_ID_76_LEN 32 +#define MAVLINK_MSG_ID_COMMAND_LONG_LEN 33 +#define MAVLINK_MSG_ID_76_LEN 33 @@ -32,10 +32,10 @@ typedef struct __mavlink_command_long_t { "param5", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_command_long_t, param5) }, \ { "param6", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_command_long_t, param6) }, \ { "param7", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_command_long_t, param7) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_command_long_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_command_long_t, target_component) }, \ - { "command", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_command_long_t, command) }, \ - { "confirmation", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_command_long_t, confirmation) }, \ + { "command", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_command_long_t, command) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_command_long_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_command_long_t, target_component) }, \ + { "confirmation", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_command_long_t, confirmation) }, \ } \ } @@ -60,10 +60,10 @@ typedef struct __mavlink_command_long_t * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_command_long_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7) + 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[32]; + char buf[33]; _mav_put_float(buf, 0, param1); _mav_put_float(buf, 4, param2); _mav_put_float(buf, 8, param3); @@ -71,12 +71,12 @@ static inline uint16_t mavlink_msg_command_long_pack(uint8_t system_id, uint8_t _mav_put_float(buf, 16, param5); _mav_put_float(buf, 20, param6); _mav_put_float(buf, 24, param7); - _mav_put_uint8_t(buf, 28, target_system); - _mav_put_uint8_t(buf, 29, target_component); - _mav_put_uint8_t(buf, 30, command); - _mav_put_uint8_t(buf, 31, confirmation); + _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); - memcpy(_MAV_PAYLOAD(msg), buf, 32); + memcpy(_MAV_PAYLOAD(msg), buf, 33); #else mavlink_command_long_t packet; packet.param1 = param1; @@ -86,16 +86,16 @@ static inline uint16_t mavlink_msg_command_long_pack(uint8_t system_id, uint8_t packet.param5 = param5; packet.param6 = param6; packet.param7 = param7; + packet.command = command; packet.target_system = target_system; packet.target_component = target_component; - packet.command = command; packet.confirmation = confirmation; - memcpy(_MAV_PAYLOAD(msg), &packet, 32); + memcpy(_MAV_PAYLOAD(msg), &packet, 33); #endif msg->msgid = MAVLINK_MSG_ID_COMMAND_LONG; - return mavlink_finalize_message(msg, system_id, component_id, 32, 168); + return mavlink_finalize_message(msg, system_id, component_id, 33, 152); } /** @@ -119,10 +119,10 @@ static inline uint16_t mavlink_msg_command_long_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_command_long_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,uint8_t confirmation,float param1,float param2,float param3,float param4,float param5,float param6,float param7) + 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[32]; + char buf[33]; _mav_put_float(buf, 0, param1); _mav_put_float(buf, 4, param2); _mav_put_float(buf, 8, param3); @@ -130,12 +130,12 @@ static inline uint16_t mavlink_msg_command_long_pack_chan(uint8_t system_id, uin _mav_put_float(buf, 16, param5); _mav_put_float(buf, 20, param6); _mav_put_float(buf, 24, param7); - _mav_put_uint8_t(buf, 28, target_system); - _mav_put_uint8_t(buf, 29, target_component); - _mav_put_uint8_t(buf, 30, command); - _mav_put_uint8_t(buf, 31, confirmation); + _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); - memcpy(_MAV_PAYLOAD(msg), buf, 32); + memcpy(_MAV_PAYLOAD(msg), buf, 33); #else mavlink_command_long_t packet; packet.param1 = param1; @@ -145,16 +145,16 @@ static inline uint16_t mavlink_msg_command_long_pack_chan(uint8_t system_id, uin packet.param5 = param5; packet.param6 = param6; packet.param7 = param7; + packet.command = command; packet.target_system = target_system; packet.target_component = target_component; - packet.command = command; packet.confirmation = confirmation; - memcpy(_MAV_PAYLOAD(msg), &packet, 32); + memcpy(_MAV_PAYLOAD(msg), &packet, 33); #endif msg->msgid = MAVLINK_MSG_ID_COMMAND_LONG; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 168); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 33, 152); } /** @@ -188,10 +188,10 @@ static inline uint16_t mavlink_msg_command_long_encode(uint8_t system_id, uint8_ */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_command_long_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7) +static inline void mavlink_msg_command_long_send(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[32]; + char buf[33]; _mav_put_float(buf, 0, param1); _mav_put_float(buf, 4, param2); _mav_put_float(buf, 8, param3); @@ -199,12 +199,12 @@ static inline void mavlink_msg_command_long_send(mavlink_channel_t chan, uint8_t _mav_put_float(buf, 16, param5); _mav_put_float(buf, 20, param6); _mav_put_float(buf, 24, param7); - _mav_put_uint8_t(buf, 28, target_system); - _mav_put_uint8_t(buf, 29, target_component); - _mav_put_uint8_t(buf, 30, command); - _mav_put_uint8_t(buf, 31, confirmation); + _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); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, buf, 32, 168); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, buf, 33, 152); #else mavlink_command_long_t packet; packet.param1 = param1; @@ -214,12 +214,12 @@ static inline void mavlink_msg_command_long_send(mavlink_channel_t chan, uint8_t packet.param5 = param5; packet.param6 = param6; packet.param7 = param7; + packet.command = command; packet.target_system = target_system; packet.target_component = target_component; - packet.command = command; packet.confirmation = confirmation; - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, (const char *)&packet, 32, 168); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, (const char *)&packet, 33, 152); #endif } @@ -235,7 +235,7 @@ static inline void mavlink_msg_command_long_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_command_long_get_target_system(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 28); + return _MAV_RETURN_uint8_t(msg, 30); } /** @@ -245,7 +245,7 @@ static inline uint8_t mavlink_msg_command_long_get_target_system(const mavlink_m */ static inline uint8_t mavlink_msg_command_long_get_target_component(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 29); + return _MAV_RETURN_uint8_t(msg, 31); } /** @@ -253,9 +253,9 @@ static inline uint8_t mavlink_msg_command_long_get_target_component(const mavlin * * @return Command ID, as defined by MAV_CMD enum. */ -static inline uint8_t mavlink_msg_command_long_get_command(const mavlink_message_t* msg) +static inline uint16_t mavlink_msg_command_long_get_command(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 30); + return _MAV_RETURN_uint16_t(msg, 28); } /** @@ -265,7 +265,7 @@ static inline uint8_t mavlink_msg_command_long_get_command(const mavlink_message */ static inline uint8_t mavlink_msg_command_long_get_confirmation(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 31); + return _MAV_RETURN_uint8_t(msg, 32); } /** @@ -354,11 +354,11 @@ static inline void mavlink_msg_command_long_decode(const mavlink_message_t* msg, command_long->param5 = mavlink_msg_command_long_get_param5(msg); command_long->param6 = mavlink_msg_command_long_get_param6(msg); command_long->param7 = mavlink_msg_command_long_get_param7(msg); + command_long->command = mavlink_msg_command_long_get_command(msg); command_long->target_system = mavlink_msg_command_long_get_target_system(msg); command_long->target_component = mavlink_msg_command_long_get_target_component(msg); - command_long->command = mavlink_msg_command_long_get_command(msg); command_long->confirmation = mavlink_msg_command_long_get_confirmation(msg); #else - memcpy(command_long, _MAV_PAYLOAD(msg), 32); + memcpy(command_long, _MAV_PAYLOAD(msg), 33); #endif } diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_command_short.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_command_short.h deleted file mode 100644 index f4a087b3e1..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_command_short.h +++ /dev/null @@ -1,298 +0,0 @@ -// MESSAGE COMMAND_SHORT PACKING - -#define MAVLINK_MSG_ID_COMMAND_SHORT 75 - -typedef struct __mavlink_command_short_t -{ - float param1; ///< Parameter 1, as defined by MAV_CMD enum. - float param2; ///< Parameter 2, as defined by MAV_CMD enum. - float param3; ///< Parameter 3, as defined by MAV_CMD enum. - float param4; ///< Parameter 4, as defined by MAV_CMD enum. - uint8_t target_system; ///< System which should execute the command - uint8_t target_component; ///< Component which should execute the command, 0 for all components - uint8_t command; ///< Command ID, as defined by MAV_CMD enum. - uint8_t confirmation; ///< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) -} mavlink_command_short_t; - -#define MAVLINK_MSG_ID_COMMAND_SHORT_LEN 20 -#define MAVLINK_MSG_ID_75_LEN 20 - - - -#define MAVLINK_MESSAGE_INFO_COMMAND_SHORT { \ - "COMMAND_SHORT", \ - 8, \ - { { "param1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_command_short_t, param1) }, \ - { "param2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_command_short_t, param2) }, \ - { "param3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_command_short_t, param3) }, \ - { "param4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_command_short_t, param4) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_command_short_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_command_short_t, target_component) }, \ - { "command", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_command_short_t, command) }, \ - { "confirmation", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_command_short_t, confirmation) }, \ - } \ -} - - -/** - * @brief Pack a command_short 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 which should execute the command - * @param target_component Component which should execute the command, 0 for all components - * @param command Command ID, as defined by MAV_CMD enum. - * @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - * @param param1 Parameter 1, as defined by MAV_CMD enum. - * @param param2 Parameter 2, as defined by MAV_CMD enum. - * @param param3 Parameter 3, as defined by MAV_CMD enum. - * @param param4 Parameter 4, as defined by MAV_CMD enum. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_command_short_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[20]; - _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_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - _mav_put_uint8_t(buf, 18, command); - _mav_put_uint8_t(buf, 19, confirmation); - - memcpy(_MAV_PAYLOAD(msg), buf, 20); -#else - mavlink_command_short_t packet; - packet.param1 = param1; - packet.param2 = param2; - packet.param3 = param3; - packet.param4 = param4; - packet.target_system = target_system; - packet.target_component = target_component; - packet.command = command; - packet.confirmation = confirmation; - - memcpy(_MAV_PAYLOAD(msg), &packet, 20); -#endif - - msg->msgid = MAVLINK_MSG_ID_COMMAND_SHORT; - return mavlink_finalize_message(msg, system_id, component_id, 20, 160); -} - -/** - * @brief Pack a command_short 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System which should execute the command - * @param target_component Component which should execute the command, 0 for all components - * @param command Command ID, as defined by MAV_CMD enum. - * @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - * @param param1 Parameter 1, as defined by MAV_CMD enum. - * @param param2 Parameter 2, as defined by MAV_CMD enum. - * @param param3 Parameter 3, as defined by MAV_CMD enum. - * @param param4 Parameter 4, as defined by MAV_CMD enum. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_command_short_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,uint8_t confirmation,float param1,float param2,float param3,float param4) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[20]; - _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_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - _mav_put_uint8_t(buf, 18, command); - _mav_put_uint8_t(buf, 19, confirmation); - - memcpy(_MAV_PAYLOAD(msg), buf, 20); -#else - mavlink_command_short_t packet; - packet.param1 = param1; - packet.param2 = param2; - packet.param3 = param3; - packet.param4 = param4; - packet.target_system = target_system; - packet.target_component = target_component; - packet.command = command; - packet.confirmation = confirmation; - - memcpy(_MAV_PAYLOAD(msg), &packet, 20); -#endif - - msg->msgid = MAVLINK_MSG_ID_COMMAND_SHORT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 160); -} - -/** - * @brief Encode a command_short struct into 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 command_short C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_command_short_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_command_short_t* command_short) -{ - return mavlink_msg_command_short_pack(system_id, component_id, msg, command_short->target_system, command_short->target_component, command_short->command, command_short->confirmation, command_short->param1, command_short->param2, command_short->param3, command_short->param4); -} - -/** - * @brief Send a command_short message - * @param chan MAVLink channel to send the message - * - * @param target_system System which should execute the command - * @param target_component Component which should execute the command, 0 for all components - * @param command Command ID, as defined by MAV_CMD enum. - * @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - * @param param1 Parameter 1, as defined by MAV_CMD enum. - * @param param2 Parameter 2, as defined by MAV_CMD enum. - * @param param3 Parameter 3, as defined by MAV_CMD enum. - * @param param4 Parameter 4, as defined by MAV_CMD enum. - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_command_short_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[20]; - _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_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - _mav_put_uint8_t(buf, 18, command); - _mav_put_uint8_t(buf, 19, confirmation); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_SHORT, buf, 20, 160); -#else - mavlink_command_short_t packet; - packet.param1 = param1; - packet.param2 = param2; - packet.param3 = param3; - packet.param4 = param4; - packet.target_system = target_system; - packet.target_component = target_component; - packet.command = command; - packet.confirmation = confirmation; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_SHORT, (const char *)&packet, 20, 160); -#endif -} - -#endif - -// MESSAGE COMMAND_SHORT UNPACKING - - -/** - * @brief Get field target_system from command_short message - * - * @return System which should execute the command - */ -static inline uint8_t mavlink_msg_command_short_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Get field target_component from command_short message - * - * @return Component which should execute the command, 0 for all components - */ -static inline uint8_t mavlink_msg_command_short_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 17); -} - -/** - * @brief Get field command from command_short message - * - * @return Command ID, as defined by MAV_CMD enum. - */ -static inline uint8_t mavlink_msg_command_short_get_command(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 18); -} - -/** - * @brief Get field confirmation from command_short message - * - * @return 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - */ -static inline uint8_t mavlink_msg_command_short_get_confirmation(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 19); -} - -/** - * @brief Get field param1 from command_short message - * - * @return Parameter 1, as defined by MAV_CMD enum. - */ -static inline float mavlink_msg_command_short_get_param1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field param2 from command_short message - * - * @return Parameter 2, as defined by MAV_CMD enum. - */ -static inline float mavlink_msg_command_short_get_param2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field param3 from command_short message - * - * @return Parameter 3, as defined by MAV_CMD enum. - */ -static inline float mavlink_msg_command_short_get_param3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field param4 from command_short message - * - * @return Parameter 4, as defined by MAV_CMD enum. - */ -static inline float mavlink_msg_command_short_get_param4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a command_short message into a struct - * - * @param msg The message to decode - * @param command_short C-struct to decode the message contents into - */ -static inline void mavlink_msg_command_short_decode(const mavlink_message_t* msg, mavlink_command_short_t* command_short) -{ -#if MAVLINK_NEED_BYTE_SWAP - command_short->param1 = mavlink_msg_command_short_get_param1(msg); - command_short->param2 = mavlink_msg_command_short_get_param2(msg); - command_short->param3 = mavlink_msg_command_short_get_param3(msg); - command_short->param4 = mavlink_msg_command_short_get_param4(msg); - command_short->target_system = mavlink_msg_command_short_get_target_system(msg); - command_short->target_component = mavlink_msg_command_short_get_target_component(msg); - command_short->command = mavlink_msg_command_short_get_command(msg); - command_short->confirmation = mavlink_msg_command_short_get_confirmation(msg); -#else - memcpy(command_short, _MAV_PAYLOAD(msg), 20); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_global_position.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_global_position.h deleted file mode 100644 index 6e902393da..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_global_position.h +++ /dev/null @@ -1,276 +0,0 @@ -// MESSAGE GLOBAL_POSITION PACKING - -#define MAVLINK_MSG_ID_GLOBAL_POSITION 33 - -typedef struct __mavlink_global_position_t -{ - uint64_t usec; ///< Timestamp (microseconds since unix epoch) - float lat; ///< Latitude, in degrees - float lon; ///< Longitude, in degrees - float alt; ///< Absolute altitude, in meters - float vx; ///< X Speed (in Latitude direction, positive: going north) - float vy; ///< Y Speed (in Longitude direction, positive: going east) - float vz; ///< Z Speed (in Altitude direction, positive: going up) -} mavlink_global_position_t; - -#define MAVLINK_MSG_ID_GLOBAL_POSITION_LEN 32 -#define MAVLINK_MSG_ID_33_LEN 32 - - - -#define MAVLINK_MESSAGE_INFO_GLOBAL_POSITION { \ - "GLOBAL_POSITION", \ - 7, \ - { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_global_position_t, usec) }, \ - { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_global_position_t, lat) }, \ - { "lon", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_global_position_t, lon) }, \ - { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_global_position_t, alt) }, \ - { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_global_position_t, vx) }, \ - { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_global_position_t, vy) }, \ - { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_global_position_t, vz) }, \ - } \ -} - - -/** - * @brief Pack a global_position message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param usec Timestamp (microseconds since unix epoch) - * @param lat Latitude, in degrees - * @param lon Longitude, in degrees - * @param alt Absolute altitude, in meters - * @param vx X Speed (in Latitude direction, positive: going north) - * @param vy Y Speed (in Longitude direction, positive: going east) - * @param vz Z Speed (in Altitude direction, positive: going up) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_global_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t usec, float lat, float lon, float alt, float vx, float vy, float vz) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[32]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, lat); - _mav_put_float(buf, 12, lon); - _mav_put_float(buf, 16, alt); - _mav_put_float(buf, 20, vx); - _mav_put_float(buf, 24, vy); - _mav_put_float(buf, 28, vz); - - memcpy(_MAV_PAYLOAD(msg), buf, 32); -#else - mavlink_global_position_t packet; - packet.usec = usec; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - - memcpy(_MAV_PAYLOAD(msg), &packet, 32); -#endif - - msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; - return mavlink_finalize_message(msg, system_id, component_id, 32, 147); -} - -/** - * @brief Pack a global_position message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (microseconds since unix epoch) - * @param lat Latitude, in degrees - * @param lon Longitude, in degrees - * @param alt Absolute altitude, in meters - * @param vx X Speed (in Latitude direction, positive: going north) - * @param vy Y Speed (in Longitude direction, positive: going east) - * @param vz Z Speed (in Altitude direction, positive: going up) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_global_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t usec,float lat,float lon,float alt,float vx,float vy,float vz) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[32]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, lat); - _mav_put_float(buf, 12, lon); - _mav_put_float(buf, 16, alt); - _mav_put_float(buf, 20, vx); - _mav_put_float(buf, 24, vy); - _mav_put_float(buf, 28, vz); - - memcpy(_MAV_PAYLOAD(msg), buf, 32); -#else - mavlink_global_position_t packet; - packet.usec = usec; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - - memcpy(_MAV_PAYLOAD(msg), &packet, 32); -#endif - - msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 147); -} - -/** - * @brief Encode a global_position struct into 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 global_position C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_global_position_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_position_t* global_position) -{ - return mavlink_msg_global_position_pack(system_id, component_id, msg, global_position->usec, global_position->lat, global_position->lon, global_position->alt, global_position->vx, global_position->vy, global_position->vz); -} - -/** - * @brief Send a global_position message - * @param chan MAVLink channel to send the message - * - * @param usec Timestamp (microseconds since unix epoch) - * @param lat Latitude, in degrees - * @param lon Longitude, in degrees - * @param alt Absolute altitude, in meters - * @param vx X Speed (in Latitude direction, positive: going north) - * @param vy Y Speed (in Longitude direction, positive: going east) - * @param vz Z Speed (in Altitude direction, positive: going up) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_global_position_send(mavlink_channel_t chan, uint64_t usec, float lat, float lon, float alt, float vx, float vy, float vz) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[32]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, lat); - _mav_put_float(buf, 12, lon); - _mav_put_float(buf, 16, alt); - _mav_put_float(buf, 20, vx); - _mav_put_float(buf, 24, vy); - _mav_put_float(buf, 28, vz); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION, buf, 32, 147); -#else - mavlink_global_position_t packet; - packet.usec = usec; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION, (const char *)&packet, 32, 147); -#endif -} - -#endif - -// MESSAGE GLOBAL_POSITION UNPACKING - - -/** - * @brief Get field usec from global_position message - * - * @return Timestamp (microseconds since unix epoch) - */ -static inline uint64_t mavlink_msg_global_position_get_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field lat from global_position message - * - * @return Latitude, in degrees - */ -static inline float mavlink_msg_global_position_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field lon from global_position message - * - * @return Longitude, in degrees - */ -static inline float mavlink_msg_global_position_get_lon(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field alt from global_position message - * - * @return Absolute altitude, in meters - */ -static inline float mavlink_msg_global_position_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field vx from global_position message - * - * @return X Speed (in Latitude direction, positive: going north) - */ -static inline float mavlink_msg_global_position_get_vx(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field vy from global_position message - * - * @return Y Speed (in Longitude direction, positive: going east) - */ -static inline float mavlink_msg_global_position_get_vy(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field vz from global_position message - * - * @return Z Speed (in Altitude direction, positive: going up) - */ -static inline float mavlink_msg_global_position_get_vz(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Decode a global_position message into a struct - * - * @param msg The message to decode - * @param global_position C-struct to decode the message contents into - */ -static inline void mavlink_msg_global_position_decode(const mavlink_message_t* msg, mavlink_global_position_t* global_position) -{ -#if MAVLINK_NEED_BYTE_SWAP - global_position->usec = mavlink_msg_global_position_get_usec(msg); - global_position->lat = mavlink_msg_global_position_get_lat(msg); - global_position->lon = mavlink_msg_global_position_get_lon(msg); - global_position->alt = mavlink_msg_global_position_get_alt(msg); - global_position->vx = mavlink_msg_global_position_get_vx(msg); - global_position->vy = mavlink_msg_global_position_get_vy(msg); - global_position->vz = mavlink_msg_global_position_get_vz(msg); -#else - memcpy(global_position, _MAV_PAYLOAD(msg), 32); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_global_position_int.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_global_position_int.h index 3d624bc051..c5be7fec62 100644 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_global_position_int.h +++ b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_global_position_int.h @@ -1,6 +1,6 @@ // MESSAGE GLOBAL_POSITION_INT PACKING -#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT 34 +#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT 33 typedef struct __mavlink_global_position_int_t { @@ -16,7 +16,7 @@ typedef struct __mavlink_global_position_int_t } mavlink_global_position_int_t; #define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN 28 -#define MAVLINK_MSG_ID_34_LEN 28 +#define MAVLINK_MSG_ID_33_LEN 28 diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_gps_raw.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_gps_raw.h deleted file mode 100644 index eb6ca89c5e..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_gps_raw.h +++ /dev/null @@ -1,342 +0,0 @@ -// MESSAGE GPS_RAW PACKING - -#define MAVLINK_MSG_ID_GPS_RAW 32 - -typedef struct __mavlink_gps_raw_t -{ - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - float lat; ///< Latitude in degrees - float lon; ///< Longitude in degrees - float alt; ///< Altitude in meters - float eph; ///< GPS HDOP - float epv; ///< GPS VDOP - float v; ///< GPS ground speed - float hdg; ///< Compass heading in degrees, 0..360 degrees - uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - uint8_t satellites_visible; ///< Number of satellites visible -} mavlink_gps_raw_t; - -#define MAVLINK_MSG_ID_GPS_RAW_LEN 38 -#define MAVLINK_MSG_ID_32_LEN 38 - - - -#define MAVLINK_MESSAGE_INFO_GPS_RAW { \ - "GPS_RAW", \ - 10, \ - { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps_raw_t, usec) }, \ - { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_gps_raw_t, lat) }, \ - { "lon", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_gps_raw_t, lon) }, \ - { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gps_raw_t, alt) }, \ - { "eph", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gps_raw_t, eph) }, \ - { "epv", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gps_raw_t, epv) }, \ - { "v", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gps_raw_t, v) }, \ - { "hdg", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_gps_raw_t, hdg) }, \ - { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_gps_raw_t, fix_type) }, \ - { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_gps_raw_t, satellites_visible) }, \ - } \ -} - - -/** - * @brief Pack a gps_raw 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 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 lat Latitude in degrees - * @param lon Longitude in degrees - * @param alt Altitude in meters - * @param eph GPS HDOP - * @param epv GPS VDOP - * @param v GPS ground speed - * @param hdg Compass heading in degrees, 0..360 degrees - * @param satellites_visible Number of satellites visible - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg, uint8_t satellites_visible) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[38]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, lat); - _mav_put_float(buf, 12, lon); - _mav_put_float(buf, 16, alt); - _mav_put_float(buf, 20, eph); - _mav_put_float(buf, 24, epv); - _mav_put_float(buf, 28, v); - _mav_put_float(buf, 32, hdg); - _mav_put_uint8_t(buf, 36, fix_type); - _mav_put_uint8_t(buf, 37, satellites_visible); - - memcpy(_MAV_PAYLOAD(msg), buf, 38); -#else - mavlink_gps_raw_t packet; - packet.usec = usec; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.eph = eph; - packet.epv = epv; - packet.v = v; - packet.hdg = hdg; - packet.fix_type = fix_type; - packet.satellites_visible = satellites_visible; - - memcpy(_MAV_PAYLOAD(msg), &packet, 38); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS_RAW; - return mavlink_finalize_message(msg, system_id, component_id, 38, 198); -} - -/** - * @brief Pack a gps_raw 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param 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 lat Latitude in degrees - * @param lon Longitude in degrees - * @param alt Altitude in meters - * @param eph GPS HDOP - * @param epv GPS VDOP - * @param v GPS ground speed - * @param hdg Compass heading in degrees, 0..360 degrees - * @param satellites_visible Number of satellites visible - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t usec,uint8_t fix_type,float lat,float lon,float alt,float eph,float epv,float v,float hdg,uint8_t satellites_visible) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[38]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, lat); - _mav_put_float(buf, 12, lon); - _mav_put_float(buf, 16, alt); - _mav_put_float(buf, 20, eph); - _mav_put_float(buf, 24, epv); - _mav_put_float(buf, 28, v); - _mav_put_float(buf, 32, hdg); - _mav_put_uint8_t(buf, 36, fix_type); - _mav_put_uint8_t(buf, 37, satellites_visible); - - memcpy(_MAV_PAYLOAD(msg), buf, 38); -#else - mavlink_gps_raw_t packet; - packet.usec = usec; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.eph = eph; - packet.epv = epv; - packet.v = v; - packet.hdg = hdg; - packet.fix_type = fix_type; - packet.satellites_visible = satellites_visible; - - memcpy(_MAV_PAYLOAD(msg), &packet, 38); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS_RAW; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 38, 198); -} - -/** - * @brief Encode a gps_raw struct into 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 gps_raw C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_gps_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_raw_t* gps_raw) -{ - return mavlink_msg_gps_raw_pack(system_id, component_id, msg, gps_raw->usec, gps_raw->fix_type, gps_raw->lat, gps_raw->lon, gps_raw->alt, gps_raw->eph, gps_raw->epv, gps_raw->v, gps_raw->hdg, gps_raw->satellites_visible); -} - -/** - * @brief Send a gps_raw message - * @param chan MAVLink channel to send the message - * - * @param 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 lat Latitude in degrees - * @param lon Longitude in degrees - * @param alt Altitude in meters - * @param eph GPS HDOP - * @param epv GPS VDOP - * @param v GPS ground speed - * @param hdg Compass heading in degrees, 0..360 degrees - * @param satellites_visible Number of satellites visible - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_gps_raw_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg, uint8_t satellites_visible) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[38]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, lat); - _mav_put_float(buf, 12, lon); - _mav_put_float(buf, 16, alt); - _mav_put_float(buf, 20, eph); - _mav_put_float(buf, 24, epv); - _mav_put_float(buf, 28, v); - _mav_put_float(buf, 32, hdg); - _mav_put_uint8_t(buf, 36, fix_type); - _mav_put_uint8_t(buf, 37, satellites_visible); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW, buf, 38, 198); -#else - mavlink_gps_raw_t packet; - packet.usec = usec; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.eph = eph; - packet.epv = epv; - packet.v = v; - packet.hdg = hdg; - packet.fix_type = fix_type; - packet.satellites_visible = satellites_visible; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW, (const char *)&packet, 38, 198); -#endif -} - -#endif - -// MESSAGE GPS_RAW UNPACKING - - -/** - * @brief Get field usec from gps_raw message - * - * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) - */ -static inline uint64_t mavlink_msg_gps_raw_get_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field fix_type from gps_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. - */ -static inline uint8_t mavlink_msg_gps_raw_get_fix_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 36); -} - -/** - * @brief Get field lat from gps_raw message - * - * @return Latitude in degrees - */ -static inline float mavlink_msg_gps_raw_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field lon from gps_raw message - * - * @return Longitude in degrees - */ -static inline float mavlink_msg_gps_raw_get_lon(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field alt from gps_raw message - * - * @return Altitude in meters - */ -static inline float mavlink_msg_gps_raw_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field eph from gps_raw message - * - * @return GPS HDOP - */ -static inline float mavlink_msg_gps_raw_get_eph(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field epv from gps_raw message - * - * @return GPS VDOP - */ -static inline float mavlink_msg_gps_raw_get_epv(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field v from gps_raw message - * - * @return GPS ground speed - */ -static inline float mavlink_msg_gps_raw_get_v(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Get field hdg from gps_raw message - * - * @return Compass heading in degrees, 0..360 degrees - */ -static inline float mavlink_msg_gps_raw_get_hdg(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 32); -} - -/** - * @brief Get field satellites_visible from gps_raw message - * - * @return Number of satellites visible - */ -static inline uint8_t mavlink_msg_gps_raw_get_satellites_visible(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 37); -} - -/** - * @brief Decode a gps_raw message into a struct - * - * @param msg The message to decode - * @param gps_raw C-struct to decode the message contents into - */ -static inline void mavlink_msg_gps_raw_decode(const mavlink_message_t* msg, mavlink_gps_raw_t* gps_raw) -{ -#if MAVLINK_NEED_BYTE_SWAP - gps_raw->usec = mavlink_msg_gps_raw_get_usec(msg); - gps_raw->lat = mavlink_msg_gps_raw_get_lat(msg); - gps_raw->lon = mavlink_msg_gps_raw_get_lon(msg); - gps_raw->alt = mavlink_msg_gps_raw_get_alt(msg); - gps_raw->eph = mavlink_msg_gps_raw_get_eph(msg); - gps_raw->epv = mavlink_msg_gps_raw_get_epv(msg); - gps_raw->v = mavlink_msg_gps_raw_get_v(msg); - gps_raw->hdg = mavlink_msg_gps_raw_get_hdg(msg); - gps_raw->fix_type = mavlink_msg_gps_raw_get_fix_type(msg); - gps_raw->satellites_visible = mavlink_msg_gps_raw_get_satellites_visible(msg); -#else - memcpy(gps_raw, _MAV_PAYLOAD(msg), 38); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_gps_set_global_origin.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_gps_set_global_origin.h deleted file mode 100644 index 97ce149078..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_gps_set_global_origin.h +++ /dev/null @@ -1,232 +0,0 @@ -// MESSAGE GPS_SET_GLOBAL_ORIGIN PACKING - -#define MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN 48 - -typedef struct __mavlink_gps_set_global_origin_t -{ - int32_t latitude; ///< global position * 1E7 - int32_t longitude; ///< global position * 1E7 - int32_t altitude; ///< global position * 1000 - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_gps_set_global_origin_t; - -#define MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN 14 -#define MAVLINK_MSG_ID_48_LEN 14 - - - -#define MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN { \ - "GPS_SET_GLOBAL_ORIGIN", \ - 5, \ - { { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_gps_set_global_origin_t, latitude) }, \ - { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_gps_set_global_origin_t, longitude) }, \ - { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_set_global_origin_t, altitude) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_gps_set_global_origin_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_gps_set_global_origin_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a gps_set_global_origin 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 latitude global position * 1E7 - * @param longitude global position * 1E7 - * @param altitude global position * 1000 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps_set_global_origin_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, int32_t latitude, int32_t longitude, int32_t altitude) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[14]; - _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); - _mav_put_uint8_t(buf, 13, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 14); -#else - mavlink_gps_set_global_origin_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 14); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; - return mavlink_finalize_message(msg, system_id, component_id, 14, 170); -} - -/** - * @brief Pack a gps_set_global_origin 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param latitude global position * 1E7 - * @param longitude global position * 1E7 - * @param altitude global position * 1000 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps_set_global_origin_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 latitude,int32_t longitude,int32_t altitude) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[14]; - _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); - _mav_put_uint8_t(buf, 13, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 14); -#else - mavlink_gps_set_global_origin_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 14); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 170); -} - -/** - * @brief Encode a gps_set_global_origin struct into 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 gps_set_global_origin C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_gps_set_global_origin_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_set_global_origin_t* gps_set_global_origin) -{ - return mavlink_msg_gps_set_global_origin_pack(system_id, component_id, msg, gps_set_global_origin->target_system, gps_set_global_origin->target_component, gps_set_global_origin->latitude, gps_set_global_origin->longitude, gps_set_global_origin->altitude); -} - -/** - * @brief Send a gps_set_global_origin message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param latitude global position * 1E7 - * @param longitude global position * 1E7 - * @param altitude global position * 1000 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_gps_set_global_origin_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t latitude, int32_t longitude, int32_t altitude) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[14]; - _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); - _mav_put_uint8_t(buf, 13, target_component); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN, buf, 14, 170); -#else - mavlink_gps_set_global_origin_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - packet.target_system = target_system; - packet.target_component = target_component; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN, (const char *)&packet, 14, 170); -#endif -} - -#endif - -// MESSAGE GPS_SET_GLOBAL_ORIGIN UNPACKING - - -/** - * @brief Get field target_system from gps_set_global_origin message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_gps_set_global_origin_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 12); -} - -/** - * @brief Get field target_component from gps_set_global_origin message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_gps_set_global_origin_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 13); -} - -/** - * @brief Get field latitude from gps_set_global_origin message - * - * @return global position * 1E7 - */ -static inline int32_t mavlink_msg_gps_set_global_origin_get_latitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field longitude from gps_set_global_origin message - * - * @return global position * 1E7 - */ -static inline int32_t mavlink_msg_gps_set_global_origin_get_longitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field altitude from gps_set_global_origin message - * - * @return global position * 1000 - */ -static inline int32_t mavlink_msg_gps_set_global_origin_get_altitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Decode a gps_set_global_origin message into a struct - * - * @param msg The message to decode - * @param gps_set_global_origin C-struct to decode the message contents into - */ -static inline void mavlink_msg_gps_set_global_origin_decode(const mavlink_message_t* msg, mavlink_gps_set_global_origin_t* gps_set_global_origin) -{ -#if MAVLINK_NEED_BYTE_SWAP - gps_set_global_origin->latitude = mavlink_msg_gps_set_global_origin_get_latitude(msg); - gps_set_global_origin->longitude = mavlink_msg_gps_set_global_origin_get_longitude(msg); - gps_set_global_origin->altitude = mavlink_msg_gps_set_global_origin_get_altitude(msg); - gps_set_global_origin->target_system = mavlink_msg_gps_set_global_origin_get_target_system(msg); - gps_set_global_origin->target_component = mavlink_msg_gps_set_global_origin_get_target_component(msg); -#else - memcpy(gps_set_global_origin, _MAV_PAYLOAD(msg), 14); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_count.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_count.h index e93c1e8677..967071d8c8 100644 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_count.h +++ b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_count.h @@ -4,7 +4,7 @@ typedef struct __mavlink_mission_count_t { - uint16_t count; ///< Number of MISSIONs in the Sequence + uint16_t count; ///< Number of mission items in the sequence uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID } mavlink_mission_count_t; @@ -32,7 +32,7 @@ typedef struct __mavlink_mission_count_t * * @param target_system System ID * @param target_component Component ID - * @param count Number of MISSIONs in the Sequence + * @param count Number of mission items in the sequence * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_count_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -66,7 +66,7 @@ static inline uint16_t mavlink_msg_mission_count_pack(uint8_t system_id, uint8_t * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID - * @param count Number of MISSIONs in the Sequence + * @param count Number of mission items in the sequence * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_count_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -112,7 +112,7 @@ static inline uint16_t mavlink_msg_mission_count_encode(uint8_t system_id, uint8 * * @param target_system System ID * @param target_component Component ID - * @param count Number of MISSIONs in the Sequence + * @param count Number of mission items in the sequence */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -163,7 +163,7 @@ static inline uint8_t mavlink_msg_mission_count_get_target_component(const mavli /** * @brief Get field count from mission_count message * - * @return Number of MISSIONs in the Sequence + * @return Number of mission items in the sequence */ static inline uint16_t mavlink_msg_mission_count_get_count(const mavlink_message_t* msg) { diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_item.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_item.h index 77f106cb01..e2d894aeea 100644 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_item.h +++ b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_item.h @@ -12,16 +12,16 @@ typedef struct __mavlink_mission_item_t float y; ///< PARAM6 / y position: global: longitude float z; ///< PARAM7 / z position: global: altitude uint16_t seq; ///< Sequence + 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 command; ///< The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs uint8_t current; ///< false:0, true:1 uint8_t autocontinue; ///< autocontinue to next wp } mavlink_mission_item_t; -#define MAVLINK_MSG_ID_MISSION_ITEM_LEN 36 -#define MAVLINK_MSG_ID_39_LEN 36 +#define MAVLINK_MSG_ID_MISSION_ITEM_LEN 37 +#define MAVLINK_MSG_ID_39_LEN 37 @@ -36,12 +36,12 @@ typedef struct __mavlink_mission_item_t { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_mission_item_t, y) }, \ { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mission_item_t, z) }, \ { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_mission_item_t, seq) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_mission_item_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_mission_item_t, target_component) }, \ - { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_mission_item_t, frame) }, \ - { "command", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_mission_item_t, command) }, \ - { "current", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_mission_item_t, current) }, \ - { "autocontinue", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_mission_item_t, autocontinue) }, \ + { "command", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_mission_item_t, command) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_mission_item_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_mission_item_t, target_component) }, \ + { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_mission_item_t, frame) }, \ + { "current", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_mission_item_t, current) }, \ + { "autocontinue", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_mission_item_t, autocontinue) }, \ } \ } @@ -69,10 +69,10 @@ typedef struct __mavlink_mission_item_t * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) + uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[36]; + char buf[37]; _mav_put_float(buf, 0, param1); _mav_put_float(buf, 4, param2); _mav_put_float(buf, 8, param3); @@ -81,14 +81,14 @@ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t _mav_put_float(buf, 20, y); _mav_put_float(buf, 24, z); _mav_put_uint16_t(buf, 28, seq); - _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, command); - _mav_put_uint8_t(buf, 34, current); - _mav_put_uint8_t(buf, 35, autocontinue); + _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(msg), buf, 36); + memcpy(_MAV_PAYLOAD(msg), buf, 37); #else mavlink_mission_item_t packet; packet.param1 = param1; @@ -99,18 +99,18 @@ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t 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.command = command; packet.current = current; packet.autocontinue = autocontinue; - memcpy(_MAV_PAYLOAD(msg), &packet, 36); + memcpy(_MAV_PAYLOAD(msg), &packet, 37); #endif msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM; - return mavlink_finalize_message(msg, system_id, component_id, 36, 158); + return mavlink_finalize_message(msg, system_id, component_id, 37, 254); } /** @@ -137,10 +137,10 @@ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t seq,uint8_t frame,uint8_t command,uint8_t current,uint8_t autocontinue,float param1,float param2,float param3,float param4,float x,float y,float z) + uint8_t target_system,uint8_t target_component,uint16_t seq,uint8_t frame,uint16_t command,uint8_t current,uint8_t autocontinue,float param1,float param2,float param3,float param4,float x,float y,float z) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[36]; + char buf[37]; _mav_put_float(buf, 0, param1); _mav_put_float(buf, 4, param2); _mav_put_float(buf, 8, param3); @@ -149,14 +149,14 @@ static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uin _mav_put_float(buf, 20, y); _mav_put_float(buf, 24, z); _mav_put_uint16_t(buf, 28, seq); - _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, command); - _mav_put_uint8_t(buf, 34, current); - _mav_put_uint8_t(buf, 35, autocontinue); + _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(msg), buf, 36); + memcpy(_MAV_PAYLOAD(msg), buf, 37); #else mavlink_mission_item_t packet; packet.param1 = param1; @@ -167,18 +167,18 @@ static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uin 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.command = command; packet.current = current; packet.autocontinue = autocontinue; - memcpy(_MAV_PAYLOAD(msg), &packet, 36); + memcpy(_MAV_PAYLOAD(msg), &packet, 37); #endif msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36, 158); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 37, 254); } /** @@ -215,10 +215,10 @@ static inline uint16_t mavlink_msg_mission_item_encode(uint8_t system_id, uint8_ */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) +static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[36]; + char buf[37]; _mav_put_float(buf, 0, param1); _mav_put_float(buf, 4, param2); _mav_put_float(buf, 8, param3); @@ -227,14 +227,14 @@ static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t _mav_put_float(buf, 20, y); _mav_put_float(buf, 24, z); _mav_put_uint16_t(buf, 28, seq); - _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, command); - _mav_put_uint8_t(buf, 34, current); - _mav_put_uint8_t(buf, 35, autocontinue); + _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); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, 36, 158); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, 37, 254); #else mavlink_mission_item_t packet; packet.param1 = param1; @@ -245,14 +245,14 @@ static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t 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.command = command; packet.current = current; packet.autocontinue = autocontinue; - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)&packet, 36, 158); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)&packet, 37, 254); #endif } @@ -268,7 +268,7 @@ static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_mission_item_get_target_system(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 30); + return _MAV_RETURN_uint8_t(msg, 32); } /** @@ -278,7 +278,7 @@ static inline uint8_t mavlink_msg_mission_item_get_target_system(const mavlink_m */ static inline uint8_t mavlink_msg_mission_item_get_target_component(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 31); + return _MAV_RETURN_uint8_t(msg, 33); } /** @@ -298,7 +298,7 @@ static inline uint16_t mavlink_msg_mission_item_get_seq(const mavlink_message_t* */ static inline uint8_t mavlink_msg_mission_item_get_frame(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 32); + return _MAV_RETURN_uint8_t(msg, 34); } /** @@ -306,9 +306,9 @@ static inline uint8_t mavlink_msg_mission_item_get_frame(const mavlink_message_t * * @return The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs */ -static inline uint8_t mavlink_msg_mission_item_get_command(const mavlink_message_t* msg) +static inline uint16_t mavlink_msg_mission_item_get_command(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 33); + return _MAV_RETURN_uint16_t(msg, 30); } /** @@ -318,7 +318,7 @@ static inline uint8_t mavlink_msg_mission_item_get_command(const mavlink_message */ static inline uint8_t mavlink_msg_mission_item_get_current(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 34); + return _MAV_RETURN_uint8_t(msg, 35); } /** @@ -328,7 +328,7 @@ static inline uint8_t mavlink_msg_mission_item_get_current(const mavlink_message */ static inline uint8_t mavlink_msg_mission_item_get_autocontinue(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 35); + return _MAV_RETURN_uint8_t(msg, 36); } /** @@ -418,13 +418,13 @@ static inline void mavlink_msg_mission_item_decode(const mavlink_message_t* msg, mission_item->y = mavlink_msg_mission_item_get_y(msg); mission_item->z = mavlink_msg_mission_item_get_z(msg); mission_item->seq = mavlink_msg_mission_item_get_seq(msg); + mission_item->command = mavlink_msg_mission_item_get_command(msg); mission_item->target_system = mavlink_msg_mission_item_get_target_system(msg); mission_item->target_component = mavlink_msg_mission_item_get_target_component(msg); mission_item->frame = mavlink_msg_mission_item_get_frame(msg); - mission_item->command = mavlink_msg_mission_item_get_command(msg); mission_item->current = mavlink_msg_mission_item_get_current(msg); mission_item->autocontinue = mavlink_msg_mission_item_get_autocontinue(msg); #else - memcpy(mission_item, _MAV_PAYLOAD(msg), 36); + memcpy(mission_item, _MAV_PAYLOAD(msg), 37); #endif } diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_request_partial_list.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_request_partial_list.h new file mode 100644 index 0000000000..92e0ea745c --- /dev/null +++ b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_request_partial_list.h @@ -0,0 +1,210 @@ +// MESSAGE MISSION_REQUEST_PARTIAL_LIST PACKING + +#define MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST 37 + +typedef struct __mavlink_mission_request_partial_list_t +{ + int16_t start_index; ///< Start index, 0 by default + int16_t end_index; ///< End index, -1 by default (-1: send list to end). Else a valid index of the list + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_mission_request_partial_list_t; + +#define MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN 6 +#define MAVLINK_MSG_ID_37_LEN 6 + + + +#define MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST { \ + "MISSION_REQUEST_PARTIAL_LIST", \ + 4, \ + { { "start_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_mission_request_partial_list_t, start_index) }, \ + { "end_index", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_mission_request_partial_list_t, end_index) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_request_partial_list_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mission_request_partial_list_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a mission_request_partial_list message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param start_index Start index, 0 by default + * @param end_index End index, -1 by default (-1: send list to end). Else a valid index of the list + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_request_partial_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[6]; + _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); + + memcpy(_MAV_PAYLOAD(msg), buf, 6); +#else + mavlink_mission_request_partial_list_t packet; + packet.start_index = start_index; + packet.end_index = end_index; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 6); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST; + return mavlink_finalize_message(msg, system_id, component_id, 6, 212); +} + +/** + * @brief Pack a mission_request_partial_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 was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param start_index Start index, 0 by default + * @param end_index End index, -1 by default (-1: send list to end). Else a valid index of the list + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_request_partial_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,int16_t start_index,int16_t end_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[6]; + _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); + + memcpy(_MAV_PAYLOAD(msg), buf, 6); +#else + mavlink_mission_request_partial_list_t packet; + packet.start_index = start_index; + packet.end_index = end_index; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 6); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 6, 212); +} + +/** + * @brief Encode a mission_request_partial_list struct into 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 mission_request_partial_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_request_partial_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_request_partial_list_t* mission_request_partial_list) +{ + return mavlink_msg_mission_request_partial_list_pack(system_id, component_id, msg, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index); +} + +/** + * @brief Send a mission_request_partial_list message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param start_index Start index, 0 by default + * @param end_index End index, -1 by default (-1: send list to end). Else a valid index of the list + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mission_request_partial_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[6]; + _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); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, 6, 212); +#else + mavlink_mission_request_partial_list_t packet; + packet.start_index = start_index; + packet.end_index = end_index; + packet.target_system = target_system; + packet.target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)&packet, 6, 212); +#endif +} + +#endif + +// MESSAGE MISSION_REQUEST_PARTIAL_LIST UNPACKING + + +/** + * @brief Get field target_system from mission_request_partial_list message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_mission_request_partial_list_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field target_component from mission_request_partial_list message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_mission_request_partial_list_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field start_index from mission_request_partial_list message + * + * @return Start index, 0 by default + */ +static inline int16_t mavlink_msg_mission_request_partial_list_get_start_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 0); +} + +/** + * @brief Get field end_index from mission_request_partial_list message + * + * @return End index, -1 by default (-1: send list to end). Else a valid index of the list + */ +static inline int16_t mavlink_msg_mission_request_partial_list_get_end_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 2); +} + +/** + * @brief Decode a mission_request_partial_list message into a struct + * + * @param msg The message to decode + * @param mission_request_partial_list C-struct to decode the message contents into + */ +static inline void mavlink_msg_mission_request_partial_list_decode(const mavlink_message_t* msg, mavlink_mission_request_partial_list_t* mission_request_partial_list) +{ +#if MAVLINK_NEED_BYTE_SWAP + mission_request_partial_list->start_index = mavlink_msg_mission_request_partial_list_get_start_index(msg); + mission_request_partial_list->end_index = mavlink_msg_mission_request_partial_list_get_end_index(msg); + mission_request_partial_list->target_system = mavlink_msg_mission_request_partial_list_get_target_system(msg); + mission_request_partial_list->target_component = mavlink_msg_mission_request_partial_list_get_target_component(msg); +#else + memcpy(mission_request_partial_list, _MAV_PAYLOAD(msg), 6); +#endif +} diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_write_partial_list.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_write_partial_list.h new file mode 100644 index 0000000000..59f438de23 --- /dev/null +++ b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_write_partial_list.h @@ -0,0 +1,210 @@ +// MESSAGE MISSION_WRITE_PARTIAL_LIST PACKING + +#define MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST 38 + +typedef struct __mavlink_mission_write_partial_list_t +{ + int16_t start_index; ///< Start index, 0 by default and smaller / equal to the largest index of the current onboard list. + int16_t end_index; ///< End index, equal or greater than start index. + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_mission_write_partial_list_t; + +#define MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN 6 +#define MAVLINK_MSG_ID_38_LEN 6 + + + +#define MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST { \ + "MISSION_WRITE_PARTIAL_LIST", \ + 4, \ + { { "start_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_mission_write_partial_list_t, start_index) }, \ + { "end_index", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_mission_write_partial_list_t, end_index) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_write_partial_list_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mission_write_partial_list_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a mission_write_partial_list message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param start_index Start index, 0 by default and smaller / equal to the largest index of the current onboard list. + * @param end_index End index, equal or greater than start index. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_write_partial_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[6]; + _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); + + memcpy(_MAV_PAYLOAD(msg), buf, 6); +#else + mavlink_mission_write_partial_list_t packet; + packet.start_index = start_index; + packet.end_index = end_index; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 6); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST; + return mavlink_finalize_message(msg, system_id, component_id, 6, 9); +} + +/** + * @brief Pack a mission_write_partial_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 was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param start_index Start index, 0 by default and smaller / equal to the largest index of the current onboard list. + * @param end_index End index, equal or greater than start index. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_write_partial_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,int16_t start_index,int16_t end_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[6]; + _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); + + memcpy(_MAV_PAYLOAD(msg), buf, 6); +#else + mavlink_mission_write_partial_list_t packet; + packet.start_index = start_index; + packet.end_index = end_index; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 6); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 6, 9); +} + +/** + * @brief Encode a mission_write_partial_list struct into 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 mission_write_partial_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_write_partial_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_write_partial_list_t* mission_write_partial_list) +{ + return mavlink_msg_mission_write_partial_list_pack(system_id, component_id, msg, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index); +} + +/** + * @brief Send a mission_write_partial_list message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param start_index Start index, 0 by default and smaller / equal to the largest index of the current onboard list. + * @param end_index End index, equal or greater than start index. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mission_write_partial_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[6]; + _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); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, 6, 9); +#else + mavlink_mission_write_partial_list_t packet; + packet.start_index = start_index; + packet.end_index = end_index; + packet.target_system = target_system; + packet.target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)&packet, 6, 9); +#endif +} + +#endif + +// MESSAGE MISSION_WRITE_PARTIAL_LIST UNPACKING + + +/** + * @brief Get field target_system from mission_write_partial_list message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_mission_write_partial_list_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field target_component from mission_write_partial_list message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_mission_write_partial_list_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field start_index from mission_write_partial_list message + * + * @return Start index, 0 by default and smaller / equal to the largest index of the current onboard list. + */ +static inline int16_t mavlink_msg_mission_write_partial_list_get_start_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 0); +} + +/** + * @brief Get field end_index from mission_write_partial_list message + * + * @return End index, equal or greater than start index. + */ +static inline int16_t mavlink_msg_mission_write_partial_list_get_end_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 2); +} + +/** + * @brief Decode a mission_write_partial_list message into a struct + * + * @param msg The message to decode + * @param mission_write_partial_list C-struct to decode the message contents into + */ +static inline void mavlink_msg_mission_write_partial_list_decode(const mavlink_message_t* msg, mavlink_mission_write_partial_list_t* mission_write_partial_list) +{ +#if MAVLINK_NEED_BYTE_SWAP + mission_write_partial_list->start_index = mavlink_msg_mission_write_partial_list_get_start_index(msg); + mission_write_partial_list->end_index = mavlink_msg_mission_write_partial_list_get_end_index(msg); + mission_write_partial_list->target_system = mavlink_msg_mission_write_partial_list_get_target_system(msg); + mission_write_partial_list->target_component = mavlink_msg_mission_write_partial_list_get_target_component(msg); +#else + memcpy(mission_write_partial_list, _MAV_PAYLOAD(msg), 6); +#endif +} diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_position_target.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_position_target.h deleted file mode 100644 index 44c0e6931b..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_position_target.h +++ /dev/null @@ -1,210 +0,0 @@ -// MESSAGE POSITION_TARGET PACKING - -#define MAVLINK_MSG_ID_POSITION_TARGET 63 - -typedef struct __mavlink_position_target_t -{ - float x; ///< x position - float y; ///< y position - float z; ///< z position - float yaw; ///< yaw orientation in radians, 0 = NORTH -} mavlink_position_target_t; - -#define MAVLINK_MSG_ID_POSITION_TARGET_LEN 16 -#define MAVLINK_MSG_ID_63_LEN 16 - - - -#define MAVLINK_MESSAGE_INFO_POSITION_TARGET { \ - "POSITION_TARGET", \ - 4, \ - { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_position_target_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_position_target_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_position_target_t, z) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_position_target_t, yaw) }, \ - } \ -} - - -/** - * @brief Pack a position_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 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_target_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[16]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - - memcpy(_MAV_PAYLOAD(msg), buf, 16); -#else - mavlink_position_target_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - - memcpy(_MAV_PAYLOAD(msg), &packet, 16); -#endif - - msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET; - return mavlink_finalize_message(msg, system_id, component_id, 16, 126); -} - -/** - * @brief Pack a position_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 was sent over - * @param msg The MAVLink message to compress the data into - * @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_target_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float x,float y,float z,float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[16]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - - memcpy(_MAV_PAYLOAD(msg), buf, 16); -#else - mavlink_position_target_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - - memcpy(_MAV_PAYLOAD(msg), &packet, 16); -#endif - - msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 126); -} - -/** - * @brief Encode a position_target struct into 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 position_target C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_position_target_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_position_target_t* position_target) -{ - return mavlink_msg_position_target_pack(system_id, component_id, msg, position_target->x, position_target->y, position_target->z, position_target->yaw); -} - -/** - * @brief Send a position_target message - * @param chan MAVLink channel to send the message - * - * @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_target_send(mavlink_channel_t chan, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[16]; - _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_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET, buf, 16, 126); -#else - mavlink_position_target_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET, (const char *)&packet, 16, 126); -#endif -} - -#endif - -// MESSAGE POSITION_TARGET UNPACKING - - -/** - * @brief Get field x from position_target message - * - * @return x position - */ -static inline float mavlink_msg_position_target_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field y from position_target message - * - * @return y position - */ -static inline float mavlink_msg_position_target_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field z from position_target message - * - * @return z position - */ -static inline float mavlink_msg_position_target_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yaw from position_target message - * - * @return yaw orientation in radians, 0 = NORTH - */ -static inline float mavlink_msg_position_target_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a position_target message into a struct - * - * @param msg The message to decode - * @param position_target C-struct to decode the message contents into - */ -static inline void mavlink_msg_position_target_decode(const mavlink_message_t* msg, mavlink_position_target_t* position_target) -{ -#if MAVLINK_NEED_BYTE_SWAP - position_target->x = mavlink_msg_position_target_get_x(msg); - position_target->y = mavlink_msg_position_target_get_y(msg); - position_target->z = mavlink_msg_position_target_get_z(msg); - position_target->yaw = mavlink_msg_position_target_get_yaw(msg); -#else - memcpy(position_target, _MAV_PAYLOAD(msg), 16); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_rc_channels_scaled.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_rc_channels_scaled.h index 9a4a7912e2..a7a4e5b97f 100644 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_rc_channels_scaled.h +++ b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_rc_channels_scaled.h @@ -1,6 +1,6 @@ // MESSAGE RC_CHANNELS_SCALED PACKING -#define MAVLINK_MSG_ID_RC_CHANNELS_SCALED 36 +#define MAVLINK_MSG_ID_RC_CHANNELS_SCALED 34 typedef struct __mavlink_rc_channels_scaled_t { @@ -18,7 +18,7 @@ typedef struct __mavlink_rc_channels_scaled_t } mavlink_rc_channels_scaled_t; #define MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN 22 -#define MAVLINK_MSG_ID_36_LEN 22 +#define MAVLINK_MSG_ID_34_LEN 22 diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_servo_output_raw.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_servo_output_raw.h index 2608edfa56..dce71fd6e8 100644 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_servo_output_raw.h +++ b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_servo_output_raw.h @@ -1,6 +1,6 @@ // MESSAGE SERVO_OUTPUT_RAW PACKING -#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW 37 +#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW 36 typedef struct __mavlink_servo_output_raw_t { @@ -17,7 +17,7 @@ typedef struct __mavlink_servo_output_raw_t } mavlink_servo_output_raw_t; #define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN 21 -#define MAVLINK_MSG_ID_37_LEN 21 +#define MAVLINK_MSG_ID_36_LEN 21 diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_set_flight_mode.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_set_flight_mode.h deleted file mode 100644 index b04abd6e84..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_set_flight_mode.h +++ /dev/null @@ -1,166 +0,0 @@ -// MESSAGE SET_FLIGHT_MODE PACKING - -#define MAVLINK_MSG_ID_SET_FLIGHT_MODE 12 - -typedef struct __mavlink_set_flight_mode_t -{ - uint8_t target; ///< The system setting the mode - uint8_t flight_mode; ///< The new navigation mode -} mavlink_set_flight_mode_t; - -#define MAVLINK_MSG_ID_SET_FLIGHT_MODE_LEN 2 -#define MAVLINK_MSG_ID_12_LEN 2 - - - -#define MAVLINK_MESSAGE_INFO_SET_FLIGHT_MODE { \ - "SET_FLIGHT_MODE", \ - 2, \ - { { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_set_flight_mode_t, target) }, \ - { "flight_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_set_flight_mode_t, flight_mode) }, \ - } \ -} - - -/** - * @brief Pack a set_flight_mode 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 The system setting the mode - * @param flight_mode The new navigation mode - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_flight_mode_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target, uint8_t flight_mode) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, target); - _mav_put_uint8_t(buf, 1, flight_mode); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_set_flight_mode_t packet; - packet.target = target; - packet.flight_mode = flight_mode; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_FLIGHT_MODE; - return mavlink_finalize_message(msg, system_id, component_id, 2, 194); -} - -/** - * @brief Pack a set_flight_mode 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param target The system setting the mode - * @param flight_mode The new navigation mode - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_flight_mode_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target,uint8_t flight_mode) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, target); - _mav_put_uint8_t(buf, 1, flight_mode); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_set_flight_mode_t packet; - packet.target = target; - packet.flight_mode = flight_mode; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_FLIGHT_MODE; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 194); -} - -/** - * @brief Encode a set_flight_mode struct into 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 set_flight_mode C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_flight_mode_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_flight_mode_t* set_flight_mode) -{ - return mavlink_msg_set_flight_mode_pack(system_id, component_id, msg, set_flight_mode->target, set_flight_mode->flight_mode); -} - -/** - * @brief Send a set_flight_mode message - * @param chan MAVLink channel to send the message - * - * @param target The system setting the mode - * @param flight_mode The new navigation mode - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_flight_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t flight_mode) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, target); - _mav_put_uint8_t(buf, 1, flight_mode); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_FLIGHT_MODE, buf, 2, 194); -#else - mavlink_set_flight_mode_t packet; - packet.target = target; - packet.flight_mode = flight_mode; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_FLIGHT_MODE, (const char *)&packet, 2, 194); -#endif -} - -#endif - -// MESSAGE SET_FLIGHT_MODE UNPACKING - - -/** - * @brief Get field target from set_flight_mode message - * - * @return The system setting the mode - */ -static inline uint8_t mavlink_msg_set_flight_mode_get_target(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field flight_mode from set_flight_mode message - * - * @return The new navigation mode - */ -static inline uint8_t mavlink_msg_set_flight_mode_get_flight_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Decode a set_flight_mode message into a struct - * - * @param msg The message to decode - * @param set_flight_mode C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_flight_mode_decode(const mavlink_message_t* msg, mavlink_set_flight_mode_t* set_flight_mode) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_flight_mode->target = mavlink_msg_set_flight_mode_get_target(msg); - set_flight_mode->flight_mode = mavlink_msg_set_flight_mode_get_flight_mode(msg); -#else - memcpy(set_flight_mode, _MAV_PAYLOAD(msg), 2); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_set_safety_mode.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_set_safety_mode.h deleted file mode 100644 index b12bf75f6d..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_set_safety_mode.h +++ /dev/null @@ -1,166 +0,0 @@ -// MESSAGE SET_SAFETY_MODE PACKING - -#define MAVLINK_MSG_ID_SET_SAFETY_MODE 13 - -typedef struct __mavlink_set_safety_mode_t -{ - uint8_t target; ///< The system setting the mode - uint8_t safety_mode; ///< The new safety mode. The MAV will reject some mode changes during flight. -} mavlink_set_safety_mode_t; - -#define MAVLINK_MSG_ID_SET_SAFETY_MODE_LEN 2 -#define MAVLINK_MSG_ID_13_LEN 2 - - - -#define MAVLINK_MESSAGE_INFO_SET_SAFETY_MODE { \ - "SET_SAFETY_MODE", \ - 2, \ - { { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_set_safety_mode_t, target) }, \ - { "safety_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_set_safety_mode_t, safety_mode) }, \ - } \ -} - - -/** - * @brief Pack a set_safety_mode 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 The system setting the mode - * @param safety_mode The new safety mode. The MAV will reject some mode changes during flight. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_safety_mode_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target, uint8_t safety_mode) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, target); - _mav_put_uint8_t(buf, 1, safety_mode); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_set_safety_mode_t packet; - packet.target = target; - packet.safety_mode = safety_mode; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_SAFETY_MODE; - return mavlink_finalize_message(msg, system_id, component_id, 2, 8); -} - -/** - * @brief Pack a set_safety_mode 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param target The system setting the mode - * @param safety_mode The new safety mode. The MAV will reject some mode changes during flight. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_safety_mode_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target,uint8_t safety_mode) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, target); - _mav_put_uint8_t(buf, 1, safety_mode); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_set_safety_mode_t packet; - packet.target = target; - packet.safety_mode = safety_mode; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_SAFETY_MODE; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 8); -} - -/** - * @brief Encode a set_safety_mode struct into 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 set_safety_mode C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_safety_mode_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_safety_mode_t* set_safety_mode) -{ - return mavlink_msg_set_safety_mode_pack(system_id, component_id, msg, set_safety_mode->target, set_safety_mode->safety_mode); -} - -/** - * @brief Send a set_safety_mode message - * @param chan MAVLink channel to send the message - * - * @param target The system setting the mode - * @param safety_mode The new safety mode. The MAV will reject some mode changes during flight. - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_safety_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t safety_mode) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, target); - _mav_put_uint8_t(buf, 1, safety_mode); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_SAFETY_MODE, buf, 2, 8); -#else - mavlink_set_safety_mode_t packet; - packet.target = target; - packet.safety_mode = safety_mode; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_SAFETY_MODE, (const char *)&packet, 2, 8); -#endif -} - -#endif - -// MESSAGE SET_SAFETY_MODE UNPACKING - - -/** - * @brief Get field target from set_safety_mode message - * - * @return The system setting the mode - */ -static inline uint8_t mavlink_msg_set_safety_mode_get_target(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field safety_mode from set_safety_mode message - * - * @return The new safety mode. The MAV will reject some mode changes during flight. - */ -static inline uint8_t mavlink_msg_set_safety_mode_get_safety_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Decode a set_safety_mode message into a struct - * - * @param msg The message to decode - * @param set_safety_mode C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_safety_mode_decode(const mavlink_message_t* msg, mavlink_set_safety_mode_t* set_safety_mode) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_safety_mode->target = mavlink_msg_set_safety_mode_get_target(msg); - set_safety_mode->safety_mode = mavlink_msg_set_safety_mode_get_safety_mode(msg); -#else - memcpy(set_safety_mode, _MAV_PAYLOAD(msg), 2); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_system_time_utc.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_system_time_utc.h deleted file mode 100644 index fb0ed4e49c..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_system_time_utc.h +++ /dev/null @@ -1,166 +0,0 @@ -// MESSAGE SYSTEM_TIME_UTC PACKING - -#define MAVLINK_MSG_ID_SYSTEM_TIME_UTC 3 - -typedef struct __mavlink_system_time_utc_t -{ - uint32_t utc_date; ///< GPS UTC date ddmmyy - uint32_t utc_time; ///< GPS UTC time hhmmss -} mavlink_system_time_utc_t; - -#define MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN 8 -#define MAVLINK_MSG_ID_3_LEN 8 - - - -#define MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC { \ - "SYSTEM_TIME_UTC", \ - 2, \ - { { "utc_date", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_system_time_utc_t, utc_date) }, \ - { "utc_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_system_time_utc_t, utc_time) }, \ - } \ -} - - -/** - * @brief Pack a system_time_utc 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 utc_date GPS UTC date ddmmyy - * @param utc_time GPS UTC time hhmmss - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_system_time_utc_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t utc_date, uint32_t utc_time) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[8]; - _mav_put_uint32_t(buf, 0, utc_date); - _mav_put_uint32_t(buf, 4, utc_time); - - memcpy(_MAV_PAYLOAD(msg), buf, 8); -#else - mavlink_system_time_utc_t packet; - packet.utc_date = utc_date; - packet.utc_time = utc_time; - - memcpy(_MAV_PAYLOAD(msg), &packet, 8); -#endif - - msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; - return mavlink_finalize_message(msg, system_id, component_id, 8, 191); -} - -/** - * @brief Pack a system_time_utc 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param utc_date GPS UTC date ddmmyy - * @param utc_time GPS UTC time hhmmss - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_system_time_utc_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t utc_date,uint32_t utc_time) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[8]; - _mav_put_uint32_t(buf, 0, utc_date); - _mav_put_uint32_t(buf, 4, utc_time); - - memcpy(_MAV_PAYLOAD(msg), buf, 8); -#else - mavlink_system_time_utc_t packet; - packet.utc_date = utc_date; - packet.utc_time = utc_time; - - memcpy(_MAV_PAYLOAD(msg), &packet, 8); -#endif - - msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 191); -} - -/** - * @brief Encode a system_time_utc struct into 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 system_time_utc C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_system_time_utc_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_system_time_utc_t* system_time_utc) -{ - return mavlink_msg_system_time_utc_pack(system_id, component_id, msg, system_time_utc->utc_date, system_time_utc->utc_time); -} - -/** - * @brief Send a system_time_utc message - * @param chan MAVLink channel to send the message - * - * @param utc_date GPS UTC date ddmmyy - * @param utc_time GPS UTC time hhmmss - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_system_time_utc_send(mavlink_channel_t chan, uint32_t utc_date, uint32_t utc_time) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[8]; - _mav_put_uint32_t(buf, 0, utc_date); - _mav_put_uint32_t(buf, 4, utc_time); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME_UTC, buf, 8, 191); -#else - mavlink_system_time_utc_t packet; - packet.utc_date = utc_date; - packet.utc_time = utc_time; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME_UTC, (const char *)&packet, 8, 191); -#endif -} - -#endif - -// MESSAGE SYSTEM_TIME_UTC UNPACKING - - -/** - * @brief Get field utc_date from system_time_utc message - * - * @return GPS UTC date ddmmyy - */ -static inline uint32_t mavlink_msg_system_time_utc_get_utc_date(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field utc_time from system_time_utc message - * - * @return GPS UTC time hhmmss - */ -static inline uint32_t mavlink_msg_system_time_utc_get_utc_time(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 4); -} - -/** - * @brief Decode a system_time_utc message into a struct - * - * @param msg The message to decode - * @param system_time_utc C-struct to decode the message contents into - */ -static inline void mavlink_msg_system_time_utc_decode(const mavlink_message_t* msg, mavlink_system_time_utc_t* system_time_utc) -{ -#if MAVLINK_NEED_BYTE_SWAP - system_time_utc->utc_date = mavlink_msg_system_time_utc_get_utc_date(msg); - system_time_utc->utc_time = mavlink_msg_system_time_utc_get_utc_time(msg); -#else - memcpy(system_time_utc, _MAV_PAYLOAD(msg), 8); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint.h deleted file mode 100644 index 455607c44c..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint.h +++ /dev/null @@ -1,430 +0,0 @@ -// MESSAGE WAYPOINT PACKING - -#define MAVLINK_MSG_ID_WAYPOINT 39 - -typedef struct __mavlink_waypoint_t -{ - float param1; ///< PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - float param2; ///< PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - float param3; ///< PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - float param4; ///< PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - float x; ///< PARAM5 / local: x position, global: latitude - float y; ///< PARAM6 / y position: global: longitude - float z; ///< PARAM7 / z position: global: altitude - uint16_t seq; ///< Sequence - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t frame; ///< The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - uint8_t command; ///< The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - uint8_t current; ///< false:0, true:1 - uint8_t autocontinue; ///< autocontinue to next wp -} mavlink_waypoint_t; - -#define MAVLINK_MSG_ID_WAYPOINT_LEN 36 -#define MAVLINK_MSG_ID_39_LEN 36 - - - -#define MAVLINK_MESSAGE_INFO_WAYPOINT { \ - "WAYPOINT", \ - 14, \ - { { "param1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_waypoint_t, param1) }, \ - { "param2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_waypoint_t, param2) }, \ - { "param3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_waypoint_t, param3) }, \ - { "param4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_waypoint_t, param4) }, \ - { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_waypoint_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_waypoint_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_waypoint_t, z) }, \ - { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_waypoint_t, seq) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_waypoint_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_waypoint_t, target_component) }, \ - { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_waypoint_t, frame) }, \ - { "command", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_waypoint_t, command) }, \ - { "current", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_waypoint_t, current) }, \ - { "autocontinue", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_waypoint_t, autocontinue) }, \ - } \ -} - - -/** - * @brief Pack a waypoint 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 Sequence - * @param frame The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - * @param command The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - * @param current false:0, true:1 - * @param autocontinue autocontinue to next wp - * @param param1 PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - * @param param2 PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - * @param param3 PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - * @param param4 PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - * @param x PARAM5 / local: x position, global: latitude - * @param y PARAM6 / y position: global: longitude - * @param z PARAM7 / z position: global: altitude - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_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, uint8_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[36]; - _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_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, command); - _mav_put_uint8_t(buf, 34, current); - _mav_put_uint8_t(buf, 35, autocontinue); - - memcpy(_MAV_PAYLOAD(msg), buf, 36); -#else - mavlink_waypoint_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.target_system = target_system; - packet.target_component = target_component; - packet.frame = frame; - packet.command = command; - packet.current = current; - packet.autocontinue = autocontinue; - - memcpy(_MAV_PAYLOAD(msg), &packet, 36); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT; - return mavlink_finalize_message(msg, system_id, component_id, 36, 205); -} - -/** - * @brief Pack a waypoint 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param seq Sequence - * @param frame The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - * @param command The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - * @param current false:0, true:1 - * @param autocontinue autocontinue to next wp - * @param param1 PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - * @param param2 PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - * @param param3 PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - * @param param4 PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - * @param x PARAM5 / local: x position, global: latitude - * @param y PARAM6 / y position: global: longitude - * @param z PARAM7 / z position: global: altitude - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_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,uint8_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[36]; - _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_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, command); - _mav_put_uint8_t(buf, 34, current); - _mav_put_uint8_t(buf, 35, autocontinue); - - memcpy(_MAV_PAYLOAD(msg), buf, 36); -#else - mavlink_waypoint_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.target_system = target_system; - packet.target_component = target_component; - packet.frame = frame; - packet.command = command; - packet.current = current; - packet.autocontinue = autocontinue; - - memcpy(_MAV_PAYLOAD(msg), &packet, 36); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36, 205); -} - -/** - * @brief Encode a waypoint struct into 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 waypoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_waypoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_waypoint_t* waypoint) -{ - return mavlink_msg_waypoint_pack(system_id, component_id, msg, waypoint->target_system, waypoint->target_component, waypoint->seq, waypoint->frame, waypoint->command, waypoint->current, waypoint->autocontinue, waypoint->param1, waypoint->param2, waypoint->param3, waypoint->param4, waypoint->x, waypoint->y, waypoint->z); -} - -/** - * @brief Send a waypoint message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param seq Sequence - * @param frame The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - * @param command The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - * @param current false:0, true:1 - * @param autocontinue autocontinue to next wp - * @param param1 PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - * @param param2 PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - * @param param3 PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - * @param param4 PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - * @param x PARAM5 / local: x position, global: latitude - * @param y PARAM6 / y position: global: longitude - * @param z PARAM7 / z position: global: altitude - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_waypoint_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint8_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[36]; - _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_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, command); - _mav_put_uint8_t(buf, 34, current); - _mav_put_uint8_t(buf, 35, autocontinue); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT, buf, 36, 205); -#else - mavlink_waypoint_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.target_system = target_system; - packet.target_component = target_component; - packet.frame = frame; - packet.command = command; - packet.current = current; - packet.autocontinue = autocontinue; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT, (const char *)&packet, 36, 205); -#endif -} - -#endif - -// MESSAGE WAYPOINT UNPACKING - - -/** - * @brief Get field target_system from waypoint message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_waypoint_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 30); -} - -/** - * @brief Get field target_component from waypoint message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_waypoint_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 31); -} - -/** - * @brief Get field seq from waypoint message - * - * @return Sequence - */ -static inline uint16_t mavlink_msg_waypoint_get_seq(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 28); -} - -/** - * @brief Get field frame from waypoint message - * - * @return The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - */ -static inline uint8_t mavlink_msg_waypoint_get_frame(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 32); -} - -/** - * @brief Get field command from waypoint message - * - * @return The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - */ -static inline uint8_t mavlink_msg_waypoint_get_command(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 33); -} - -/** - * @brief Get field current from waypoint message - * - * @return false:0, true:1 - */ -static inline uint8_t mavlink_msg_waypoint_get_current(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 34); -} - -/** - * @brief Get field autocontinue from waypoint message - * - * @return autocontinue to next wp - */ -static inline uint8_t mavlink_msg_waypoint_get_autocontinue(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 35); -} - -/** - * @brief Get field param1 from waypoint message - * - * @return PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - */ -static inline float mavlink_msg_waypoint_get_param1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field param2 from waypoint message - * - * @return PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - */ -static inline float mavlink_msg_waypoint_get_param2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field param3 from waypoint message - * - * @return PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - */ -static inline float mavlink_msg_waypoint_get_param3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field param4 from waypoint message - * - * @return PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - */ -static inline float mavlink_msg_waypoint_get_param4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field x from waypoint message - * - * @return PARAM5 / local: x position, global: latitude - */ -static inline float mavlink_msg_waypoint_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field y from waypoint message - * - * @return PARAM6 / y position: global: longitude - */ -static inline float mavlink_msg_waypoint_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field z from waypoint message - * - * @return PARAM7 / z position: global: altitude - */ -static inline float mavlink_msg_waypoint_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Decode a waypoint message into a struct - * - * @param msg The message to decode - * @param waypoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_waypoint_decode(const mavlink_message_t* msg, mavlink_waypoint_t* waypoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - waypoint->param1 = mavlink_msg_waypoint_get_param1(msg); - waypoint->param2 = mavlink_msg_waypoint_get_param2(msg); - waypoint->param3 = mavlink_msg_waypoint_get_param3(msg); - waypoint->param4 = mavlink_msg_waypoint_get_param4(msg); - waypoint->x = mavlink_msg_waypoint_get_x(msg); - waypoint->y = mavlink_msg_waypoint_get_y(msg); - waypoint->z = mavlink_msg_waypoint_get_z(msg); - waypoint->seq = mavlink_msg_waypoint_get_seq(msg); - waypoint->target_system = mavlink_msg_waypoint_get_target_system(msg); - waypoint->target_component = mavlink_msg_waypoint_get_target_component(msg); - waypoint->frame = mavlink_msg_waypoint_get_frame(msg); - waypoint->command = mavlink_msg_waypoint_get_command(msg); - waypoint->current = mavlink_msg_waypoint_get_current(msg); - waypoint->autocontinue = mavlink_msg_waypoint_get_autocontinue(msg); -#else - memcpy(waypoint, _MAV_PAYLOAD(msg), 36); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_ack.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_ack.h deleted file mode 100644 index 846ae88393..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_ack.h +++ /dev/null @@ -1,188 +0,0 @@ -// MESSAGE WAYPOINT_ACK PACKING - -#define MAVLINK_MSG_ID_WAYPOINT_ACK 47 - -typedef struct __mavlink_waypoint_ack_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t type; ///< 0: OK, 1: Error -} mavlink_waypoint_ack_t; - -#define MAVLINK_MSG_ID_WAYPOINT_ACK_LEN 3 -#define MAVLINK_MSG_ID_47_LEN 3 - - - -#define MAVLINK_MESSAGE_INFO_WAYPOINT_ACK { \ - "WAYPOINT_ACK", \ - 3, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_waypoint_ack_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_waypoint_ack_t, target_component) }, \ - { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_waypoint_ack_t, type) }, \ - } \ -} - - -/** - * @brief Pack a waypoint_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 type 0: OK, 1: Error - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[3]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, type); - - memcpy(_MAV_PAYLOAD(msg), buf, 3); -#else - mavlink_waypoint_ack_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.type = type; - - memcpy(_MAV_PAYLOAD(msg), &packet, 3); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; - return mavlink_finalize_message(msg, system_id, component_id, 3, 214); -} - -/** - * @brief Pack a waypoint_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 was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param type 0: OK, 1: Error - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[3]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, type); - - memcpy(_MAV_PAYLOAD(msg), buf, 3); -#else - mavlink_waypoint_ack_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.type = type; - - memcpy(_MAV_PAYLOAD(msg), &packet, 3); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 214); -} - -/** - * @brief Encode a waypoint_ack struct into 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 waypoint_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_waypoint_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_waypoint_ack_t* waypoint_ack) -{ - return mavlink_msg_waypoint_ack_pack(system_id, component_id, msg, waypoint_ack->target_system, waypoint_ack->target_component, waypoint_ack->type); -} - -/** - * @brief Send a waypoint_ack message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param type 0: OK, 1: Error - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_waypoint_ack_send(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[3]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, type); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_ACK, buf, 3, 214); -#else - mavlink_waypoint_ack_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.type = type; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_ACK, (const char *)&packet, 3, 214); -#endif -} - -#endif - -// MESSAGE WAYPOINT_ACK UNPACKING - - -/** - * @brief Get field target_system from waypoint_ack message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_waypoint_ack_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from waypoint_ack message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_waypoint_ack_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field type from waypoint_ack message - * - * @return 0: OK, 1: Error - */ -static inline uint8_t mavlink_msg_waypoint_ack_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Decode a waypoint_ack message into a struct - * - * @param msg The message to decode - * @param waypoint_ack C-struct to decode the message contents into - */ -static inline void mavlink_msg_waypoint_ack_decode(const mavlink_message_t* msg, mavlink_waypoint_ack_t* waypoint_ack) -{ -#if MAVLINK_NEED_BYTE_SWAP - waypoint_ack->target_system = mavlink_msg_waypoint_ack_get_target_system(msg); - waypoint_ack->target_component = mavlink_msg_waypoint_ack_get_target_component(msg); - waypoint_ack->type = mavlink_msg_waypoint_ack_get_type(msg); -#else - memcpy(waypoint_ack, _MAV_PAYLOAD(msg), 3); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_clear_all.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_clear_all.h deleted file mode 100644 index 4133b58d06..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_clear_all.h +++ /dev/null @@ -1,166 +0,0 @@ -// MESSAGE WAYPOINT_CLEAR_ALL PACKING - -#define MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL 45 - -typedef struct __mavlink_waypoint_clear_all_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_waypoint_clear_all_t; - -#define MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN 2 -#define MAVLINK_MSG_ID_45_LEN 2 - - - -#define MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL { \ - "WAYPOINT_CLEAR_ALL", \ - 2, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_waypoint_clear_all_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_waypoint_clear_all_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a waypoint_clear_all 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_waypoint_clear_all_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[2]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_waypoint_clear_all_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; - return mavlink_finalize_message(msg, system_id, component_id, 2, 229); -} - -/** - * @brief Pack a waypoint_clear_all 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 was 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_waypoint_clear_all_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_waypoint_clear_all_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 229); -} - -/** - * @brief Encode a waypoint_clear_all struct into 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 waypoint_clear_all C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_waypoint_clear_all_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_waypoint_clear_all_t* waypoint_clear_all) -{ - return mavlink_msg_waypoint_clear_all_pack(system_id, component_id, msg, waypoint_clear_all->target_system, waypoint_clear_all->target_component); -} - -/** - * @brief Send a waypoint_clear_all 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_waypoint_clear_all_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL, buf, 2, 229); -#else - mavlink_waypoint_clear_all_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL, (const char *)&packet, 2, 229); -#endif -} - -#endif - -// MESSAGE WAYPOINT_CLEAR_ALL UNPACKING - - -/** - * @brief Get field target_system from waypoint_clear_all message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_waypoint_clear_all_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from waypoint_clear_all message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_waypoint_clear_all_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Decode a waypoint_clear_all message into a struct - * - * @param msg The message to decode - * @param waypoint_clear_all C-struct to decode the message contents into - */ -static inline void mavlink_msg_waypoint_clear_all_decode(const mavlink_message_t* msg, mavlink_waypoint_clear_all_t* waypoint_clear_all) -{ -#if MAVLINK_NEED_BYTE_SWAP - waypoint_clear_all->target_system = mavlink_msg_waypoint_clear_all_get_target_system(msg); - waypoint_clear_all->target_component = mavlink_msg_waypoint_clear_all_get_target_component(msg); -#else - memcpy(waypoint_clear_all, _MAV_PAYLOAD(msg), 2); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_count.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_count.h deleted file mode 100644 index cfa433338b..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_count.h +++ /dev/null @@ -1,188 +0,0 @@ -// MESSAGE WAYPOINT_COUNT PACKING - -#define MAVLINK_MSG_ID_WAYPOINT_COUNT 44 - -typedef struct __mavlink_waypoint_count_t -{ - uint16_t count; ///< Number of Waypoints in the Sequence - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_waypoint_count_t; - -#define MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN 4 -#define MAVLINK_MSG_ID_44_LEN 4 - - - -#define MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT { \ - "WAYPOINT_COUNT", \ - 3, \ - { { "count", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_waypoint_count_t, count) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_waypoint_count_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_waypoint_count_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a waypoint_count 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 count Number of Waypoints in the Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_count_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t count) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint16_t(buf, 0, count); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 4); -#else - mavlink_waypoint_count_t packet; - packet.count = count; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; - return mavlink_finalize_message(msg, system_id, component_id, 4, 8); -} - -/** - * @brief Pack a waypoint_count 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param count Number of Waypoints in the Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_count_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t count) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint16_t(buf, 0, count); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 4); -#else - mavlink_waypoint_count_t packet; - packet.count = count; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 8); -} - -/** - * @brief Encode a waypoint_count struct into 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 waypoint_count C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_waypoint_count_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_waypoint_count_t* waypoint_count) -{ - return mavlink_msg_waypoint_count_pack(system_id, component_id, msg, waypoint_count->target_system, waypoint_count->target_component, waypoint_count->count); -} - -/** - * @brief Send a waypoint_count message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param count Number of Waypoints in the Sequence - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_waypoint_count_send(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[4]; - _mav_put_uint16_t(buf, 0, count); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_COUNT, buf, 4, 8); -#else - mavlink_waypoint_count_t packet; - packet.count = count; - packet.target_system = target_system; - packet.target_component = target_component; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_COUNT, (const char *)&packet, 4, 8); -#endif -} - -#endif - -// MESSAGE WAYPOINT_COUNT UNPACKING - - -/** - * @brief Get field target_system from waypoint_count message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_waypoint_count_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field target_component from waypoint_count message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_waypoint_count_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field count from waypoint_count message - * - * @return Number of Waypoints in the Sequence - */ -static inline uint16_t mavlink_msg_waypoint_count_get_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Decode a waypoint_count message into a struct - * - * @param msg The message to decode - * @param waypoint_count C-struct to decode the message contents into - */ -static inline void mavlink_msg_waypoint_count_decode(const mavlink_message_t* msg, mavlink_waypoint_count_t* waypoint_count) -{ -#if MAVLINK_NEED_BYTE_SWAP - waypoint_count->count = mavlink_msg_waypoint_count_get_count(msg); - waypoint_count->target_system = mavlink_msg_waypoint_count_get_target_system(msg); - waypoint_count->target_component = mavlink_msg_waypoint_count_get_target_component(msg); -#else - memcpy(waypoint_count, _MAV_PAYLOAD(msg), 4); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_current.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_current.h deleted file mode 100644 index a2438a4568..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_current.h +++ /dev/null @@ -1,144 +0,0 @@ -// MESSAGE WAYPOINT_CURRENT PACKING - -#define MAVLINK_MSG_ID_WAYPOINT_CURRENT 42 - -typedef struct __mavlink_waypoint_current_t -{ - uint16_t seq; ///< Sequence -} mavlink_waypoint_current_t; - -#define MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN 2 -#define MAVLINK_MSG_ID_42_LEN 2 - - - -#define MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT { \ - "WAYPOINT_CURRENT", \ - 1, \ - { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_waypoint_current_t, seq) }, \ - } \ -} - - -/** - * @brief Pack a waypoint_current 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 seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_current_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint16_t(buf, 0, seq); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_waypoint_current_t packet; - packet.seq = seq; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; - return mavlink_finalize_message(msg, system_id, component_id, 2, 101); -} - -/** - * @brief Pack a waypoint_current 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_current_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint16_t(buf, 0, seq); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_waypoint_current_t packet; - packet.seq = seq; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 101); -} - -/** - * @brief Encode a waypoint_current struct into 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 waypoint_current C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_waypoint_current_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_waypoint_current_t* waypoint_current) -{ - return mavlink_msg_waypoint_current_pack(system_id, component_id, msg, waypoint_current->seq); -} - -/** - * @brief Send a waypoint_current message - * @param chan MAVLink channel to send the message - * - * @param seq Sequence - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint16_t(buf, 0, seq); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_CURRENT, buf, 2, 101); -#else - mavlink_waypoint_current_t packet; - packet.seq = seq; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_CURRENT, (const char *)&packet, 2, 101); -#endif -} - -#endif - -// MESSAGE WAYPOINT_CURRENT UNPACKING - - -/** - * @brief Get field seq from waypoint_current message - * - * @return Sequence - */ -static inline uint16_t mavlink_msg_waypoint_current_get_seq(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Decode a waypoint_current message into a struct - * - * @param msg The message to decode - * @param waypoint_current C-struct to decode the message contents into - */ -static inline void mavlink_msg_waypoint_current_decode(const mavlink_message_t* msg, mavlink_waypoint_current_t* waypoint_current) -{ -#if MAVLINK_NEED_BYTE_SWAP - waypoint_current->seq = mavlink_msg_waypoint_current_get_seq(msg); -#else - memcpy(waypoint_current, _MAV_PAYLOAD(msg), 2); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_reached.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_reached.h deleted file mode 100644 index e1d64b4b66..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_reached.h +++ /dev/null @@ -1,144 +0,0 @@ -// MESSAGE WAYPOINT_REACHED PACKING - -#define MAVLINK_MSG_ID_WAYPOINT_REACHED 46 - -typedef struct __mavlink_waypoint_reached_t -{ - uint16_t seq; ///< Sequence -} mavlink_waypoint_reached_t; - -#define MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN 2 -#define MAVLINK_MSG_ID_46_LEN 2 - - - -#define MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED { \ - "WAYPOINT_REACHED", \ - 1, \ - { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_waypoint_reached_t, seq) }, \ - } \ -} - - -/** - * @brief Pack a waypoint_reached 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 seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_reached_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint16_t(buf, 0, seq); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_waypoint_reached_t packet; - packet.seq = seq; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; - return mavlink_finalize_message(msg, system_id, component_id, 2, 21); -} - -/** - * @brief Pack a waypoint_reached 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_reached_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint16_t(buf, 0, seq); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_waypoint_reached_t packet; - packet.seq = seq; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 21); -} - -/** - * @brief Encode a waypoint_reached struct into 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 waypoint_reached C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_waypoint_reached_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_waypoint_reached_t* waypoint_reached) -{ - return mavlink_msg_waypoint_reached_pack(system_id, component_id, msg, waypoint_reached->seq); -} - -/** - * @brief Send a waypoint_reached message - * @param chan MAVLink channel to send the message - * - * @param seq Sequence - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint16_t(buf, 0, seq); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_REACHED, buf, 2, 21); -#else - mavlink_waypoint_reached_t packet; - packet.seq = seq; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_REACHED, (const char *)&packet, 2, 21); -#endif -} - -#endif - -// MESSAGE WAYPOINT_REACHED UNPACKING - - -/** - * @brief Get field seq from waypoint_reached message - * - * @return Sequence - */ -static inline uint16_t mavlink_msg_waypoint_reached_get_seq(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Decode a waypoint_reached message into a struct - * - * @param msg The message to decode - * @param waypoint_reached C-struct to decode the message contents into - */ -static inline void mavlink_msg_waypoint_reached_decode(const mavlink_message_t* msg, mavlink_waypoint_reached_t* waypoint_reached) -{ -#if MAVLINK_NEED_BYTE_SWAP - waypoint_reached->seq = mavlink_msg_waypoint_reached_get_seq(msg); -#else - memcpy(waypoint_reached, _MAV_PAYLOAD(msg), 2); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_request.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_request.h deleted file mode 100644 index b88f4dbfbe..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_request.h +++ /dev/null @@ -1,188 +0,0 @@ -// MESSAGE WAYPOINT_REQUEST PACKING - -#define MAVLINK_MSG_ID_WAYPOINT_REQUEST 40 - -typedef struct __mavlink_waypoint_request_t -{ - uint16_t seq; ///< Sequence - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_waypoint_request_t; - -#define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN 4 -#define MAVLINK_MSG_ID_40_LEN 4 - - - -#define MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST { \ - "WAYPOINT_REQUEST", \ - 3, \ - { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_waypoint_request_t, seq) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_waypoint_request_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_waypoint_request_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a waypoint_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 target_system System ID - * @param target_component Component ID - * @param seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_request_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint16_t(buf, 0, seq); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 4); -#else - mavlink_waypoint_request_t packet; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; - return mavlink_finalize_message(msg, system_id, component_id, 4, 51); -} - -/** - * @brief Pack a waypoint_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 was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_request_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint16_t(buf, 0, seq); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 4); -#else - mavlink_waypoint_request_t packet; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 51); -} - -/** - * @brief Encode a waypoint_request struct into 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 waypoint_request C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_waypoint_request_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_waypoint_request_t* waypoint_request) -{ - return mavlink_msg_waypoint_request_pack(system_id, component_id, msg, waypoint_request->target_system, waypoint_request->target_component, waypoint_request->seq); -} - -/** - * @brief Send a waypoint_request message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param seq Sequence - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_waypoint_request_send(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[4]; - _mav_put_uint16_t(buf, 0, seq); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST, buf, 4, 51); -#else - mavlink_waypoint_request_t packet; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST, (const char *)&packet, 4, 51); -#endif -} - -#endif - -// MESSAGE WAYPOINT_REQUEST UNPACKING - - -/** - * @brief Get field target_system from waypoint_request message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_waypoint_request_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field target_component from waypoint_request message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_waypoint_request_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field seq from waypoint_request message - * - * @return Sequence - */ -static inline uint16_t mavlink_msg_waypoint_request_get_seq(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Decode a waypoint_request message into a struct - * - * @param msg The message to decode - * @param waypoint_request C-struct to decode the message contents into - */ -static inline void mavlink_msg_waypoint_request_decode(const mavlink_message_t* msg, mavlink_waypoint_request_t* waypoint_request) -{ -#if MAVLINK_NEED_BYTE_SWAP - waypoint_request->seq = mavlink_msg_waypoint_request_get_seq(msg); - waypoint_request->target_system = mavlink_msg_waypoint_request_get_target_system(msg); - waypoint_request->target_component = mavlink_msg_waypoint_request_get_target_component(msg); -#else - memcpy(waypoint_request, _MAV_PAYLOAD(msg), 4); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_request_list.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_request_list.h deleted file mode 100644 index 61e8d834bd..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_request_list.h +++ /dev/null @@ -1,166 +0,0 @@ -// MESSAGE WAYPOINT_REQUEST_LIST PACKING - -#define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST 43 - -typedef struct __mavlink_waypoint_request_list_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_waypoint_request_list_t; - -#define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN 2 -#define MAVLINK_MSG_ID_43_LEN 2 - - - -#define MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST { \ - "WAYPOINT_REQUEST_LIST", \ - 2, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_waypoint_request_list_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_waypoint_request_list_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a waypoint_request_list message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_waypoint_request_list_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; - return mavlink_finalize_message(msg, system_id, component_id, 2, 213); -} - -/** - * @brief Pack a waypoint_request_list message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was 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_waypoint_request_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_waypoint_request_list_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 213); -} - -/** - * @brief Encode a waypoint_request_list struct into 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 waypoint_request_list C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_waypoint_request_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_waypoint_request_list_t* waypoint_request_list) -{ - return mavlink_msg_waypoint_request_list_pack(system_id, component_id, msg, waypoint_request_list->target_system, waypoint_request_list->target_component); -} - -/** - * @brief Send a waypoint_request_list message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_waypoint_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST, buf, 2, 213); -#else - mavlink_waypoint_request_list_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST, (const char *)&packet, 2, 213); -#endif -} - -#endif - -// MESSAGE WAYPOINT_REQUEST_LIST UNPACKING - - -/** - * @brief Get field target_system from waypoint_request_list message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_waypoint_request_list_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from waypoint_request_list message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_waypoint_request_list_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Decode a waypoint_request_list message into a struct - * - * @param msg The message to decode - * @param waypoint_request_list C-struct to decode the message contents into - */ -static inline void mavlink_msg_waypoint_request_list_decode(const mavlink_message_t* msg, mavlink_waypoint_request_list_t* waypoint_request_list) -{ -#if MAVLINK_NEED_BYTE_SWAP - waypoint_request_list->target_system = mavlink_msg_waypoint_request_list_get_target_system(msg); - waypoint_request_list->target_component = mavlink_msg_waypoint_request_list_get_target_component(msg); -#else - memcpy(waypoint_request_list, _MAV_PAYLOAD(msg), 2); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_set_current.h b/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_set_current.h deleted file mode 100644 index e2c683ee3c..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_set_current.h +++ /dev/null @@ -1,188 +0,0 @@ -// MESSAGE WAYPOINT_SET_CURRENT PACKING - -#define MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT 41 - -typedef struct __mavlink_waypoint_set_current_t -{ - uint16_t seq; ///< Sequence - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_waypoint_set_current_t; - -#define MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN 4 -#define MAVLINK_MSG_ID_41_LEN 4 - - - -#define MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT { \ - "WAYPOINT_SET_CURRENT", \ - 3, \ - { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_waypoint_set_current_t, seq) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_waypoint_set_current_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_waypoint_set_current_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a waypoint_set_current 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 Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_set_current_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint16_t(buf, 0, seq); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 4); -#else - mavlink_waypoint_set_current_t packet; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; - return mavlink_finalize_message(msg, system_id, component_id, 4, 80); -} - -/** - * @brief Pack a waypoint_set_current 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_set_current_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) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint16_t(buf, 0, seq); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 4); -#else - mavlink_waypoint_set_current_t packet; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 80); -} - -/** - * @brief Encode a waypoint_set_current struct into 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 waypoint_set_current C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_waypoint_set_current_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_waypoint_set_current_t* waypoint_set_current) -{ - return mavlink_msg_waypoint_set_current_pack(system_id, component_id, msg, waypoint_set_current->target_system, waypoint_set_current->target_component, waypoint_set_current->seq); -} - -/** - * @brief Send a waypoint_set_current message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param seq Sequence - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_waypoint_set_current_send(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[4]; - _mav_put_uint16_t(buf, 0, seq); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, buf, 4, 80); -#else - mavlink_waypoint_set_current_t packet; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, (const char *)&packet, 4, 80); -#endif -} - -#endif - -// MESSAGE WAYPOINT_SET_CURRENT UNPACKING - - -/** - * @brief Get field target_system from waypoint_set_current message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_waypoint_set_current_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field target_component from waypoint_set_current message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_waypoint_set_current_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field seq from waypoint_set_current message - * - * @return Sequence - */ -static inline uint16_t mavlink_msg_waypoint_set_current_get_seq(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Decode a waypoint_set_current message into a struct - * - * @param msg The message to decode - * @param waypoint_set_current C-struct to decode the message contents into - */ -static inline void mavlink_msg_waypoint_set_current_decode(const mavlink_message_t* msg, mavlink_waypoint_set_current_t* waypoint_set_current) -{ -#if MAVLINK_NEED_BYTE_SWAP - waypoint_set_current->seq = mavlink_msg_waypoint_set_current_get_seq(msg); - waypoint_set_current->target_system = mavlink_msg_waypoint_set_current_get_target_system(msg); - waypoint_set_current->target_component = mavlink_msg_waypoint_set_current_get_target_component(msg); -#else - memcpy(waypoint_set_current, _MAV_PAYLOAD(msg), 4); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/common/testsuite.h b/libraries/GCS_MAVLink/include_v1.0/common/testsuite.h index 3c5c9f14ac..5d4681435c 100644 --- a/libraries/GCS_MAVLink/include_v1.0/common/testsuite.h +++ b/libraries/GCS_MAVLink/include_v1.0/common/testsuite.h @@ -1182,69 +1182,6 @@ static void mavlink_test_global_position_int(uint8_t system_id, uint8_t componen MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } -static void mavlink_test_rc_channels_raw(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_rc_channels_raw_t packet_in = { - 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; - packet1.chan1_raw = packet_in.chan1_raw; - packet1.chan2_raw = packet_in.chan2_raw; - packet1.chan3_raw = packet_in.chan3_raw; - packet1.chan4_raw = packet_in.chan4_raw; - packet1.chan5_raw = packet_in.chan5_raw; - packet1.chan6_raw = packet_in.chan6_raw; - packet1.chan7_raw = packet_in.chan7_raw; - packet1.chan8_raw = packet_in.chan8_raw; - packet1.port = packet_in.port; - packet1.rssi = packet_in.rssi; - - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rc_channels_raw_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_rc_channels_raw_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rc_channels_raw_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.port , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw , packet1.rssi ); - mavlink_msg_rc_channels_raw_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rc_channels_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.port , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw , packet1.rssi ); - mavlink_msg_rc_channels_raw_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_to_send_buffer(buffer, &msg); - for (i=0; imsgid = MAVLINK_MSG_ID_HEARTBEAT; - return mavlink_finalize_message(msg, system_id, component_id, 9, 50); -} - -/** - * @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 was 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_CLASS ENUM - * @param base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h - * @param custom_mode Navigation mode bitfield, see MAV_AUTOPILOT_CUSTOM_MODE ENUM for some examples. This field is autopilot-specific. - * @param system_status System status flag, see MAV_STATUS 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[9]; - _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(msg), buf, 9); -#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(msg), &packet, 9); -#endif - - msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 9, 50); -} - -/** - * @brief Encode a heartbeat struct into 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 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 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_CLASS ENUM - * @param base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h - * @param custom_mode Navigation mode bitfield, see MAV_AUTOPILOT_CUSTOM_MODE ENUM for some examples. This field is autopilot-specific. - * @param system_status System status flag, see MAV_STATUS 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[9]; - _mav_put_uint32_t(buf, 0, custom_mode); - _mav_put_uint8_t(buf, 4, type); - _mav_put_uint8_t(buf, 5, autopilot); - _mav_put_uint8_t(buf, 6, base_mode); - _mav_put_uint8_t(buf, 7, system_status); - _mav_put_uint8_t(buf, 8, 2); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, 9, 50); -#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; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, 9, 50); -#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_CLASS 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 Navigation mode bitfield, see MAV_AUTOPILOT_CUSTOM_MODE ENUM for some examples. This field is autopilot-specific. - */ -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_STATUS 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), 9); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/minimal/minimal.h b/libraries/GCS_MAVLink/include_v1.0/minimal/minimal.h deleted file mode 100644 index 895c79d0a4..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/minimal/minimal.h +++ /dev/null @@ -1,53 +0,0 @@ -/** @file - * @brief MAVLink comm protocol generated from minimal.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef MINIMAL_H -#define MINIMAL_H - -#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, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}} -#endif - -#include "../protocol.h" - -#define MAVLINK_ENABLED_MINIMAL - - - -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -// ENUM DEFINITIONS - - - -// MESSAGE DEFINITIONS -#include "./mavlink_msg_heartbeat.h" - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // MINIMAL_H diff --git a/libraries/GCS_MAVLink/include_v1.0/minimal/testsuite.h b/libraries/GCS_MAVLink/include_v1.0/minimal/testsuite.h deleted file mode 100644 index 1758c912cf..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/minimal/testsuite.h +++ /dev/null @@ -1,88 +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; imsgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; - return mavlink_finalize_message(msg, system_id, component_id, 21, 254); -} - -/** - * @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 was 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[21]; - _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(msg), buf, 21); -#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(msg), &packet, 21); -#endif - - msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 21, 254); -} - -/** - * @brief Encode a attitude_control struct into 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 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 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[21]; - _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); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, buf, 21, 254); -#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; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, (const char *)&packet, 21, 254); -#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), 21); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_brief_feature.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_brief_feature.h deleted file mode 100644 index 3e6b374c2a..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_brief_feature.h +++ /dev/null @@ -1,292 +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_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[53]; - _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(msg), buf, 53); -#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; - memcpy(packet.descriptor, descriptor, sizeof(uint8_t)*32); - memcpy(_MAV_PAYLOAD(msg), &packet, 53); -#endif - - msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; - return mavlink_finalize_message(msg, system_id, component_id, 53, 88); -} - -/** - * @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 was 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[53]; - _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(msg), buf, 53); -#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; - memcpy(packet.descriptor, descriptor, sizeof(uint8_t)*32); - memcpy(_MAV_PAYLOAD(msg), &packet, 53); -#endif - - msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 53, 88); -} - -/** - * @brief Encode a brief_feature struct into 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 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 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[53]; - _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); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, buf, 53, 88); -#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; - memcpy(packet.descriptor, descriptor, sizeof(uint8_t)*32); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, (const char *)&packet, 53, 88); -#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), 53); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_data_transmission_handshake.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_data_transmission_handshake.h deleted file mode 100644 index dccaace75b..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_data_transmission_handshake.h +++ /dev/null @@ -1,232 +0,0 @@ -// MESSAGE DATA_TRANSMISSION_HANDSHAKE PACKING - -#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE 193 - -typedef struct __mavlink_data_transmission_handshake_t -{ - uint32_t size; ///< total data size in bytes (set on ACK only) - uint8_t type; ///< type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - uint8_t packets; ///< number of packets beeing sent (set on ACK only) - uint8_t payload; ///< payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - uint8_t jpg_quality; ///< JPEG quality out of [1,100] -} mavlink_data_transmission_handshake_t; - -#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN 8 -#define MAVLINK_MSG_ID_193_LEN 8 - - - -#define MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE { \ - "DATA_TRANSMISSION_HANDSHAKE", \ - 5, \ - { { "size", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_data_transmission_handshake_t, size) }, \ - { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_data_transmission_handshake_t, type) }, \ - { "packets", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_data_transmission_handshake_t, packets) }, \ - { "payload", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_data_transmission_handshake_t, payload) }, \ - { "jpg_quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_data_transmission_handshake_t, jpg_quality) }, \ - } \ -} - - -/** - * @brief Pack a data_transmission_handshake 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 requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - * @param size total data size in bytes (set on ACK only) - * @param packets number of packets beeing sent (set on ACK only) - * @param payload payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - * @param jpg_quality JPEG quality out of [1,100] - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t type, uint32_t size, uint8_t packets, uint8_t payload, uint8_t jpg_quality) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[8]; - _mav_put_uint32_t(buf, 0, size); - _mav_put_uint8_t(buf, 4, type); - _mav_put_uint8_t(buf, 5, packets); - _mav_put_uint8_t(buf, 6, payload); - _mav_put_uint8_t(buf, 7, jpg_quality); - - memcpy(_MAV_PAYLOAD(msg), buf, 8); -#else - mavlink_data_transmission_handshake_t packet; - packet.size = size; - packet.type = type; - packet.packets = packets; - packet.payload = payload; - packet.jpg_quality = jpg_quality; - - memcpy(_MAV_PAYLOAD(msg), &packet, 8); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; - return mavlink_finalize_message(msg, system_id, component_id, 8, 148); -} - -/** - * @brief Pack a data_transmission_handshake 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param type type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - * @param size total data size in bytes (set on ACK only) - * @param packets number of packets beeing sent (set on ACK only) - * @param payload payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - * @param jpg_quality JPEG quality out of [1,100] - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data_transmission_handshake_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t type,uint32_t size,uint8_t packets,uint8_t payload,uint8_t jpg_quality) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[8]; - _mav_put_uint32_t(buf, 0, size); - _mav_put_uint8_t(buf, 4, type); - _mav_put_uint8_t(buf, 5, packets); - _mav_put_uint8_t(buf, 6, payload); - _mav_put_uint8_t(buf, 7, jpg_quality); - - memcpy(_MAV_PAYLOAD(msg), buf, 8); -#else - mavlink_data_transmission_handshake_t packet; - packet.size = size; - packet.type = type; - packet.packets = packets; - packet.payload = payload; - packet.jpg_quality = jpg_quality; - - memcpy(_MAV_PAYLOAD(msg), &packet, 8); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 148); -} - -/** - * @brief Encode a data_transmission_handshake struct into 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 data_transmission_handshake C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data_transmission_handshake_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data_transmission_handshake_t* data_transmission_handshake) -{ - return mavlink_msg_data_transmission_handshake_pack(system_id, component_id, msg, data_transmission_handshake->type, data_transmission_handshake->size, data_transmission_handshake->packets, data_transmission_handshake->payload, data_transmission_handshake->jpg_quality); -} - -/** - * @brief Send a data_transmission_handshake message - * @param chan MAVLink channel to send the message - * - * @param type type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - * @param size total data size in bytes (set on ACK only) - * @param packets number of packets beeing sent (set on ACK only) - * @param payload payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - * @param jpg_quality JPEG quality out of [1,100] - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_t chan, uint8_t type, uint32_t size, uint8_t packets, uint8_t payload, uint8_t jpg_quality) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[8]; - _mav_put_uint32_t(buf, 0, size); - _mav_put_uint8_t(buf, 4, type); - _mav_put_uint8_t(buf, 5, packets); - _mav_put_uint8_t(buf, 6, payload); - _mav_put_uint8_t(buf, 7, jpg_quality); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, buf, 8, 148); -#else - mavlink_data_transmission_handshake_t packet; - packet.size = size; - packet.type = type; - packet.packets = packets; - packet.payload = payload; - packet.jpg_quality = jpg_quality; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, (const char *)&packet, 8, 148); -#endif -} - -#endif - -// MESSAGE DATA_TRANSMISSION_HANDSHAKE UNPACKING - - -/** - * @brief Get field type from data_transmission_handshake message - * - * @return type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - */ -static inline uint8_t mavlink_msg_data_transmission_handshake_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field size from data_transmission_handshake message - * - * @return total data size in bytes (set on ACK only) - */ -static inline uint32_t mavlink_msg_data_transmission_handshake_get_size(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field packets from data_transmission_handshake message - * - * @return number of packets beeing sent (set on ACK only) - */ -static inline uint8_t mavlink_msg_data_transmission_handshake_get_packets(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field payload from data_transmission_handshake message - * - * @return payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - */ -static inline uint8_t mavlink_msg_data_transmission_handshake_get_payload(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @brief Get field jpg_quality from data_transmission_handshake message - * - * @return JPEG quality out of [1,100] - */ -static inline uint8_t mavlink_msg_data_transmission_handshake_get_jpg_quality(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 7); -} - -/** - * @brief Decode a data_transmission_handshake message into a struct - * - * @param msg The message to decode - * @param data_transmission_handshake C-struct to decode the message contents into - */ -static inline void mavlink_msg_data_transmission_handshake_decode(const mavlink_message_t* msg, mavlink_data_transmission_handshake_t* data_transmission_handshake) -{ -#if MAVLINK_NEED_BYTE_SWAP - data_transmission_handshake->size = mavlink_msg_data_transmission_handshake_get_size(msg); - data_transmission_handshake->type = mavlink_msg_data_transmission_handshake_get_type(msg); - data_transmission_handshake->packets = mavlink_msg_data_transmission_handshake_get_packets(msg); - data_transmission_handshake->payload = mavlink_msg_data_transmission_handshake_get_payload(msg); - data_transmission_handshake->jpg_quality = mavlink_msg_data_transmission_handshake_get_jpg_quality(msg); -#else - memcpy(data_transmission_handshake, _MAV_PAYLOAD(msg), 8); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_encapsulated_data.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_encapsulated_data.h deleted file mode 100644 index dcd65c2f44..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_encapsulated_data.h +++ /dev/null @@ -1,160 +0,0 @@ -// MESSAGE ENCAPSULATED_DATA PACKING - -#define MAVLINK_MSG_ID_ENCAPSULATED_DATA 194 - -typedef struct __mavlink_encapsulated_data_t -{ - uint16_t seqnr; ///< sequence number (starting with 0 on every transmission) - uint8_t data[253]; ///< image data bytes -} mavlink_encapsulated_data_t; - -#define MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN 255 -#define MAVLINK_MSG_ID_194_LEN 255 - -#define MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN 253 - -#define MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA { \ - "ENCAPSULATED_DATA", \ - 2, \ - { { "seqnr", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_encapsulated_data_t, seqnr) }, \ - { "data", NULL, MAVLINK_TYPE_UINT8_T, 253, 2, offsetof(mavlink_encapsulated_data_t, data) }, \ - } \ -} - - -/** - * @brief Pack a encapsulated_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 seqnr sequence number (starting with 0 on every transmission) - * @param data image data bytes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_encapsulated_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t seqnr, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[255]; - _mav_put_uint16_t(buf, 0, seqnr); - _mav_put_uint8_t_array(buf, 2, data, 253); - memcpy(_MAV_PAYLOAD(msg), buf, 255); -#else - mavlink_encapsulated_data_t packet; - packet.seqnr = seqnr; - memcpy(packet.data, data, sizeof(uint8_t)*253); - memcpy(_MAV_PAYLOAD(msg), &packet, 255); -#endif - - msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; - return mavlink_finalize_message(msg, system_id, component_id, 255, 223); -} - -/** - * @brief Pack a encapsulated_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 was sent over - * @param msg The MAVLink message to compress the data into - * @param seqnr sequence number (starting with 0 on every transmission) - * @param data image data bytes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_encapsulated_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t seqnr,const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[255]; - _mav_put_uint16_t(buf, 0, seqnr); - _mav_put_uint8_t_array(buf, 2, data, 253); - memcpy(_MAV_PAYLOAD(msg), buf, 255); -#else - mavlink_encapsulated_data_t packet; - packet.seqnr = seqnr; - memcpy(packet.data, data, sizeof(uint8_t)*253); - memcpy(_MAV_PAYLOAD(msg), &packet, 255); -#endif - - msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 255, 223); -} - -/** - * @brief Encode a encapsulated_data struct into 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 encapsulated_data C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_encapsulated_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_encapsulated_data_t* encapsulated_data) -{ - return mavlink_msg_encapsulated_data_pack(system_id, component_id, msg, encapsulated_data->seqnr, encapsulated_data->data); -} - -/** - * @brief Send a encapsulated_data message - * @param chan MAVLink channel to send the message - * - * @param seqnr sequence number (starting with 0 on every transmission) - * @param data image data bytes - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, uint16_t seqnr, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[255]; - _mav_put_uint16_t(buf, 0, seqnr); - _mav_put_uint8_t_array(buf, 2, data, 253); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, buf, 255, 223); -#else - mavlink_encapsulated_data_t packet; - packet.seqnr = seqnr; - memcpy(packet.data, data, sizeof(uint8_t)*253); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)&packet, 255, 223); -#endif -} - -#endif - -// MESSAGE ENCAPSULATED_DATA UNPACKING - - -/** - * @brief Get field seqnr from encapsulated_data message - * - * @return sequence number (starting with 0 on every transmission) - */ -static inline uint16_t mavlink_msg_encapsulated_data_get_seqnr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field data from encapsulated_data message - * - * @return image data bytes - */ -static inline uint16_t mavlink_msg_encapsulated_data_get_data(const mavlink_message_t* msg, uint8_t *data) -{ - return _MAV_RETURN_uint8_t_array(msg, data, 253, 2); -} - -/** - * @brief Decode a encapsulated_data message into a struct - * - * @param msg The message to decode - * @param encapsulated_data C-struct to decode the message contents into - */ -static inline void mavlink_msg_encapsulated_data_decode(const mavlink_message_t* msg, mavlink_encapsulated_data_t* encapsulated_data) -{ -#if MAVLINK_NEED_BYTE_SWAP - encapsulated_data->seqnr = mavlink_msg_encapsulated_data_get_seqnr(msg); - mavlink_msg_encapsulated_data_get_data(msg, encapsulated_data->data); -#else - memcpy(encapsulated_data, _MAV_PAYLOAD(msg), 255); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_image_available.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_image_available.h deleted file mode 100644 index cdbdf466fd..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_image_available.h +++ /dev/null @@ -1,628 +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_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[92]; - _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(msg), buf, 92); -#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(msg), &packet, 92); -#endif - - msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; - return mavlink_finalize_message(msg, system_id, component_id, 92, 224); -} - -/** - * @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 was 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[92]; - _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(msg), buf, 92); -#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(msg), &packet, 92); -#endif - - msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 92, 224); -} - -/** - * @brief Encode a image_available struct into 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 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 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[92]; - _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); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, buf, 92, 224); -#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; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, (const char *)&packet, 92, 224); -#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), 92); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_image_trigger_control.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_image_trigger_control.h deleted file mode 100644 index 08f4d7944d..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_image_trigger_control.h +++ /dev/null @@ -1,144 +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_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[1]; - _mav_put_uint8_t(buf, 0, enable); - - memcpy(_MAV_PAYLOAD(msg), buf, 1); -#else - mavlink_image_trigger_control_t packet; - packet.enable = enable; - - memcpy(_MAV_PAYLOAD(msg), &packet, 1); -#endif - - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; - return mavlink_finalize_message(msg, system_id, component_id, 1, 95); -} - -/** - * @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 was 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[1]; - _mav_put_uint8_t(buf, 0, enable); - - memcpy(_MAV_PAYLOAD(msg), buf, 1); -#else - mavlink_image_trigger_control_t packet; - packet.enable = enable; - - memcpy(_MAV_PAYLOAD(msg), &packet, 1); -#endif - - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 1, 95); -} - -/** - * @brief Encode a image_trigger_control struct into 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 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 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[1]; - _mav_put_uint8_t(buf, 0, enable); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, buf, 1, 95); -#else - mavlink_image_trigger_control_t packet; - packet.enable = enable; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, (const char *)&packet, 1, 95); -#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), 1); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_image_triggered.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_image_triggered.h deleted file mode 100644 index d82abcf8ee..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_image_triggered.h +++ /dev/null @@ -1,386 +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_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[52]; - _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(msg), buf, 52); -#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(msg), &packet, 52); -#endif - - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; - return mavlink_finalize_message(msg, system_id, component_id, 52, 86); -} - -/** - * @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 was 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[52]; - _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(msg), buf, 52); -#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(msg), &packet, 52); -#endif - - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 52, 86); -} - -/** - * @brief Encode a image_triggered struct into 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 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 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[52]; - _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); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, buf, 52, 86); -#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; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, (const char *)&packet, 52, 86); -#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), 52); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_marker.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_marker.h deleted file mode 100644 index f127d56730..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_marker.h +++ /dev/null @@ -1,276 +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_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[26]; - _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(msg), buf, 26); -#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(msg), &packet, 26); -#endif - - msg->msgid = MAVLINK_MSG_ID_MARKER; - return mavlink_finalize_message(msg, system_id, component_id, 26, 249); -} - -/** - * @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 was 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[26]; - _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(msg), buf, 26); -#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(msg), &packet, 26); -#endif - - msg->msgid = MAVLINK_MSG_ID_MARKER; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26, 249); -} - -/** - * @brief Encode a marker struct into 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 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 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[26]; - _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); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, buf, 26, 249); -#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; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, (const char *)&packet, 26, 249); -#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), 26); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_pattern_detected.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_pattern_detected.h deleted file mode 100644 index 2bbc64681a..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_pattern_detected.h +++ /dev/null @@ -1,204 +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_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[106]; - _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(msg), buf, 106); -#else - mavlink_pattern_detected_t packet; - packet.confidence = confidence; - packet.type = type; - packet.detected = detected; - memcpy(packet.file, file, sizeof(char)*100); - memcpy(_MAV_PAYLOAD(msg), &packet, 106); -#endif - - msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; - return mavlink_finalize_message(msg, system_id, component_id, 106, 90); -} - -/** - * @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 was 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[106]; - _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(msg), buf, 106); -#else - mavlink_pattern_detected_t packet; - packet.confidence = confidence; - packet.type = type; - packet.detected = detected; - memcpy(packet.file, file, sizeof(char)*100); - memcpy(_MAV_PAYLOAD(msg), &packet, 106); -#endif - - msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 106, 90); -} - -/** - * @brief Encode a pattern_detected struct into 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 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 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[106]; - _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); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, buf, 106, 90); -#else - mavlink_pattern_detected_t packet; - packet.confidence = confidence; - packet.type = type; - packet.detected = detected; - memcpy(packet.file, file, sizeof(char)*100); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, (const char *)&packet, 106, 90); -#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), 106); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_point_of_interest.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_point_of_interest.h deleted file mode 100644 index 2c14d2385a..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_point_of_interest.h +++ /dev/null @@ -1,292 +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_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[43]; - _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(msg), buf, 43); -#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; - memcpy(packet.name, name, sizeof(char)*26); - memcpy(_MAV_PAYLOAD(msg), &packet, 43); -#endif - - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; - return mavlink_finalize_message(msg, system_id, component_id, 43, 95); -} - -/** - * @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 was 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[43]; - _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(msg), buf, 43); -#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; - memcpy(packet.name, name, sizeof(char)*26); - memcpy(_MAV_PAYLOAD(msg), &packet, 43); -#endif - - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 43, 95); -} - -/** - * @brief Encode a point_of_interest struct into 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 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 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[43]; - _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); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, buf, 43, 95); -#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; - memcpy(packet.name, name, sizeof(char)*26); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, (const char *)&packet, 43, 95); -#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), 43); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h deleted file mode 100644 index b2c89ede83..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h +++ /dev/null @@ -1,358 +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_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[55]; - _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(msg), buf, 55); -#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; - memcpy(packet.name, name, sizeof(char)*26); - memcpy(_MAV_PAYLOAD(msg), &packet, 55); -#endif - - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; - return mavlink_finalize_message(msg, system_id, component_id, 55, 36); -} - -/** - * @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 was 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[55]; - _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(msg), buf, 55); -#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; - memcpy(packet.name, name, sizeof(char)*26); - memcpy(_MAV_PAYLOAD(msg), &packet, 55); -#endif - - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 55, 36); -} - -/** - * @brief Encode a point_of_interest_connection struct into 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 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 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[55]; - _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); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, buf, 55, 36); -#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; - memcpy(packet.name, name, sizeof(char)*26); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, (const char *)&packet, 55, 36); -#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), 55); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_position_control_offset_set.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_position_control_offset_set.h deleted file mode 100644 index cdb178d0ca..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_position_control_offset_set.h +++ /dev/null @@ -1,254 +0,0 @@ -// MESSAGE POSITION_CONTROL_OFFSET_SET PACKING - -#define MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET 160 - -typedef struct __mavlink_position_control_offset_set_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_position_control_offset_set_t; - -#define MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN 18 -#define MAVLINK_MSG_ID_160_LEN 18 - - - -#define MAVLINK_MESSAGE_INFO_POSITION_CONTROL_OFFSET_SET { \ - "POSITION_CONTROL_OFFSET_SET", \ - 6, \ - { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_position_control_offset_set_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_position_control_offset_set_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_position_control_offset_set_t, z) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_position_control_offset_set_t, yaw) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_position_control_offset_set_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_position_control_offset_set_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a position_control_offset_set message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param 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_position_control_offset_set_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[18]; - _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(msg), buf, 18); -#else - mavlink_position_control_offset_set_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(msg), &packet, 18); -#endif - - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; - return mavlink_finalize_message(msg, system_id, component_id, 18, 244); -} - -/** - * @brief Pack a position_control_offset_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 was 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_position_control_offset_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,float x,float y,float z,float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[18]; - _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(msg), buf, 18); -#else - mavlink_position_control_offset_set_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(msg), &packet, 18); -#endif - - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 244); -} - -/** - * @brief Encode a position_control_offset_set struct into 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 position_control_offset_set C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_position_control_offset_set_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_position_control_offset_set_t* position_control_offset_set) -{ - return mavlink_msg_position_control_offset_set_pack(system_id, component_id, msg, position_control_offset_set->target_system, position_control_offset_set->target_component, position_control_offset_set->x, position_control_offset_set->y, position_control_offset_set->z, position_control_offset_set->yaw); -} - -/** - * @brief Send a position_control_offset_set 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_position_control_offset_set_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[18]; - _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_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET, buf, 18, 244); -#else - mavlink_position_control_offset_set_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.target_system = target_system; - packet.target_component = target_component; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET, (const char *)&packet, 18, 244); -#endif -} - -#endif - -// MESSAGE POSITION_CONTROL_OFFSET_SET UNPACKING - - -/** - * @brief Get field target_system from position_control_offset_set message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_position_control_offset_set_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Get field target_component from position_control_offset_set message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_position_control_offset_set_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 17); -} - -/** - * @brief Get field x from position_control_offset_set message - * - * @return x position offset - */ -static inline float mavlink_msg_position_control_offset_set_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field y from position_control_offset_set message - * - * @return y position offset - */ -static inline float mavlink_msg_position_control_offset_set_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field z from position_control_offset_set message - * - * @return z position offset - */ -static inline float mavlink_msg_position_control_offset_set_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yaw from position_control_offset_set message - * - * @return yaw orientation offset in radians, 0 = NORTH - */ -static inline float mavlink_msg_position_control_offset_set_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a position_control_offset_set message into a struct - * - * @param msg The message to decode - * @param position_control_offset_set C-struct to decode the message contents into - */ -static inline void mavlink_msg_position_control_offset_set_decode(const mavlink_message_t* msg, mavlink_position_control_offset_set_t* position_control_offset_set) -{ -#if MAVLINK_NEED_BYTE_SWAP - position_control_offset_set->x = mavlink_msg_position_control_offset_set_get_x(msg); - position_control_offset_set->y = mavlink_msg_position_control_offset_set_get_y(msg); - position_control_offset_set->z = mavlink_msg_position_control_offset_set_get_z(msg); - position_control_offset_set->yaw = mavlink_msg_position_control_offset_set_get_yaw(msg); - position_control_offset_set->target_system = mavlink_msg_position_control_offset_set_get_target_system(msg); - position_control_offset_set->target_component = mavlink_msg_position_control_offset_set_get_target_component(msg); -#else - memcpy(position_control_offset_set, _MAV_PAYLOAD(msg), 18); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_position_control_setpoint.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_position_control_setpoint.h deleted file mode 100644 index c170fb2265..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_position_control_setpoint.h +++ /dev/null @@ -1,232 +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_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[18]; - _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(msg), buf, 18); -#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(msg), &packet, 18); -#endif - - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; - return mavlink_finalize_message(msg, system_id, component_id, 18, 28); -} - -/** - * @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 was 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[18]; - _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(msg), buf, 18); -#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(msg), &packet, 18); -#endif - - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 28); -} - -/** - * @brief Encode a position_control_setpoint struct into 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 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 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[18]; - _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); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, buf, 18, 28); -#else - mavlink_position_control_setpoint_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.id = id; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, (const char *)&packet, 18, 28); -#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), 18); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_position_control_setpoint_set.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_position_control_setpoint_set.h deleted file mode 100644 index e24b347151..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_position_control_setpoint_set.h +++ /dev/null @@ -1,276 +0,0 @@ -// MESSAGE POSITION_CONTROL_SETPOINT_SET PACKING - -#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET 159 - -typedef struct __mavlink_position_control_setpoint_set_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 - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_position_control_setpoint_set_t; - -#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN 20 -#define MAVLINK_MSG_ID_159_LEN 20 - - - -#define MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT_SET { \ - "POSITION_CONTROL_SETPOINT_SET", \ - 7, \ - { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_position_control_setpoint_set_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_position_control_setpoint_set_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_position_control_setpoint_set_t, z) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_position_control_setpoint_set_t, yaw) }, \ - { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_position_control_setpoint_set_t, id) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_position_control_setpoint_set_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_position_control_setpoint_set_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a position_control_setpoint_set message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param 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_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t id, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[20]; - _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); - _mav_put_uint8_t(buf, 18, target_system); - _mav_put_uint8_t(buf, 19, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 20); -#else - mavlink_position_control_setpoint_set_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.id = id; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 20); -#endif - - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; - return mavlink_finalize_message(msg, system_id, component_id, 20, 11); -} - -/** - * @brief Pack a position_control_setpoint_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 was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @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_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,uint16_t id,float x,float y,float z,float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[20]; - _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); - _mav_put_uint8_t(buf, 18, target_system); - _mav_put_uint8_t(buf, 19, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 20); -#else - mavlink_position_control_setpoint_set_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.id = id; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 20); -#endif - - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 11); -} - -/** - * @brief Encode a position_control_setpoint_set struct into 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 position_control_setpoint_set C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_position_control_setpoint_set_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_position_control_setpoint_set_t* position_control_setpoint_set) -{ - return mavlink_msg_position_control_setpoint_set_pack(system_id, component_id, msg, position_control_setpoint_set->target_system, position_control_setpoint_set->target_component, position_control_setpoint_set->id, position_control_setpoint_set->x, position_control_setpoint_set->y, position_control_setpoint_set->z, position_control_setpoint_set->yaw); -} - -/** - * @brief Send a position_control_setpoint_set message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @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_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t id, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[20]; - _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); - _mav_put_uint8_t(buf, 18, target_system); - _mav_put_uint8_t(buf, 19, target_component); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET, buf, 20, 11); -#else - mavlink_position_control_setpoint_set_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.id = id; - packet.target_system = target_system; - packet.target_component = target_component; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET, (const char *)&packet, 20, 11); -#endif -} - -#endif - -// MESSAGE POSITION_CONTROL_SETPOINT_SET UNPACKING - - -/** - * @brief Get field target_system from position_control_setpoint_set message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_position_control_setpoint_set_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 18); -} - -/** - * @brief Get field target_component from position_control_setpoint_set message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_position_control_setpoint_set_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 19); -} - -/** - * @brief Get field id from position_control_setpoint_set message - * - * @return ID of waypoint, 0 for plain position - */ -static inline uint16_t mavlink_msg_position_control_setpoint_set_get_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 16); -} - -/** - * @brief Get field x from position_control_setpoint_set message - * - * @return x position - */ -static inline float mavlink_msg_position_control_setpoint_set_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field y from position_control_setpoint_set message - * - * @return y position - */ -static inline float mavlink_msg_position_control_setpoint_set_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field z from position_control_setpoint_set message - * - * @return z position - */ -static inline float mavlink_msg_position_control_setpoint_set_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yaw from position_control_setpoint_set message - * - * @return yaw orientation in radians, 0 = NORTH - */ -static inline float mavlink_msg_position_control_setpoint_set_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a position_control_setpoint_set message into a struct - * - * @param msg The message to decode - * @param position_control_setpoint_set C-struct to decode the message contents into - */ -static inline void mavlink_msg_position_control_setpoint_set_decode(const mavlink_message_t* msg, mavlink_position_control_setpoint_set_t* position_control_setpoint_set) -{ -#if MAVLINK_NEED_BYTE_SWAP - position_control_setpoint_set->x = mavlink_msg_position_control_setpoint_set_get_x(msg); - position_control_setpoint_set->y = mavlink_msg_position_control_setpoint_set_get_y(msg); - position_control_setpoint_set->z = mavlink_msg_position_control_setpoint_set_get_z(msg); - position_control_setpoint_set->yaw = mavlink_msg_position_control_setpoint_set_get_yaw(msg); - position_control_setpoint_set->id = mavlink_msg_position_control_setpoint_set_get_id(msg); - position_control_setpoint_set->target_system = mavlink_msg_position_control_setpoint_set_get_target_system(msg); - position_control_setpoint_set->target_component = mavlink_msg_position_control_setpoint_set_get_target_component(msg); -#else - memcpy(position_control_setpoint_set, _MAV_PAYLOAD(msg), 20); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_raw_aux.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_raw_aux.h deleted file mode 100644 index 0bbf4d6220..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_raw_aux.h +++ /dev/null @@ -1,276 +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_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[16]; - _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(msg), buf, 16); -#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(msg), &packet, 16); -#endif - - msg->msgid = MAVLINK_MSG_ID_RAW_AUX; - return mavlink_finalize_message(msg, system_id, component_id, 16, 182); -} - -/** - * @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 was 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[16]; - _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(msg), buf, 16); -#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(msg), &packet, 16); -#endif - - msg->msgid = MAVLINK_MSG_ID_RAW_AUX; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 182); -} - -/** - * @brief Encode a raw_aux struct into 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 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 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[16]; - _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); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, buf, 16, 182); -#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; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, (const char *)&packet, 16, 182); -#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), 16); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_set_cam_shutter.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_set_cam_shutter.h deleted file mode 100644 index d72dc56975..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_set_cam_shutter.h +++ /dev/null @@ -1,254 +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_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[11]; - _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(msg), buf, 11); -#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(msg), &packet, 11); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; - return mavlink_finalize_message(msg, system_id, component_id, 11, 108); -} - -/** - * @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 was 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[11]; - _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(msg), buf, 11); -#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(msg), &packet, 11); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 11, 108); -} - -/** - * @brief Encode a set_cam_shutter struct into 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 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 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[11]; - _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); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, buf, 11, 108); -#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; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, (const char *)&packet, 11, 108); -#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), 11); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_vicon_position_estimate.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_vicon_position_estimate.h deleted file mode 100644 index 40da2274d8..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_vicon_position_estimate.h +++ /dev/null @@ -1,276 +0,0 @@ -// MESSAGE VICON_POSITION_ESTIMATE PACKING - -#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE 157 - -typedef struct __mavlink_vicon_position_estimate_t -{ - uint64_t usec; ///< Timestamp (milliseconds) - float x; ///< Global X position - float y; ///< Global Y position - float z; ///< Global Z position - float roll; ///< Roll angle in rad - float pitch; ///< Pitch angle in rad - float yaw; ///< Yaw angle in rad -} mavlink_vicon_position_estimate_t; - -#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN 32 -#define MAVLINK_MSG_ID_157_LEN 32 - - - -#define MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE { \ - "VICON_POSITION_ESTIMATE", \ - 7, \ - { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vicon_position_estimate_t, usec) }, \ - { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vicon_position_estimate_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vicon_position_estimate_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vicon_position_estimate_t, z) }, \ - { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vicon_position_estimate_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vicon_position_estimate_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vicon_position_estimate_t, yaw) }, \ - } \ -} - - -/** - * @brief Pack a vicon_position_estimate 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 usec Timestamp (milliseconds) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[32]; - _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); - - memcpy(_MAV_PAYLOAD(msg), buf, 32); -#else - mavlink_vicon_position_estimate_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - - memcpy(_MAV_PAYLOAD(msg), &packet, 32); -#endif - - msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; - return mavlink_finalize_message(msg, system_id, component_id, 32, 56); -} - -/** - * @brief Pack a vicon_position_estimate 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (milliseconds) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[32]; - _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); - - memcpy(_MAV_PAYLOAD(msg), buf, 32); -#else - mavlink_vicon_position_estimate_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - - memcpy(_MAV_PAYLOAD(msg), &packet, 32); -#endif - - msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 56); -} - -/** - * @brief Encode a vicon_position_estimate struct into 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 vicon_position_estimate C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vicon_position_estimate_t* vicon_position_estimate) -{ - return mavlink_msg_vicon_position_estimate_pack(system_id, component_id, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw); -} - -/** - * @brief Send a vicon_position_estimate message - * @param chan MAVLink channel to send the message - * - * @param usec Timestamp (milliseconds) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[32]; - _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); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, 32, 56); -#else - mavlink_vicon_position_estimate_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)&packet, 32, 56); -#endif -} - -#endif - -// MESSAGE VICON_POSITION_ESTIMATE UNPACKING - - -/** - * @brief Get field usec from vicon_position_estimate message - * - * @return Timestamp (milliseconds) - */ -static inline uint64_t mavlink_msg_vicon_position_estimate_get_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field x from vicon_position_estimate message - * - * @return Global X position - */ -static inline float mavlink_msg_vicon_position_estimate_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field y from vicon_position_estimate message - * - * @return Global Y position - */ -static inline float mavlink_msg_vicon_position_estimate_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field z from vicon_position_estimate message - * - * @return Global Z position - */ -static inline float mavlink_msg_vicon_position_estimate_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field roll from vicon_position_estimate message - * - * @return Roll angle in rad - */ -static inline float mavlink_msg_vicon_position_estimate_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field pitch from vicon_position_estimate message - * - * @return Pitch angle in rad - */ -static inline float mavlink_msg_vicon_position_estimate_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field yaw from vicon_position_estimate message - * - * @return Yaw angle in rad - */ -static inline float mavlink_msg_vicon_position_estimate_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Decode a vicon_position_estimate message into a struct - * - * @param msg The message to decode - * @param vicon_position_estimate C-struct to decode the message contents into - */ -static inline void mavlink_msg_vicon_position_estimate_decode(const mavlink_message_t* msg, mavlink_vicon_position_estimate_t* vicon_position_estimate) -{ -#if MAVLINK_NEED_BYTE_SWAP - vicon_position_estimate->usec = mavlink_msg_vicon_position_estimate_get_usec(msg); - vicon_position_estimate->x = mavlink_msg_vicon_position_estimate_get_x(msg); - vicon_position_estimate->y = mavlink_msg_vicon_position_estimate_get_y(msg); - vicon_position_estimate->z = mavlink_msg_vicon_position_estimate_get_z(msg); - vicon_position_estimate->roll = mavlink_msg_vicon_position_estimate_get_roll(msg); - vicon_position_estimate->pitch = mavlink_msg_vicon_position_estimate_get_pitch(msg); - vicon_position_estimate->yaw = mavlink_msg_vicon_position_estimate_get_yaw(msg); -#else - memcpy(vicon_position_estimate, _MAV_PAYLOAD(msg), 32); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_vision_position_estimate.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_vision_position_estimate.h deleted file mode 100644 index df39cbbfb1..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_vision_position_estimate.h +++ /dev/null @@ -1,276 +0,0 @@ -// MESSAGE VISION_POSITION_ESTIMATE PACKING - -#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE 156 - -typedef struct __mavlink_vision_position_estimate_t -{ - uint64_t usec; ///< Timestamp (milliseconds) - float x; ///< Global X position - float y; ///< Global Y position - float z; ///< Global Z position - float roll; ///< Roll angle in rad - float pitch; ///< Pitch angle in rad - float yaw; ///< Yaw angle in rad -} mavlink_vision_position_estimate_t; - -#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN 32 -#define MAVLINK_MSG_ID_156_LEN 32 - - - -#define MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE { \ - "VISION_POSITION_ESTIMATE", \ - 7, \ - { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vision_position_estimate_t, usec) }, \ - { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vision_position_estimate_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vision_position_estimate_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vision_position_estimate_t, z) }, \ - { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vision_position_estimate_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vision_position_estimate_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vision_position_estimate_t, yaw) }, \ - } \ -} - - -/** - * @brief Pack a vision_position_estimate 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 usec Timestamp (milliseconds) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[32]; - _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); - - memcpy(_MAV_PAYLOAD(msg), buf, 32); -#else - mavlink_vision_position_estimate_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - - memcpy(_MAV_PAYLOAD(msg), &packet, 32); -#endif - - msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; - return mavlink_finalize_message(msg, system_id, component_id, 32, 158); -} - -/** - * @brief Pack a vision_position_estimate 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (milliseconds) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[32]; - _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); - - memcpy(_MAV_PAYLOAD(msg), buf, 32); -#else - mavlink_vision_position_estimate_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - - memcpy(_MAV_PAYLOAD(msg), &packet, 32); -#endif - - msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 158); -} - -/** - * @brief Encode a vision_position_estimate struct into 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 vision_position_estimate C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vision_position_estimate_t* vision_position_estimate) -{ - return mavlink_msg_vision_position_estimate_pack(system_id, component_id, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw); -} - -/** - * @brief Send a vision_position_estimate message - * @param chan MAVLink channel to send the message - * - * @param usec Timestamp (milliseconds) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[32]; - _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); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, 32, 158); -#else - mavlink_vision_position_estimate_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)&packet, 32, 158); -#endif -} - -#endif - -// MESSAGE VISION_POSITION_ESTIMATE UNPACKING - - -/** - * @brief Get field usec from vision_position_estimate message - * - * @return Timestamp (milliseconds) - */ -static inline uint64_t mavlink_msg_vision_position_estimate_get_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field x from vision_position_estimate message - * - * @return Global X position - */ -static inline float mavlink_msg_vision_position_estimate_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field y from vision_position_estimate message - * - * @return Global Y position - */ -static inline float mavlink_msg_vision_position_estimate_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field z from vision_position_estimate message - * - * @return Global Z position - */ -static inline float mavlink_msg_vision_position_estimate_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field roll from vision_position_estimate message - * - * @return Roll angle in rad - */ -static inline float mavlink_msg_vision_position_estimate_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field pitch from vision_position_estimate message - * - * @return Pitch angle in rad - */ -static inline float mavlink_msg_vision_position_estimate_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field yaw from vision_position_estimate message - * - * @return Yaw angle in rad - */ -static inline float mavlink_msg_vision_position_estimate_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Decode a vision_position_estimate message into a struct - * - * @param msg The message to decode - * @param vision_position_estimate C-struct to decode the message contents into - */ -static inline void mavlink_msg_vision_position_estimate_decode(const mavlink_message_t* msg, mavlink_vision_position_estimate_t* vision_position_estimate) -{ -#if MAVLINK_NEED_BYTE_SWAP - vision_position_estimate->usec = mavlink_msg_vision_position_estimate_get_usec(msg); - vision_position_estimate->x = mavlink_msg_vision_position_estimate_get_x(msg); - vision_position_estimate->y = mavlink_msg_vision_position_estimate_get_y(msg); - vision_position_estimate->z = mavlink_msg_vision_position_estimate_get_z(msg); - vision_position_estimate->roll = mavlink_msg_vision_position_estimate_get_roll(msg); - vision_position_estimate->pitch = mavlink_msg_vision_position_estimate_get_pitch(msg); - vision_position_estimate->yaw = mavlink_msg_vision_position_estimate_get_yaw(msg); -#else - memcpy(vision_position_estimate, _MAV_PAYLOAD(msg), 32); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_vision_speed_estimate.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_vision_speed_estimate.h deleted file mode 100644 index 500947d6f4..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_vision_speed_estimate.h +++ /dev/null @@ -1,210 +0,0 @@ -// MESSAGE VISION_SPEED_ESTIMATE PACKING - -#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE 158 - -typedef struct __mavlink_vision_speed_estimate_t -{ - uint64_t usec; ///< Timestamp (milliseconds) - float x; ///< Global X speed - float y; ///< Global Y speed - float z; ///< Global Z speed -} mavlink_vision_speed_estimate_t; - -#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN 20 -#define MAVLINK_MSG_ID_158_LEN 20 - - - -#define MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE { \ - "VISION_SPEED_ESTIMATE", \ - 4, \ - { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vision_speed_estimate_t, usec) }, \ - { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vision_speed_estimate_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vision_speed_estimate_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vision_speed_estimate_t, z) }, \ - } \ -} - - -/** - * @brief Pack a vision_speed_estimate 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 usec Timestamp (milliseconds) - * @param x Global X speed - * @param y Global Y speed - * @param z Global Z speed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t usec, float x, float y, float z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[20]; - _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); - - memcpy(_MAV_PAYLOAD(msg), buf, 20); -#else - mavlink_vision_speed_estimate_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - - memcpy(_MAV_PAYLOAD(msg), &packet, 20); -#endif - - msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; - return mavlink_finalize_message(msg, system_id, component_id, 20, 208); -} - -/** - * @brief Pack a vision_speed_estimate 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (milliseconds) - * @param x Global X speed - * @param y Global Y speed - * @param z Global Z speed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t usec,float x,float y,float z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[20]; - _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); - - memcpy(_MAV_PAYLOAD(msg), buf, 20); -#else - mavlink_vision_speed_estimate_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - - memcpy(_MAV_PAYLOAD(msg), &packet, 20); -#endif - - msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 208); -} - -/** - * @brief Encode a vision_speed_estimate struct into 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 vision_speed_estimate C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vision_speed_estimate_t* vision_speed_estimate) -{ - return mavlink_msg_vision_speed_estimate_pack(system_id, component_id, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z); -} - -/** - * @brief Send a vision_speed_estimate message - * @param chan MAVLink channel to send the message - * - * @param usec Timestamp (milliseconds) - * @param x Global X speed - * @param y Global Y speed - * @param z Global Z speed - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[20]; - _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_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, 20, 208); -#else - mavlink_vision_speed_estimate_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)&packet, 20, 208); -#endif -} - -#endif - -// MESSAGE VISION_SPEED_ESTIMATE UNPACKING - - -/** - * @brief Get field usec from vision_speed_estimate message - * - * @return Timestamp (milliseconds) - */ -static inline uint64_t mavlink_msg_vision_speed_estimate_get_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field x from vision_speed_estimate message - * - * @return Global X speed - */ -static inline float mavlink_msg_vision_speed_estimate_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field y from vision_speed_estimate message - * - * @return Global Y speed - */ -static inline float mavlink_msg_vision_speed_estimate_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field z from vision_speed_estimate message - * - * @return Global Z speed - */ -static inline float mavlink_msg_vision_speed_estimate_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Decode a vision_speed_estimate message into a struct - * - * @param msg The message to decode - * @param vision_speed_estimate C-struct to decode the message contents into - */ -static inline void mavlink_msg_vision_speed_estimate_decode(const mavlink_message_t* msg, mavlink_vision_speed_estimate_t* vision_speed_estimate) -{ -#if MAVLINK_NEED_BYTE_SWAP - vision_speed_estimate->usec = mavlink_msg_vision_speed_estimate_get_usec(msg); - vision_speed_estimate->x = mavlink_msg_vision_speed_estimate_get_x(msg); - vision_speed_estimate->y = mavlink_msg_vision_speed_estimate_get_y(msg); - vision_speed_estimate->z = mavlink_msg_vision_speed_estimate_get_z(msg); -#else - memcpy(vision_speed_estimate, _MAV_PAYLOAD(msg), 20); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_watchdog_command.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_watchdog_command.h deleted file mode 100644 index 1936f00515..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_watchdog_command.h +++ /dev/null @@ -1,210 +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_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[6]; - _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(msg), buf, 6); -#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(msg), &packet, 6); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; - return mavlink_finalize_message(msg, system_id, component_id, 6, 162); -} - -/** - * @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 was 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[6]; - _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(msg), buf, 6); -#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(msg), &packet, 6); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 6, 162); -} - -/** - * @brief Encode a watchdog_command struct into 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 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 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[6]; - _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); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, buf, 6, 162); -#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; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, (const char *)&packet, 6, 162); -#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), 6); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h deleted file mode 100644 index 6c3dc333d2..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h +++ /dev/null @@ -1,166 +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_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[4]; - _mav_put_uint16_t(buf, 0, watchdog_id); - _mav_put_uint16_t(buf, 2, process_count); - - memcpy(_MAV_PAYLOAD(msg), buf, 4); -#else - mavlink_watchdog_heartbeat_t packet; - packet.watchdog_id = watchdog_id; - packet.process_count = process_count; - - memcpy(_MAV_PAYLOAD(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; - return mavlink_finalize_message(msg, system_id, component_id, 4, 153); -} - -/** - * @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 was 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[4]; - _mav_put_uint16_t(buf, 0, watchdog_id); - _mav_put_uint16_t(buf, 2, process_count); - - memcpy(_MAV_PAYLOAD(msg), buf, 4); -#else - mavlink_watchdog_heartbeat_t packet; - packet.watchdog_id = watchdog_id; - packet.process_count = process_count; - - memcpy(_MAV_PAYLOAD(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 153); -} - -/** - * @brief Encode a watchdog_heartbeat struct into 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 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 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[4]; - _mav_put_uint16_t(buf, 0, watchdog_id); - _mav_put_uint16_t(buf, 2, process_count); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, buf, 4, 153); -#else - mavlink_watchdog_heartbeat_t packet; - packet.watchdog_id = watchdog_id; - packet.process_count = process_count; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, (const char *)&packet, 4, 153); -#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), 4); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_watchdog_process_info.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_watchdog_process_info.h deleted file mode 100644 index 59b0d3048c..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_watchdog_process_info.h +++ /dev/null @@ -1,227 +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_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[255]; - _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(msg), buf, 255); -#else - mavlink_watchdog_process_info_t packet; - packet.timeout = timeout; - packet.watchdog_id = watchdog_id; - packet.process_id = process_id; - memcpy(packet.name, name, sizeof(char)*100); - memcpy(packet.arguments, arguments, sizeof(char)*147); - memcpy(_MAV_PAYLOAD(msg), &packet, 255); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; - return mavlink_finalize_message(msg, system_id, component_id, 255, 16); -} - -/** - * @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 was 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[255]; - _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(msg), buf, 255); -#else - mavlink_watchdog_process_info_t packet; - packet.timeout = timeout; - packet.watchdog_id = watchdog_id; - packet.process_id = process_id; - memcpy(packet.name, name, sizeof(char)*100); - memcpy(packet.arguments, arguments, sizeof(char)*147); - memcpy(_MAV_PAYLOAD(msg), &packet, 255); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 255, 16); -} - -/** - * @brief Encode a watchdog_process_info struct into 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 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 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[255]; - _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); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, buf, 255, 16); -#else - mavlink_watchdog_process_info_t packet; - packet.timeout = timeout; - packet.watchdog_id = watchdog_id; - packet.process_id = process_id; - memcpy(packet.name, name, sizeof(char)*100); - memcpy(packet.arguments, arguments, sizeof(char)*147); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, (const char *)&packet, 255, 16); -#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), 255); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_watchdog_process_status.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_watchdog_process_status.h deleted file mode 100644 index 6fb26e5735..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/mavlink_msg_watchdog_process_status.h +++ /dev/null @@ -1,254 +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_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[12]; - _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(msg), buf, 12); -#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(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; - return mavlink_finalize_message(msg, system_id, component_id, 12, 29); -} - -/** - * @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 was 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[12]; - _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(msg), buf, 12); -#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(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 29); -} - -/** - * @brief Encode a watchdog_process_status struct into 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 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 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[12]; - _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); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, buf, 12, 29); -#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; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, (const char *)&packet, 12, 29); -#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), 12); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/pixhawk.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/pixhawk.h deleted file mode 100644 index 51b2eb2b60..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/pixhawk.h +++ /dev/null @@ -1,80 +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, 0, 28, 22, 22, 21, 0, 36, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 0, 0, 26, 0, 36, 0, 6, 4, 0, 21, 18, 0, 0, 0, 20, 20, 32, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 18, 32, 32, 20, 32, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 11, 52, 1, 92, 0, 0, 0, 0, 20, 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, 8, 255, 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, 3} -#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, 0, 104, 244, 237, 222, 0, 158, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 0, 0, 183, 0, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 160, 168, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 19, 102, 158, 208, 56, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 108, 86, 95, 224, 0, 0, 0, 0, 11, 244, 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, 148, 223, 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, 247} -#endif - -#ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {NULL}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SET_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, 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, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, {NULL}, 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, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, {NULL}, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {NULL}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {NULL}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_SHORT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, 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, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SET_CAM_SHUTTER, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGERED, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGER_CONTROL, MAVLINK_MESSAGE_INFO_IMAGE_AVAILABLE, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT_SET, MAVLINK_MESSAGE_INFO_POSITION_CONTROL_OFFSET_SET, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT, MAVLINK_MESSAGE_INFO_MARKER, MAVLINK_MESSAGE_INFO_RAW_AUX, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_WATCHDOG_HEARTBEAT, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_INFO, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_STATUS, MAVLINK_MESSAGE_INFO_WATCHDOG_COMMAND, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PATTERN_DETECTED, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST_CONNECTION, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_BRIEF_FEATURE, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_ATTITUDE_CONTROL, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, MAVLINK_MESSAGE_INFO_EXTENDED_MESSAGE} -#endif - -#include "../protocol.h" - -#define MAVLINK_ENABLED_PIXHAWK - -#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 - -// ENUM DEFINITIONS - - -/** @brief Content Types for data transmission handshake */ -enum DATA_TYPES -{ - DATA_TYPE_JPEG_IMAGE=1, /* | */ - DATA_TYPE_RAW_IMAGE=2, /* | */ - DATA_TYPE_KINECT=3, /* | */ - DATA_TYPES_ENUM_END=4, /* | */ -}; - -// 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_position_control_setpoint_set.h" -#include "./mavlink_msg_position_control_offset_set.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_data_transmission_handshake.h" -#include "./mavlink_msg_encapsulated_data.h" -#include "./mavlink_msg_brief_feature.h" -#include "./mavlink_msg_attitude_control.h" - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // PIXHAWK_H diff --git a/libraries/GCS_MAVLink/include_v1.0/pixhawk/testsuite.h b/libraries/GCS_MAVLink/include_v1.0/pixhawk/testsuite.h deleted file mode 100644 index ef2f0273b7..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/pixhawk/testsuite.h +++ /dev/null @@ -1,1150 +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_AIR_DATA; - return mavlink_finalize_message(msg, system_id, component_id, 10, 232); -} - -/** - * @brief Pack a air_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 was sent over - * @param msg The MAVLink message to compress the data into - * @param dynamicPressure Dynamic pressure (Pa) - * @param staticPressure Static pressure (Pa) - * @param temperature Board temperature - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_air_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float dynamicPressure,float staticPressure,uint16_t temperature) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[10]; - _mav_put_float(buf, 0, dynamicPressure); - _mav_put_float(buf, 4, staticPressure); - _mav_put_uint16_t(buf, 8, temperature); - - memcpy(_MAV_PAYLOAD(msg), buf, 10); -#else - mavlink_air_data_t packet; - packet.dynamicPressure = dynamicPressure; - packet.staticPressure = staticPressure; - packet.temperature = temperature; - - memcpy(_MAV_PAYLOAD(msg), &packet, 10); -#endif - - msg->msgid = MAVLINK_MSG_ID_AIR_DATA; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 10, 232); -} - -/** - * @brief Encode a air_data struct into 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 air_data C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_air_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_air_data_t* air_data) -{ - return mavlink_msg_air_data_pack(system_id, component_id, msg, air_data->dynamicPressure, air_data->staticPressure, air_data->temperature); -} - -/** - * @brief Send a air_data message - * @param chan MAVLink channel to send the message - * - * @param dynamicPressure Dynamic pressure (Pa) - * @param staticPressure Static pressure (Pa) - * @param temperature Board temperature - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_air_data_send(mavlink_channel_t chan, float dynamicPressure, float staticPressure, uint16_t temperature) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[10]; - _mav_put_float(buf, 0, dynamicPressure); - _mav_put_float(buf, 4, staticPressure); - _mav_put_uint16_t(buf, 8, temperature); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIR_DATA, buf, 10, 232); -#else - mavlink_air_data_t packet; - packet.dynamicPressure = dynamicPressure; - packet.staticPressure = staticPressure; - packet.temperature = temperature; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIR_DATA, (const char *)&packet, 10, 232); -#endif -} - -#endif - -// MESSAGE AIR_DATA UNPACKING - - -/** - * @brief Get field dynamicPressure from air_data message - * - * @return Dynamic pressure (Pa) - */ -static inline float mavlink_msg_air_data_get_dynamicPressure(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field staticPressure from air_data message - * - * @return Static pressure (Pa) - */ -static inline float mavlink_msg_air_data_get_staticPressure(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field temperature from air_data message - * - * @return Board temperature - */ -static inline uint16_t mavlink_msg_air_data_get_temperature(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} - -/** - * @brief Decode a air_data message into a struct - * - * @param msg The message to decode - * @param air_data C-struct to decode the message contents into - */ -static inline void mavlink_msg_air_data_decode(const mavlink_message_t* msg, mavlink_air_data_t* air_data) -{ -#if MAVLINK_NEED_BYTE_SWAP - air_data->dynamicPressure = mavlink_msg_air_data_get_dynamicPressure(msg); - air_data->staticPressure = mavlink_msg_air_data_get_staticPressure(msg); - air_data->temperature = mavlink_msg_air_data_get_temperature(msg); -#else - memcpy(air_data, _MAV_PAYLOAD(msg), 10); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_cpu_load.h b/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_cpu_load.h deleted file mode 100644 index af9e03d37b..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_cpu_load.h +++ /dev/null @@ -1,188 +0,0 @@ -// MESSAGE CPU_LOAD PACKING - -#define MAVLINK_MSG_ID_CPU_LOAD 170 - -typedef struct __mavlink_cpu_load_t -{ - uint16_t batVolt; ///< Battery Voltage in millivolts - uint8_t sensLoad; ///< Sensor DSC Load - uint8_t ctrlLoad; ///< Control DSC Load -} mavlink_cpu_load_t; - -#define MAVLINK_MSG_ID_CPU_LOAD_LEN 4 -#define MAVLINK_MSG_ID_170_LEN 4 - - - -#define MAVLINK_MESSAGE_INFO_CPU_LOAD { \ - "CPU_LOAD", \ - 3, \ - { { "batVolt", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_cpu_load_t, batVolt) }, \ - { "sensLoad", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_cpu_load_t, sensLoad) }, \ - { "ctrlLoad", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_cpu_load_t, ctrlLoad) }, \ - } \ -} - - -/** - * @brief Pack a cpu_load 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 sensLoad Sensor DSC Load - * @param ctrlLoad Control DSC Load - * @param batVolt Battery Voltage in millivolts - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_cpu_load_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t sensLoad, uint8_t ctrlLoad, uint16_t batVolt) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint16_t(buf, 0, batVolt); - _mav_put_uint8_t(buf, 2, sensLoad); - _mav_put_uint8_t(buf, 3, ctrlLoad); - - memcpy(_MAV_PAYLOAD(msg), buf, 4); -#else - mavlink_cpu_load_t packet; - packet.batVolt = batVolt; - packet.sensLoad = sensLoad; - packet.ctrlLoad = ctrlLoad; - - memcpy(_MAV_PAYLOAD(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; - return mavlink_finalize_message(msg, system_id, component_id, 4, 75); -} - -/** - * @brief Pack a cpu_load 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param sensLoad Sensor DSC Load - * @param ctrlLoad Control DSC Load - * @param batVolt Battery Voltage in millivolts - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_cpu_load_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t sensLoad,uint8_t ctrlLoad,uint16_t batVolt) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint16_t(buf, 0, batVolt); - _mav_put_uint8_t(buf, 2, sensLoad); - _mav_put_uint8_t(buf, 3, ctrlLoad); - - memcpy(_MAV_PAYLOAD(msg), buf, 4); -#else - mavlink_cpu_load_t packet; - packet.batVolt = batVolt; - packet.sensLoad = sensLoad; - packet.ctrlLoad = ctrlLoad; - - memcpy(_MAV_PAYLOAD(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 75); -} - -/** - * @brief Encode a cpu_load struct into 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 cpu_load C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_cpu_load_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cpu_load_t* cpu_load) -{ - return mavlink_msg_cpu_load_pack(system_id, component_id, msg, cpu_load->sensLoad, cpu_load->ctrlLoad, cpu_load->batVolt); -} - -/** - * @brief Send a cpu_load message - * @param chan MAVLink channel to send the message - * - * @param sensLoad Sensor DSC Load - * @param ctrlLoad Control DSC Load - * @param batVolt Battery Voltage in millivolts - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_cpu_load_send(mavlink_channel_t chan, uint8_t sensLoad, uint8_t ctrlLoad, uint16_t batVolt) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint16_t(buf, 0, batVolt); - _mav_put_uint8_t(buf, 2, sensLoad); - _mav_put_uint8_t(buf, 3, ctrlLoad); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CPU_LOAD, buf, 4, 75); -#else - mavlink_cpu_load_t packet; - packet.batVolt = batVolt; - packet.sensLoad = sensLoad; - packet.ctrlLoad = ctrlLoad; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CPU_LOAD, (const char *)&packet, 4, 75); -#endif -} - -#endif - -// MESSAGE CPU_LOAD UNPACKING - - -/** - * @brief Get field sensLoad from cpu_load message - * - * @return Sensor DSC Load - */ -static inline uint8_t mavlink_msg_cpu_load_get_sensLoad(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field ctrlLoad from cpu_load message - * - * @return Control DSC Load - */ -static inline uint8_t mavlink_msg_cpu_load_get_ctrlLoad(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field batVolt from cpu_load message - * - * @return Battery Voltage in millivolts - */ -static inline uint16_t mavlink_msg_cpu_load_get_batVolt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Decode a cpu_load message into a struct - * - * @param msg The message to decode - * @param cpu_load C-struct to decode the message contents into - */ -static inline void mavlink_msg_cpu_load_decode(const mavlink_message_t* msg, mavlink_cpu_load_t* cpu_load) -{ -#if MAVLINK_NEED_BYTE_SWAP - cpu_load->batVolt = mavlink_msg_cpu_load_get_batVolt(msg); - cpu_load->sensLoad = mavlink_msg_cpu_load_get_sensLoad(msg); - cpu_load->ctrlLoad = mavlink_msg_cpu_load_get_ctrlLoad(msg); -#else - memcpy(cpu_load, _MAV_PAYLOAD(msg), 4); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_ctrl_srfc_pt.h b/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_ctrl_srfc_pt.h deleted file mode 100644 index dc445e1989..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_ctrl_srfc_pt.h +++ /dev/null @@ -1,166 +0,0 @@ -// MESSAGE CTRL_SRFC_PT PACKING - -#define MAVLINK_MSG_ID_CTRL_SRFC_PT 181 - -typedef struct __mavlink_ctrl_srfc_pt_t -{ - uint16_t bitfieldPt; ///< Bitfield containing the PT configuration - uint8_t target; ///< The system setting the commands -} mavlink_ctrl_srfc_pt_t; - -#define MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN 3 -#define MAVLINK_MSG_ID_181_LEN 3 - - - -#define MAVLINK_MESSAGE_INFO_CTRL_SRFC_PT { \ - "CTRL_SRFC_PT", \ - 2, \ - { { "bitfieldPt", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_ctrl_srfc_pt_t, bitfieldPt) }, \ - { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_ctrl_srfc_pt_t, target) }, \ - } \ -} - - -/** - * @brief Pack a ctrl_srfc_pt 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 The system setting the commands - * @param bitfieldPt Bitfield containing the PT configuration - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target, uint16_t bitfieldPt) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[3]; - _mav_put_uint16_t(buf, 0, bitfieldPt); - _mav_put_uint8_t(buf, 2, target); - - memcpy(_MAV_PAYLOAD(msg), buf, 3); -#else - mavlink_ctrl_srfc_pt_t packet; - packet.bitfieldPt = bitfieldPt; - packet.target = target; - - memcpy(_MAV_PAYLOAD(msg), &packet, 3); -#endif - - msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; - return mavlink_finalize_message(msg, system_id, component_id, 3, 104); -} - -/** - * @brief Pack a ctrl_srfc_pt 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param target The system setting the commands - * @param bitfieldPt Bitfield containing the PT configuration - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target,uint16_t bitfieldPt) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[3]; - _mav_put_uint16_t(buf, 0, bitfieldPt); - _mav_put_uint8_t(buf, 2, target); - - memcpy(_MAV_PAYLOAD(msg), buf, 3); -#else - mavlink_ctrl_srfc_pt_t packet; - packet.bitfieldPt = bitfieldPt; - packet.target = target; - - memcpy(_MAV_PAYLOAD(msg), &packet, 3); -#endif - - msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 104); -} - -/** - * @brief Encode a ctrl_srfc_pt struct into 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 ctrl_srfc_pt C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ctrl_srfc_pt_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ctrl_srfc_pt_t* ctrl_srfc_pt) -{ - return mavlink_msg_ctrl_srfc_pt_pack(system_id, component_id, msg, ctrl_srfc_pt->target, ctrl_srfc_pt->bitfieldPt); -} - -/** - * @brief Send a ctrl_srfc_pt message - * @param chan MAVLink channel to send the message - * - * @param target The system setting the commands - * @param bitfieldPt Bitfield containing the PT configuration - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_ctrl_srfc_pt_send(mavlink_channel_t chan, uint8_t target, uint16_t bitfieldPt) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[3]; - _mav_put_uint16_t(buf, 0, bitfieldPt); - _mav_put_uint8_t(buf, 2, target); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CTRL_SRFC_PT, buf, 3, 104); -#else - mavlink_ctrl_srfc_pt_t packet; - packet.bitfieldPt = bitfieldPt; - packet.target = target; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CTRL_SRFC_PT, (const char *)&packet, 3, 104); -#endif -} - -#endif - -// MESSAGE CTRL_SRFC_PT UNPACKING - - -/** - * @brief Get field target from ctrl_srfc_pt message - * - * @return The system setting the commands - */ -static inline uint8_t mavlink_msg_ctrl_srfc_pt_get_target(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field bitfieldPt from ctrl_srfc_pt message - * - * @return Bitfield containing the PT configuration - */ -static inline uint16_t mavlink_msg_ctrl_srfc_pt_get_bitfieldPt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Decode a ctrl_srfc_pt message into a struct - * - * @param msg The message to decode - * @param ctrl_srfc_pt C-struct to decode the message contents into - */ -static inline void mavlink_msg_ctrl_srfc_pt_decode(const mavlink_message_t* msg, mavlink_ctrl_srfc_pt_t* ctrl_srfc_pt) -{ -#if MAVLINK_NEED_BYTE_SWAP - ctrl_srfc_pt->bitfieldPt = mavlink_msg_ctrl_srfc_pt_get_bitfieldPt(msg); - ctrl_srfc_pt->target = mavlink_msg_ctrl_srfc_pt_get_target(msg); -#else - memcpy(ctrl_srfc_pt, _MAV_PAYLOAD(msg), 3); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_data_log.h b/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_data_log.h deleted file mode 100644 index e852684e18..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_data_log.h +++ /dev/null @@ -1,254 +0,0 @@ -// MESSAGE DATA_LOG PACKING - -#define MAVLINK_MSG_ID_DATA_LOG 177 - -typedef struct __mavlink_data_log_t -{ - float fl_1; ///< Log value 1 - float fl_2; ///< Log value 2 - float fl_3; ///< Log value 3 - float fl_4; ///< Log value 4 - float fl_5; ///< Log value 5 - float fl_6; ///< Log value 6 -} mavlink_data_log_t; - -#define MAVLINK_MSG_ID_DATA_LOG_LEN 24 -#define MAVLINK_MSG_ID_177_LEN 24 - - - -#define MAVLINK_MESSAGE_INFO_DATA_LOG { \ - "DATA_LOG", \ - 6, \ - { { "fl_1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_data_log_t, fl_1) }, \ - { "fl_2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_data_log_t, fl_2) }, \ - { "fl_3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_data_log_t, fl_3) }, \ - { "fl_4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_data_log_t, fl_4) }, \ - { "fl_5", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_data_log_t, fl_5) }, \ - { "fl_6", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_data_log_t, fl_6) }, \ - } \ -} - - -/** - * @brief Pack a data_log 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 fl_1 Log value 1 - * @param fl_2 Log value 2 - * @param fl_3 Log value 3 - * @param fl_4 Log value 4 - * @param fl_5 Log value 5 - * @param fl_6 Log value 6 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data_log_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float fl_1, float fl_2, float fl_3, float fl_4, float fl_5, float fl_6) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[24]; - _mav_put_float(buf, 0, fl_1); - _mav_put_float(buf, 4, fl_2); - _mav_put_float(buf, 8, fl_3); - _mav_put_float(buf, 12, fl_4); - _mav_put_float(buf, 16, fl_5); - _mav_put_float(buf, 20, fl_6); - - memcpy(_MAV_PAYLOAD(msg), buf, 24); -#else - mavlink_data_log_t packet; - packet.fl_1 = fl_1; - packet.fl_2 = fl_2; - packet.fl_3 = fl_3; - packet.fl_4 = fl_4; - packet.fl_5 = fl_5; - packet.fl_6 = fl_6; - - memcpy(_MAV_PAYLOAD(msg), &packet, 24); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA_LOG; - return mavlink_finalize_message(msg, system_id, component_id, 24, 167); -} - -/** - * @brief Pack a data_log 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param fl_1 Log value 1 - * @param fl_2 Log value 2 - * @param fl_3 Log value 3 - * @param fl_4 Log value 4 - * @param fl_5 Log value 5 - * @param fl_6 Log value 6 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data_log_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float fl_1,float fl_2,float fl_3,float fl_4,float fl_5,float fl_6) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[24]; - _mav_put_float(buf, 0, fl_1); - _mav_put_float(buf, 4, fl_2); - _mav_put_float(buf, 8, fl_3); - _mav_put_float(buf, 12, fl_4); - _mav_put_float(buf, 16, fl_5); - _mav_put_float(buf, 20, fl_6); - - memcpy(_MAV_PAYLOAD(msg), buf, 24); -#else - mavlink_data_log_t packet; - packet.fl_1 = fl_1; - packet.fl_2 = fl_2; - packet.fl_3 = fl_3; - packet.fl_4 = fl_4; - packet.fl_5 = fl_5; - packet.fl_6 = fl_6; - - memcpy(_MAV_PAYLOAD(msg), &packet, 24); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA_LOG; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 24, 167); -} - -/** - * @brief Encode a data_log struct into 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 data_log C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data_log_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data_log_t* data_log) -{ - return mavlink_msg_data_log_pack(system_id, component_id, msg, data_log->fl_1, data_log->fl_2, data_log->fl_3, data_log->fl_4, data_log->fl_5, data_log->fl_6); -} - -/** - * @brief Send a data_log message - * @param chan MAVLink channel to send the message - * - * @param fl_1 Log value 1 - * @param fl_2 Log value 2 - * @param fl_3 Log value 3 - * @param fl_4 Log value 4 - * @param fl_5 Log value 5 - * @param fl_6 Log value 6 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_data_log_send(mavlink_channel_t chan, float fl_1, float fl_2, float fl_3, float fl_4, float fl_5, float fl_6) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[24]; - _mav_put_float(buf, 0, fl_1); - _mav_put_float(buf, 4, fl_2); - _mav_put_float(buf, 8, fl_3); - _mav_put_float(buf, 12, fl_4); - _mav_put_float(buf, 16, fl_5); - _mav_put_float(buf, 20, fl_6); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_LOG, buf, 24, 167); -#else - mavlink_data_log_t packet; - packet.fl_1 = fl_1; - packet.fl_2 = fl_2; - packet.fl_3 = fl_3; - packet.fl_4 = fl_4; - packet.fl_5 = fl_5; - packet.fl_6 = fl_6; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_LOG, (const char *)&packet, 24, 167); -#endif -} - -#endif - -// MESSAGE DATA_LOG UNPACKING - - -/** - * @brief Get field fl_1 from data_log message - * - * @return Log value 1 - */ -static inline float mavlink_msg_data_log_get_fl_1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field fl_2 from data_log message - * - * @return Log value 2 - */ -static inline float mavlink_msg_data_log_get_fl_2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field fl_3 from data_log message - * - * @return Log value 3 - */ -static inline float mavlink_msg_data_log_get_fl_3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field fl_4 from data_log message - * - * @return Log value 4 - */ -static inline float mavlink_msg_data_log_get_fl_4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field fl_5 from data_log message - * - * @return Log value 5 - */ -static inline float mavlink_msg_data_log_get_fl_5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field fl_6 from data_log message - * - * @return Log value 6 - */ -static inline float mavlink_msg_data_log_get_fl_6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Decode a data_log message into a struct - * - * @param msg The message to decode - * @param data_log C-struct to decode the message contents into - */ -static inline void mavlink_msg_data_log_decode(const mavlink_message_t* msg, mavlink_data_log_t* data_log) -{ -#if MAVLINK_NEED_BYTE_SWAP - data_log->fl_1 = mavlink_msg_data_log_get_fl_1(msg); - data_log->fl_2 = mavlink_msg_data_log_get_fl_2(msg); - data_log->fl_3 = mavlink_msg_data_log_get_fl_3(msg); - data_log->fl_4 = mavlink_msg_data_log_get_fl_4(msg); - data_log->fl_5 = mavlink_msg_data_log_get_fl_5(msg); - data_log->fl_6 = mavlink_msg_data_log_get_fl_6(msg); -#else - memcpy(data_log, _MAV_PAYLOAD(msg), 24); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_diagnostic.h b/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_diagnostic.h deleted file mode 100644 index 90f64dedd0..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_diagnostic.h +++ /dev/null @@ -1,254 +0,0 @@ -// MESSAGE DIAGNOSTIC PACKING - -#define MAVLINK_MSG_ID_DIAGNOSTIC 173 - -typedef struct __mavlink_diagnostic_t -{ - float diagFl1; ///< Diagnostic float 1 - float diagFl2; ///< Diagnostic float 2 - float diagFl3; ///< Diagnostic float 3 - int16_t diagSh1; ///< Diagnostic short 1 - int16_t diagSh2; ///< Diagnostic short 2 - int16_t diagSh3; ///< Diagnostic short 3 -} mavlink_diagnostic_t; - -#define MAVLINK_MSG_ID_DIAGNOSTIC_LEN 18 -#define MAVLINK_MSG_ID_173_LEN 18 - - - -#define MAVLINK_MESSAGE_INFO_DIAGNOSTIC { \ - "DIAGNOSTIC", \ - 6, \ - { { "diagFl1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_diagnostic_t, diagFl1) }, \ - { "diagFl2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_diagnostic_t, diagFl2) }, \ - { "diagFl3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_diagnostic_t, diagFl3) }, \ - { "diagSh1", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_diagnostic_t, diagSh1) }, \ - { "diagSh2", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_diagnostic_t, diagSh2) }, \ - { "diagSh3", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_diagnostic_t, diagSh3) }, \ - } \ -} - - -/** - * @brief Pack a diagnostic 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 diagFl1 Diagnostic float 1 - * @param diagFl2 Diagnostic float 2 - * @param diagFl3 Diagnostic float 3 - * @param diagSh1 Diagnostic short 1 - * @param diagSh2 Diagnostic short 2 - * @param diagSh3 Diagnostic short 3 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_diagnostic_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float diagFl1, float diagFl2, float diagFl3, int16_t diagSh1, int16_t diagSh2, int16_t diagSh3) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[18]; - _mav_put_float(buf, 0, diagFl1); - _mav_put_float(buf, 4, diagFl2); - _mav_put_float(buf, 8, diagFl3); - _mav_put_int16_t(buf, 12, diagSh1); - _mav_put_int16_t(buf, 14, diagSh2); - _mav_put_int16_t(buf, 16, diagSh3); - - memcpy(_MAV_PAYLOAD(msg), buf, 18); -#else - mavlink_diagnostic_t packet; - packet.diagFl1 = diagFl1; - packet.diagFl2 = diagFl2; - packet.diagFl3 = diagFl3; - packet.diagSh1 = diagSh1; - packet.diagSh2 = diagSh2; - packet.diagSh3 = diagSh3; - - memcpy(_MAV_PAYLOAD(msg), &packet, 18); -#endif - - msg->msgid = MAVLINK_MSG_ID_DIAGNOSTIC; - return mavlink_finalize_message(msg, system_id, component_id, 18, 2); -} - -/** - * @brief Pack a diagnostic 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param diagFl1 Diagnostic float 1 - * @param diagFl2 Diagnostic float 2 - * @param diagFl3 Diagnostic float 3 - * @param diagSh1 Diagnostic short 1 - * @param diagSh2 Diagnostic short 2 - * @param diagSh3 Diagnostic short 3 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_diagnostic_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float diagFl1,float diagFl2,float diagFl3,int16_t diagSh1,int16_t diagSh2,int16_t diagSh3) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[18]; - _mav_put_float(buf, 0, diagFl1); - _mav_put_float(buf, 4, diagFl2); - _mav_put_float(buf, 8, diagFl3); - _mav_put_int16_t(buf, 12, diagSh1); - _mav_put_int16_t(buf, 14, diagSh2); - _mav_put_int16_t(buf, 16, diagSh3); - - memcpy(_MAV_PAYLOAD(msg), buf, 18); -#else - mavlink_diagnostic_t packet; - packet.diagFl1 = diagFl1; - packet.diagFl2 = diagFl2; - packet.diagFl3 = diagFl3; - packet.diagSh1 = diagSh1; - packet.diagSh2 = diagSh2; - packet.diagSh3 = diagSh3; - - memcpy(_MAV_PAYLOAD(msg), &packet, 18); -#endif - - msg->msgid = MAVLINK_MSG_ID_DIAGNOSTIC; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 2); -} - -/** - * @brief Encode a diagnostic struct into 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 diagnostic C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_diagnostic_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_diagnostic_t* diagnostic) -{ - return mavlink_msg_diagnostic_pack(system_id, component_id, msg, diagnostic->diagFl1, diagnostic->diagFl2, diagnostic->diagFl3, diagnostic->diagSh1, diagnostic->diagSh2, diagnostic->diagSh3); -} - -/** - * @brief Send a diagnostic message - * @param chan MAVLink channel to send the message - * - * @param diagFl1 Diagnostic float 1 - * @param diagFl2 Diagnostic float 2 - * @param diagFl3 Diagnostic float 3 - * @param diagSh1 Diagnostic short 1 - * @param diagSh2 Diagnostic short 2 - * @param diagSh3 Diagnostic short 3 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_diagnostic_send(mavlink_channel_t chan, float diagFl1, float diagFl2, float diagFl3, int16_t diagSh1, int16_t diagSh2, int16_t diagSh3) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[18]; - _mav_put_float(buf, 0, diagFl1); - _mav_put_float(buf, 4, diagFl2); - _mav_put_float(buf, 8, diagFl3); - _mav_put_int16_t(buf, 12, diagSh1); - _mav_put_int16_t(buf, 14, diagSh2); - _mav_put_int16_t(buf, 16, diagSh3); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIAGNOSTIC, buf, 18, 2); -#else - mavlink_diagnostic_t packet; - packet.diagFl1 = diagFl1; - packet.diagFl2 = diagFl2; - packet.diagFl3 = diagFl3; - packet.diagSh1 = diagSh1; - packet.diagSh2 = diagSh2; - packet.diagSh3 = diagSh3; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIAGNOSTIC, (const char *)&packet, 18, 2); -#endif -} - -#endif - -// MESSAGE DIAGNOSTIC UNPACKING - - -/** - * @brief Get field diagFl1 from diagnostic message - * - * @return Diagnostic float 1 - */ -static inline float mavlink_msg_diagnostic_get_diagFl1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field diagFl2 from diagnostic message - * - * @return Diagnostic float 2 - */ -static inline float mavlink_msg_diagnostic_get_diagFl2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field diagFl3 from diagnostic message - * - * @return Diagnostic float 3 - */ -static inline float mavlink_msg_diagnostic_get_diagFl3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field diagSh1 from diagnostic message - * - * @return Diagnostic short 1 - */ -static inline int16_t mavlink_msg_diagnostic_get_diagSh1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 12); -} - -/** - * @brief Get field diagSh2 from diagnostic message - * - * @return Diagnostic short 2 - */ -static inline int16_t mavlink_msg_diagnostic_get_diagSh2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 14); -} - -/** - * @brief Get field diagSh3 from diagnostic message - * - * @return Diagnostic short 3 - */ -static inline int16_t mavlink_msg_diagnostic_get_diagSh3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 16); -} - -/** - * @brief Decode a diagnostic message into a struct - * - * @param msg The message to decode - * @param diagnostic C-struct to decode the message contents into - */ -static inline void mavlink_msg_diagnostic_decode(const mavlink_message_t* msg, mavlink_diagnostic_t* diagnostic) -{ -#if MAVLINK_NEED_BYTE_SWAP - diagnostic->diagFl1 = mavlink_msg_diagnostic_get_diagFl1(msg); - diagnostic->diagFl2 = mavlink_msg_diagnostic_get_diagFl2(msg); - diagnostic->diagFl3 = mavlink_msg_diagnostic_get_diagFl3(msg); - diagnostic->diagSh1 = mavlink_msg_diagnostic_get_diagSh1(msg); - diagnostic->diagSh2 = mavlink_msg_diagnostic_get_diagSh2(msg); - diagnostic->diagSh3 = mavlink_msg_diagnostic_get_diagSh3(msg); -#else - memcpy(diagnostic, _MAV_PAYLOAD(msg), 18); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_gps_date_time.h b/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_gps_date_time.h deleted file mode 100644 index d4fb12bf87..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_gps_date_time.h +++ /dev/null @@ -1,276 +0,0 @@ -// MESSAGE GPS_DATE_TIME PACKING - -#define MAVLINK_MSG_ID_GPS_DATE_TIME 179 - -typedef struct __mavlink_gps_date_time_t -{ - uint8_t year; ///< Year reported by Gps - uint8_t month; ///< Month reported by Gps - uint8_t day; ///< Day reported by Gps - uint8_t hour; ///< Hour reported by Gps - uint8_t min; ///< Min reported by Gps - uint8_t sec; ///< Sec reported by Gps - uint8_t visSat; ///< Visible sattelites reported by Gps -} mavlink_gps_date_time_t; - -#define MAVLINK_MSG_ID_GPS_DATE_TIME_LEN 7 -#define MAVLINK_MSG_ID_179_LEN 7 - - - -#define MAVLINK_MESSAGE_INFO_GPS_DATE_TIME { \ - "GPS_DATE_TIME", \ - 7, \ - { { "year", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gps_date_time_t, year) }, \ - { "month", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_gps_date_time_t, month) }, \ - { "day", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_gps_date_time_t, day) }, \ - { "hour", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_gps_date_time_t, hour) }, \ - { "min", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_gps_date_time_t, min) }, \ - { "sec", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_gps_date_time_t, sec) }, \ - { "visSat", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_gps_date_time_t, visSat) }, \ - } \ -} - - -/** - * @brief Pack a gps_date_time 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 year Year reported by Gps - * @param month Month reported by Gps - * @param day Day reported by Gps - * @param hour Hour reported by Gps - * @param min Min reported by Gps - * @param sec Sec reported by Gps - * @param visSat Visible sattelites reported by Gps - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps_date_time_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec, uint8_t visSat) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[7]; - _mav_put_uint8_t(buf, 0, year); - _mav_put_uint8_t(buf, 1, month); - _mav_put_uint8_t(buf, 2, day); - _mav_put_uint8_t(buf, 3, hour); - _mav_put_uint8_t(buf, 4, min); - _mav_put_uint8_t(buf, 5, sec); - _mav_put_uint8_t(buf, 6, visSat); - - memcpy(_MAV_PAYLOAD(msg), buf, 7); -#else - mavlink_gps_date_time_t packet; - packet.year = year; - packet.month = month; - packet.day = day; - packet.hour = hour; - packet.min = min; - packet.sec = sec; - packet.visSat = visSat; - - memcpy(_MAV_PAYLOAD(msg), &packet, 7); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; - return mavlink_finalize_message(msg, system_id, component_id, 7, 16); -} - -/** - * @brief Pack a gps_date_time 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param year Year reported by Gps - * @param month Month reported by Gps - * @param day Day reported by Gps - * @param hour Hour reported by Gps - * @param min Min reported by Gps - * @param sec Sec reported by Gps - * @param visSat Visible sattelites reported by Gps - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps_date_time_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t year,uint8_t month,uint8_t day,uint8_t hour,uint8_t min,uint8_t sec,uint8_t visSat) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[7]; - _mav_put_uint8_t(buf, 0, year); - _mav_put_uint8_t(buf, 1, month); - _mav_put_uint8_t(buf, 2, day); - _mav_put_uint8_t(buf, 3, hour); - _mav_put_uint8_t(buf, 4, min); - _mav_put_uint8_t(buf, 5, sec); - _mav_put_uint8_t(buf, 6, visSat); - - memcpy(_MAV_PAYLOAD(msg), buf, 7); -#else - mavlink_gps_date_time_t packet; - packet.year = year; - packet.month = month; - packet.day = day; - packet.hour = hour; - packet.min = min; - packet.sec = sec; - packet.visSat = visSat; - - memcpy(_MAV_PAYLOAD(msg), &packet, 7); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 7, 16); -} - -/** - * @brief Encode a gps_date_time struct into 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 gps_date_time C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_gps_date_time_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_date_time_t* gps_date_time) -{ - return mavlink_msg_gps_date_time_pack(system_id, component_id, msg, gps_date_time->year, gps_date_time->month, gps_date_time->day, gps_date_time->hour, gps_date_time->min, gps_date_time->sec, gps_date_time->visSat); -} - -/** - * @brief Send a gps_date_time message - * @param chan MAVLink channel to send the message - * - * @param year Year reported by Gps - * @param month Month reported by Gps - * @param day Day reported by Gps - * @param hour Hour reported by Gps - * @param min Min reported by Gps - * @param sec Sec reported by Gps - * @param visSat Visible sattelites reported by Gps - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_gps_date_time_send(mavlink_channel_t chan, uint8_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec, uint8_t visSat) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[7]; - _mav_put_uint8_t(buf, 0, year); - _mav_put_uint8_t(buf, 1, month); - _mav_put_uint8_t(buf, 2, day); - _mav_put_uint8_t(buf, 3, hour); - _mav_put_uint8_t(buf, 4, min); - _mav_put_uint8_t(buf, 5, sec); - _mav_put_uint8_t(buf, 6, visSat); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_DATE_TIME, buf, 7, 16); -#else - mavlink_gps_date_time_t packet; - packet.year = year; - packet.month = month; - packet.day = day; - packet.hour = hour; - packet.min = min; - packet.sec = sec; - packet.visSat = visSat; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_DATE_TIME, (const char *)&packet, 7, 16); -#endif -} - -#endif - -// MESSAGE GPS_DATE_TIME UNPACKING - - -/** - * @brief Get field year from gps_date_time message - * - * @return Year reported by Gps - */ -static inline uint8_t mavlink_msg_gps_date_time_get_year(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field month from gps_date_time message - * - * @return Month reported by Gps - */ -static inline uint8_t mavlink_msg_gps_date_time_get_month(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field day from gps_date_time message - * - * @return Day reported by Gps - */ -static inline uint8_t mavlink_msg_gps_date_time_get_day(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field hour from gps_date_time message - * - * @return Hour reported by Gps - */ -static inline uint8_t mavlink_msg_gps_date_time_get_hour(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field min from gps_date_time message - * - * @return Min reported by Gps - */ -static inline uint8_t mavlink_msg_gps_date_time_get_min(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field sec from gps_date_time message - * - * @return Sec reported by Gps - */ -static inline uint8_t mavlink_msg_gps_date_time_get_sec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field visSat from gps_date_time message - * - * @return Visible sattelites reported by Gps - */ -static inline uint8_t mavlink_msg_gps_date_time_get_visSat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @brief Decode a gps_date_time message into a struct - * - * @param msg The message to decode - * @param gps_date_time C-struct to decode the message contents into - */ -static inline void mavlink_msg_gps_date_time_decode(const mavlink_message_t* msg, mavlink_gps_date_time_t* gps_date_time) -{ -#if MAVLINK_NEED_BYTE_SWAP - gps_date_time->year = mavlink_msg_gps_date_time_get_year(msg); - gps_date_time->month = mavlink_msg_gps_date_time_get_month(msg); - gps_date_time->day = mavlink_msg_gps_date_time_get_day(msg); - gps_date_time->hour = mavlink_msg_gps_date_time_get_hour(msg); - gps_date_time->min = mavlink_msg_gps_date_time_get_min(msg); - gps_date_time->sec = mavlink_msg_gps_date_time_get_sec(msg); - gps_date_time->visSat = mavlink_msg_gps_date_time_get_visSat(msg); -#else - memcpy(gps_date_time, _MAV_PAYLOAD(msg), 7); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_mid_lvl_cmds.h b/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_mid_lvl_cmds.h deleted file mode 100644 index 59e4012b12..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_mid_lvl_cmds.h +++ /dev/null @@ -1,210 +0,0 @@ -// MESSAGE MID_LVL_CMDS PACKING - -#define MAVLINK_MSG_ID_MID_LVL_CMDS 180 - -typedef struct __mavlink_mid_lvl_cmds_t -{ - float hCommand; ///< Commanded Airspeed - float uCommand; ///< Log value 2 - float rCommand; ///< Log value 3 - uint8_t target; ///< The system setting the commands -} mavlink_mid_lvl_cmds_t; - -#define MAVLINK_MSG_ID_MID_LVL_CMDS_LEN 13 -#define MAVLINK_MSG_ID_180_LEN 13 - - - -#define MAVLINK_MESSAGE_INFO_MID_LVL_CMDS { \ - "MID_LVL_CMDS", \ - 4, \ - { { "hCommand", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_mid_lvl_cmds_t, hCommand) }, \ - { "uCommand", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mid_lvl_cmds_t, uCommand) }, \ - { "rCommand", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mid_lvl_cmds_t, rCommand) }, \ - { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_mid_lvl_cmds_t, target) }, \ - } \ -} - - -/** - * @brief Pack a mid_lvl_cmds 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 The system setting the commands - * @param hCommand Commanded Airspeed - * @param uCommand Log value 2 - * @param rCommand Log value 3 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mid_lvl_cmds_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target, float hCommand, float uCommand, float rCommand) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[13]; - _mav_put_float(buf, 0, hCommand); - _mav_put_float(buf, 4, uCommand); - _mav_put_float(buf, 8, rCommand); - _mav_put_uint8_t(buf, 12, target); - - memcpy(_MAV_PAYLOAD(msg), buf, 13); -#else - mavlink_mid_lvl_cmds_t packet; - packet.hCommand = hCommand; - packet.uCommand = uCommand; - packet.rCommand = rCommand; - packet.target = target; - - memcpy(_MAV_PAYLOAD(msg), &packet, 13); -#endif - - msg->msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; - return mavlink_finalize_message(msg, system_id, component_id, 13, 146); -} - -/** - * @brief Pack a mid_lvl_cmds 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param target The system setting the commands - * @param hCommand Commanded Airspeed - * @param uCommand Log value 2 - * @param rCommand Log value 3 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mid_lvl_cmds_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target,float hCommand,float uCommand,float rCommand) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[13]; - _mav_put_float(buf, 0, hCommand); - _mav_put_float(buf, 4, uCommand); - _mav_put_float(buf, 8, rCommand); - _mav_put_uint8_t(buf, 12, target); - - memcpy(_MAV_PAYLOAD(msg), buf, 13); -#else - mavlink_mid_lvl_cmds_t packet; - packet.hCommand = hCommand; - packet.uCommand = uCommand; - packet.rCommand = rCommand; - packet.target = target; - - memcpy(_MAV_PAYLOAD(msg), &packet, 13); -#endif - - msg->msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 13, 146); -} - -/** - * @brief Encode a mid_lvl_cmds struct into 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 mid_lvl_cmds C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mid_lvl_cmds_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mid_lvl_cmds_t* mid_lvl_cmds) -{ - return mavlink_msg_mid_lvl_cmds_pack(system_id, component_id, msg, mid_lvl_cmds->target, mid_lvl_cmds->hCommand, mid_lvl_cmds->uCommand, mid_lvl_cmds->rCommand); -} - -/** - * @brief Send a mid_lvl_cmds message - * @param chan MAVLink channel to send the message - * - * @param target The system setting the commands - * @param hCommand Commanded Airspeed - * @param uCommand Log value 2 - * @param rCommand Log value 3 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_mid_lvl_cmds_send(mavlink_channel_t chan, uint8_t target, float hCommand, float uCommand, float rCommand) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[13]; - _mav_put_float(buf, 0, hCommand); - _mav_put_float(buf, 4, uCommand); - _mav_put_float(buf, 8, rCommand); - _mav_put_uint8_t(buf, 12, target); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MID_LVL_CMDS, buf, 13, 146); -#else - mavlink_mid_lvl_cmds_t packet; - packet.hCommand = hCommand; - packet.uCommand = uCommand; - packet.rCommand = rCommand; - packet.target = target; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MID_LVL_CMDS, (const char *)&packet, 13, 146); -#endif -} - -#endif - -// MESSAGE MID_LVL_CMDS UNPACKING - - -/** - * @brief Get field target from mid_lvl_cmds message - * - * @return The system setting the commands - */ -static inline uint8_t mavlink_msg_mid_lvl_cmds_get_target(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 12); -} - -/** - * @brief Get field hCommand from mid_lvl_cmds message - * - * @return Commanded Airspeed - */ -static inline float mavlink_msg_mid_lvl_cmds_get_hCommand(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field uCommand from mid_lvl_cmds message - * - * @return Log value 2 - */ -static inline float mavlink_msg_mid_lvl_cmds_get_uCommand(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field rCommand from mid_lvl_cmds message - * - * @return Log value 3 - */ -static inline float mavlink_msg_mid_lvl_cmds_get_rCommand(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Decode a mid_lvl_cmds message into a struct - * - * @param msg The message to decode - * @param mid_lvl_cmds C-struct to decode the message contents into - */ -static inline void mavlink_msg_mid_lvl_cmds_decode(const mavlink_message_t* msg, mavlink_mid_lvl_cmds_t* mid_lvl_cmds) -{ -#if MAVLINK_NEED_BYTE_SWAP - mid_lvl_cmds->hCommand = mavlink_msg_mid_lvl_cmds_get_hCommand(msg); - mid_lvl_cmds->uCommand = mavlink_msg_mid_lvl_cmds_get_uCommand(msg); - mid_lvl_cmds->rCommand = mavlink_msg_mid_lvl_cmds_get_rCommand(msg); - mid_lvl_cmds->target = mavlink_msg_mid_lvl_cmds_get_target(msg); -#else - memcpy(mid_lvl_cmds, _MAV_PAYLOAD(msg), 13); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_sensor_bias.h b/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_sensor_bias.h deleted file mode 100644 index 3bf6803a60..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_sensor_bias.h +++ /dev/null @@ -1,254 +0,0 @@ -// MESSAGE SENSOR_BIAS PACKING - -#define MAVLINK_MSG_ID_SENSOR_BIAS 172 - -typedef struct __mavlink_sensor_bias_t -{ - float axBias; ///< Accelerometer X bias (m/s) - float ayBias; ///< Accelerometer Y bias (m/s) - float azBias; ///< Accelerometer Z bias (m/s) - float gxBias; ///< Gyro X bias (rad/s) - float gyBias; ///< Gyro Y bias (rad/s) - float gzBias; ///< Gyro Z bias (rad/s) -} mavlink_sensor_bias_t; - -#define MAVLINK_MSG_ID_SENSOR_BIAS_LEN 24 -#define MAVLINK_MSG_ID_172_LEN 24 - - - -#define MAVLINK_MESSAGE_INFO_SENSOR_BIAS { \ - "SENSOR_BIAS", \ - 6, \ - { { "axBias", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_sensor_bias_t, axBias) }, \ - { "ayBias", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_sensor_bias_t, ayBias) }, \ - { "azBias", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_sensor_bias_t, azBias) }, \ - { "gxBias", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_sensor_bias_t, gxBias) }, \ - { "gyBias", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_sensor_bias_t, gyBias) }, \ - { "gzBias", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_sensor_bias_t, gzBias) }, \ - } \ -} - - -/** - * @brief Pack a sensor_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 axBias Accelerometer X bias (m/s) - * @param ayBias Accelerometer Y bias (m/s) - * @param azBias Accelerometer Z bias (m/s) - * @param gxBias Gyro X bias (rad/s) - * @param gyBias Gyro Y bias (rad/s) - * @param gzBias Gyro Z bias (rad/s) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_sensor_bias_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float axBias, float ayBias, float azBias, float gxBias, float gyBias, float gzBias) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[24]; - _mav_put_float(buf, 0, axBias); - _mav_put_float(buf, 4, ayBias); - _mav_put_float(buf, 8, azBias); - _mav_put_float(buf, 12, gxBias); - _mav_put_float(buf, 16, gyBias); - _mav_put_float(buf, 20, gzBias); - - memcpy(_MAV_PAYLOAD(msg), buf, 24); -#else - mavlink_sensor_bias_t packet; - packet.axBias = axBias; - packet.ayBias = ayBias; - packet.azBias = azBias; - packet.gxBias = gxBias; - packet.gyBias = gyBias; - packet.gzBias = gzBias; - - memcpy(_MAV_PAYLOAD(msg), &packet, 24); -#endif - - msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS; - return mavlink_finalize_message(msg, system_id, component_id, 24, 168); -} - -/** - * @brief Pack a sensor_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 was sent over - * @param msg The MAVLink message to compress the data into - * @param axBias Accelerometer X bias (m/s) - * @param ayBias Accelerometer Y bias (m/s) - * @param azBias Accelerometer Z bias (m/s) - * @param gxBias Gyro X bias (rad/s) - * @param gyBias Gyro Y bias (rad/s) - * @param gzBias Gyro Z bias (rad/s) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_sensor_bias_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float axBias,float ayBias,float azBias,float gxBias,float gyBias,float gzBias) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[24]; - _mav_put_float(buf, 0, axBias); - _mav_put_float(buf, 4, ayBias); - _mav_put_float(buf, 8, azBias); - _mav_put_float(buf, 12, gxBias); - _mav_put_float(buf, 16, gyBias); - _mav_put_float(buf, 20, gzBias); - - memcpy(_MAV_PAYLOAD(msg), buf, 24); -#else - mavlink_sensor_bias_t packet; - packet.axBias = axBias; - packet.ayBias = ayBias; - packet.azBias = azBias; - packet.gxBias = gxBias; - packet.gyBias = gyBias; - packet.gzBias = gzBias; - - memcpy(_MAV_PAYLOAD(msg), &packet, 24); -#endif - - msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 24, 168); -} - -/** - * @brief Encode a sensor_bias struct into 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 sensor_bias C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_sensor_bias_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sensor_bias_t* sensor_bias) -{ - return mavlink_msg_sensor_bias_pack(system_id, component_id, msg, sensor_bias->axBias, sensor_bias->ayBias, sensor_bias->azBias, sensor_bias->gxBias, sensor_bias->gyBias, sensor_bias->gzBias); -} - -/** - * @brief Send a sensor_bias message - * @param chan MAVLink channel to send the message - * - * @param axBias Accelerometer X bias (m/s) - * @param ayBias Accelerometer Y bias (m/s) - * @param azBias Accelerometer Z bias (m/s) - * @param gxBias Gyro X bias (rad/s) - * @param gyBias Gyro Y bias (rad/s) - * @param gzBias Gyro Z bias (rad/s) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_sensor_bias_send(mavlink_channel_t chan, float axBias, float ayBias, float azBias, float gxBias, float gyBias, float gzBias) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[24]; - _mav_put_float(buf, 0, axBias); - _mav_put_float(buf, 4, ayBias); - _mav_put_float(buf, 8, azBias); - _mav_put_float(buf, 12, gxBias); - _mav_put_float(buf, 16, gyBias); - _mav_put_float(buf, 20, gzBias); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_BIAS, buf, 24, 168); -#else - mavlink_sensor_bias_t packet; - packet.axBias = axBias; - packet.ayBias = ayBias; - packet.azBias = azBias; - packet.gxBias = gxBias; - packet.gyBias = gyBias; - packet.gzBias = gzBias; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_BIAS, (const char *)&packet, 24, 168); -#endif -} - -#endif - -// MESSAGE SENSOR_BIAS UNPACKING - - -/** - * @brief Get field axBias from sensor_bias message - * - * @return Accelerometer X bias (m/s) - */ -static inline float mavlink_msg_sensor_bias_get_axBias(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field ayBias from sensor_bias message - * - * @return Accelerometer Y bias (m/s) - */ -static inline float mavlink_msg_sensor_bias_get_ayBias(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field azBias from sensor_bias message - * - * @return Accelerometer Z bias (m/s) - */ -static inline float mavlink_msg_sensor_bias_get_azBias(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field gxBias from sensor_bias message - * - * @return Gyro X bias (rad/s) - */ -static inline float mavlink_msg_sensor_bias_get_gxBias(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field gyBias from sensor_bias message - * - * @return Gyro Y bias (rad/s) - */ -static inline float mavlink_msg_sensor_bias_get_gyBias(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field gzBias from sensor_bias message - * - * @return Gyro Z bias (rad/s) - */ -static inline float mavlink_msg_sensor_bias_get_gzBias(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Decode a sensor_bias message into a struct - * - * @param msg The message to decode - * @param sensor_bias C-struct to decode the message contents into - */ -static inline void mavlink_msg_sensor_bias_decode(const mavlink_message_t* msg, mavlink_sensor_bias_t* sensor_bias) -{ -#if MAVLINK_NEED_BYTE_SWAP - sensor_bias->axBias = mavlink_msg_sensor_bias_get_axBias(msg); - sensor_bias->ayBias = mavlink_msg_sensor_bias_get_ayBias(msg); - sensor_bias->azBias = mavlink_msg_sensor_bias_get_azBias(msg); - sensor_bias->gxBias = mavlink_msg_sensor_bias_get_gxBias(msg); - sensor_bias->gyBias = mavlink_msg_sensor_bias_get_gyBias(msg); - sensor_bias->gzBias = mavlink_msg_sensor_bias_get_gzBias(msg); -#else - memcpy(sensor_bias, _MAV_PAYLOAD(msg), 24); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_slugs_action.h b/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_slugs_action.h deleted file mode 100644 index 2382a17c47..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_slugs_action.h +++ /dev/null @@ -1,188 +0,0 @@ -// MESSAGE SLUGS_ACTION PACKING - -#define MAVLINK_MSG_ID_SLUGS_ACTION 183 - -typedef struct __mavlink_slugs_action_t -{ - uint16_t actionVal; ///< Value associated with the action - uint8_t target; ///< The system reporting the action - uint8_t actionId; ///< Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names -} mavlink_slugs_action_t; - -#define MAVLINK_MSG_ID_SLUGS_ACTION_LEN 4 -#define MAVLINK_MSG_ID_183_LEN 4 - - - -#define MAVLINK_MESSAGE_INFO_SLUGS_ACTION { \ - "SLUGS_ACTION", \ - 3, \ - { { "actionVal", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_slugs_action_t, actionVal) }, \ - { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_slugs_action_t, target) }, \ - { "actionId", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_slugs_action_t, actionId) }, \ - } \ -} - - -/** - * @brief Pack a slugs_action 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 The system reporting the action - * @param actionId Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - * @param actionVal Value associated with the action - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_slugs_action_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target, uint8_t actionId, uint16_t actionVal) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint16_t(buf, 0, actionVal); - _mav_put_uint8_t(buf, 2, target); - _mav_put_uint8_t(buf, 3, actionId); - - memcpy(_MAV_PAYLOAD(msg), buf, 4); -#else - mavlink_slugs_action_t packet; - packet.actionVal = actionVal; - packet.target = target; - packet.actionId = actionId; - - memcpy(_MAV_PAYLOAD(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_SLUGS_ACTION; - return mavlink_finalize_message(msg, system_id, component_id, 4, 65); -} - -/** - * @brief Pack a slugs_action 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param target The system reporting the action - * @param actionId Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - * @param actionVal Value associated with the action - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_slugs_action_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target,uint8_t actionId,uint16_t actionVal) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint16_t(buf, 0, actionVal); - _mav_put_uint8_t(buf, 2, target); - _mav_put_uint8_t(buf, 3, actionId); - - memcpy(_MAV_PAYLOAD(msg), buf, 4); -#else - mavlink_slugs_action_t packet; - packet.actionVal = actionVal; - packet.target = target; - packet.actionId = actionId; - - memcpy(_MAV_PAYLOAD(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_SLUGS_ACTION; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 65); -} - -/** - * @brief Encode a slugs_action struct into 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 slugs_action C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_slugs_action_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_slugs_action_t* slugs_action) -{ - return mavlink_msg_slugs_action_pack(system_id, component_id, msg, slugs_action->target, slugs_action->actionId, slugs_action->actionVal); -} - -/** - * @brief Send a slugs_action message - * @param chan MAVLink channel to send the message - * - * @param target The system reporting the action - * @param actionId Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - * @param actionVal Value associated with the action - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_slugs_action_send(mavlink_channel_t chan, uint8_t target, uint8_t actionId, uint16_t actionVal) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint16_t(buf, 0, actionVal); - _mav_put_uint8_t(buf, 2, target); - _mav_put_uint8_t(buf, 3, actionId); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SLUGS_ACTION, buf, 4, 65); -#else - mavlink_slugs_action_t packet; - packet.actionVal = actionVal; - packet.target = target; - packet.actionId = actionId; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SLUGS_ACTION, (const char *)&packet, 4, 65); -#endif -} - -#endif - -// MESSAGE SLUGS_ACTION UNPACKING - - -/** - * @brief Get field target from slugs_action message - * - * @return The system reporting the action - */ -static inline uint8_t mavlink_msg_slugs_action_get_target(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field actionId from slugs_action message - * - * @return Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - */ -static inline uint8_t mavlink_msg_slugs_action_get_actionId(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field actionVal from slugs_action message - * - * @return Value associated with the action - */ -static inline uint16_t mavlink_msg_slugs_action_get_actionVal(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Decode a slugs_action message into a struct - * - * @param msg The message to decode - * @param slugs_action C-struct to decode the message contents into - */ -static inline void mavlink_msg_slugs_action_decode(const mavlink_message_t* msg, mavlink_slugs_action_t* slugs_action) -{ -#if MAVLINK_NEED_BYTE_SWAP - slugs_action->actionVal = mavlink_msg_slugs_action_get_actionVal(msg); - slugs_action->target = mavlink_msg_slugs_action_get_target(msg); - slugs_action->actionId = mavlink_msg_slugs_action_get_actionId(msg); -#else - memcpy(slugs_action, _MAV_PAYLOAD(msg), 4); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_slugs_navigation.h b/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_slugs_navigation.h deleted file mode 100644 index a0253a8483..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/slugs/mavlink_msg_slugs_navigation.h +++ /dev/null @@ -1,320 +0,0 @@ -// MESSAGE SLUGS_NAVIGATION PACKING - -#define MAVLINK_MSG_ID_SLUGS_NAVIGATION 176 - -typedef struct __mavlink_slugs_navigation_t -{ - float u_m; ///< Measured Airspeed prior to the Nav Filter - float phi_c; ///< Commanded Roll - float theta_c; ///< Commanded Pitch - float psiDot_c; ///< Commanded Turn rate - float ay_body; ///< Y component of the body acceleration - float totalDist; ///< Total Distance to Run on this leg of Navigation - float dist2Go; ///< Remaining distance to Run on this leg of Navigation - uint8_t fromWP; ///< Origin WP - uint8_t toWP; ///< Destination WP -} mavlink_slugs_navigation_t; - -#define MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN 30 -#define MAVLINK_MSG_ID_176_LEN 30 - - - -#define MAVLINK_MESSAGE_INFO_SLUGS_NAVIGATION { \ - "SLUGS_NAVIGATION", \ - 9, \ - { { "u_m", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_slugs_navigation_t, u_m) }, \ - { "phi_c", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_slugs_navigation_t, phi_c) }, \ - { "theta_c", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_slugs_navigation_t, theta_c) }, \ - { "psiDot_c", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_slugs_navigation_t, psiDot_c) }, \ - { "ay_body", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_slugs_navigation_t, ay_body) }, \ - { "totalDist", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_slugs_navigation_t, totalDist) }, \ - { "dist2Go", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_slugs_navigation_t, dist2Go) }, \ - { "fromWP", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_slugs_navigation_t, fromWP) }, \ - { "toWP", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_slugs_navigation_t, toWP) }, \ - } \ -} - - -/** - * @brief Pack a slugs_navigation 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 u_m Measured Airspeed prior to the Nav Filter - * @param phi_c Commanded Roll - * @param theta_c Commanded Pitch - * @param psiDot_c Commanded Turn rate - * @param ay_body Y component of the body acceleration - * @param totalDist Total Distance to Run on this leg of Navigation - * @param dist2Go Remaining distance to Run on this leg of Navigation - * @param fromWP Origin WP - * @param toWP Destination WP - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_slugs_navigation_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float u_m, float phi_c, float theta_c, float psiDot_c, float ay_body, float totalDist, float dist2Go, uint8_t fromWP, uint8_t toWP) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[30]; - _mav_put_float(buf, 0, u_m); - _mav_put_float(buf, 4, phi_c); - _mav_put_float(buf, 8, theta_c); - _mav_put_float(buf, 12, psiDot_c); - _mav_put_float(buf, 16, ay_body); - _mav_put_float(buf, 20, totalDist); - _mav_put_float(buf, 24, dist2Go); - _mav_put_uint8_t(buf, 28, fromWP); - _mav_put_uint8_t(buf, 29, toWP); - - memcpy(_MAV_PAYLOAD(msg), buf, 30); -#else - mavlink_slugs_navigation_t packet; - packet.u_m = u_m; - packet.phi_c = phi_c; - packet.theta_c = theta_c; - packet.psiDot_c = psiDot_c; - packet.ay_body = ay_body; - packet.totalDist = totalDist; - packet.dist2Go = dist2Go; - packet.fromWP = fromWP; - packet.toWP = toWP; - - memcpy(_MAV_PAYLOAD(msg), &packet, 30); -#endif - - msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; - return mavlink_finalize_message(msg, system_id, component_id, 30, 120); -} - -/** - * @brief Pack a slugs_navigation 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param u_m Measured Airspeed prior to the Nav Filter - * @param phi_c Commanded Roll - * @param theta_c Commanded Pitch - * @param psiDot_c Commanded Turn rate - * @param ay_body Y component of the body acceleration - * @param totalDist Total Distance to Run on this leg of Navigation - * @param dist2Go Remaining distance to Run on this leg of Navigation - * @param fromWP Origin WP - * @param toWP Destination WP - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_slugs_navigation_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float u_m,float phi_c,float theta_c,float psiDot_c,float ay_body,float totalDist,float dist2Go,uint8_t fromWP,uint8_t toWP) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[30]; - _mav_put_float(buf, 0, u_m); - _mav_put_float(buf, 4, phi_c); - _mav_put_float(buf, 8, theta_c); - _mav_put_float(buf, 12, psiDot_c); - _mav_put_float(buf, 16, ay_body); - _mav_put_float(buf, 20, totalDist); - _mav_put_float(buf, 24, dist2Go); - _mav_put_uint8_t(buf, 28, fromWP); - _mav_put_uint8_t(buf, 29, toWP); - - memcpy(_MAV_PAYLOAD(msg), buf, 30); -#else - mavlink_slugs_navigation_t packet; - packet.u_m = u_m; - packet.phi_c = phi_c; - packet.theta_c = theta_c; - packet.psiDot_c = psiDot_c; - packet.ay_body = ay_body; - packet.totalDist = totalDist; - packet.dist2Go = dist2Go; - packet.fromWP = fromWP; - packet.toWP = toWP; - - memcpy(_MAV_PAYLOAD(msg), &packet, 30); -#endif - - msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 30, 120); -} - -/** - * @brief Encode a slugs_navigation struct into 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 slugs_navigation C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_slugs_navigation_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_slugs_navigation_t* slugs_navigation) -{ - return mavlink_msg_slugs_navigation_pack(system_id, component_id, msg, slugs_navigation->u_m, slugs_navigation->phi_c, slugs_navigation->theta_c, slugs_navigation->psiDot_c, slugs_navigation->ay_body, slugs_navigation->totalDist, slugs_navigation->dist2Go, slugs_navigation->fromWP, slugs_navigation->toWP); -} - -/** - * @brief Send a slugs_navigation message - * @param chan MAVLink channel to send the message - * - * @param u_m Measured Airspeed prior to the Nav Filter - * @param phi_c Commanded Roll - * @param theta_c Commanded Pitch - * @param psiDot_c Commanded Turn rate - * @param ay_body Y component of the body acceleration - * @param totalDist Total Distance to Run on this leg of Navigation - * @param dist2Go Remaining distance to Run on this leg of Navigation - * @param fromWP Origin WP - * @param toWP Destination WP - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_slugs_navigation_send(mavlink_channel_t chan, float u_m, float phi_c, float theta_c, float psiDot_c, float ay_body, float totalDist, float dist2Go, uint8_t fromWP, uint8_t toWP) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[30]; - _mav_put_float(buf, 0, u_m); - _mav_put_float(buf, 4, phi_c); - _mav_put_float(buf, 8, theta_c); - _mav_put_float(buf, 12, psiDot_c); - _mav_put_float(buf, 16, ay_body); - _mav_put_float(buf, 20, totalDist); - _mav_put_float(buf, 24, dist2Go); - _mav_put_uint8_t(buf, 28, fromWP); - _mav_put_uint8_t(buf, 29, toWP); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SLUGS_NAVIGATION, buf, 30, 120); -#else - mavlink_slugs_navigation_t packet; - packet.u_m = u_m; - packet.phi_c = phi_c; - packet.theta_c = theta_c; - packet.psiDot_c = psiDot_c; - packet.ay_body = ay_body; - packet.totalDist = totalDist; - packet.dist2Go = dist2Go; - packet.fromWP = fromWP; - packet.toWP = toWP; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SLUGS_NAVIGATION, (const char *)&packet, 30, 120); -#endif -} - -#endif - -// MESSAGE SLUGS_NAVIGATION UNPACKING - - -/** - * @brief Get field u_m from slugs_navigation message - * - * @return Measured Airspeed prior to the Nav Filter - */ -static inline float mavlink_msg_slugs_navigation_get_u_m(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field phi_c from slugs_navigation message - * - * @return Commanded Roll - */ -static inline float mavlink_msg_slugs_navigation_get_phi_c(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field theta_c from slugs_navigation message - * - * @return Commanded Pitch - */ -static inline float mavlink_msg_slugs_navigation_get_theta_c(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field psiDot_c from slugs_navigation message - * - * @return Commanded Turn rate - */ -static inline float mavlink_msg_slugs_navigation_get_psiDot_c(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field ay_body from slugs_navigation message - * - * @return Y component of the body acceleration - */ -static inline float mavlink_msg_slugs_navigation_get_ay_body(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field totalDist from slugs_navigation message - * - * @return Total Distance to Run on this leg of Navigation - */ -static inline float mavlink_msg_slugs_navigation_get_totalDist(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field dist2Go from slugs_navigation message - * - * @return Remaining distance to Run on this leg of Navigation - */ -static inline float mavlink_msg_slugs_navigation_get_dist2Go(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field fromWP from slugs_navigation message - * - * @return Origin WP - */ -static inline uint8_t mavlink_msg_slugs_navigation_get_fromWP(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 28); -} - -/** - * @brief Get field toWP from slugs_navigation message - * - * @return Destination WP - */ -static inline uint8_t mavlink_msg_slugs_navigation_get_toWP(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 29); -} - -/** - * @brief Decode a slugs_navigation message into a struct - * - * @param msg The message to decode - * @param slugs_navigation C-struct to decode the message contents into - */ -static inline void mavlink_msg_slugs_navigation_decode(const mavlink_message_t* msg, mavlink_slugs_navigation_t* slugs_navigation) -{ -#if MAVLINK_NEED_BYTE_SWAP - slugs_navigation->u_m = mavlink_msg_slugs_navigation_get_u_m(msg); - slugs_navigation->phi_c = mavlink_msg_slugs_navigation_get_phi_c(msg); - slugs_navigation->theta_c = mavlink_msg_slugs_navigation_get_theta_c(msg); - slugs_navigation->psiDot_c = mavlink_msg_slugs_navigation_get_psiDot_c(msg); - slugs_navigation->ay_body = mavlink_msg_slugs_navigation_get_ay_body(msg); - slugs_navigation->totalDist = mavlink_msg_slugs_navigation_get_totalDist(msg); - slugs_navigation->dist2Go = mavlink_msg_slugs_navigation_get_dist2Go(msg); - slugs_navigation->fromWP = mavlink_msg_slugs_navigation_get_fromWP(msg); - slugs_navigation->toWP = mavlink_msg_slugs_navigation_get_toWP(msg); -#else - memcpy(slugs_navigation, _MAV_PAYLOAD(msg), 30); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/slugs/slugs.h b/libraries/GCS_MAVLink/include_v1.0/slugs/slugs.h deleted file mode 100644 index 31cde68b0c..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/slugs/slugs.h +++ /dev/null @@ -1,62 +0,0 @@ -/** @file - * @brief MAVLink comm protocol generated from slugs.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef SLUGS_H -#define SLUGS_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, 0, 28, 22, 22, 21, 0, 36, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 0, 0, 26, 0, 36, 0, 6, 4, 0, 21, 18, 0, 0, 0, 20, 20, 32, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 18, 32, 32, 20, 32, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 10, 24, 18, 0, 0, 30, 24, 0, 7, 13, 3, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 3} -#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, 0, 104, 244, 237, 222, 0, 158, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 0, 0, 183, 0, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 160, 168, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 19, 102, 158, 208, 56, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 75, 232, 168, 2, 0, 0, 120, 167, 0, 16, 146, 104, 0, 65, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 247} -#endif - -#ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {NULL}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SET_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, 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, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, {NULL}, 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, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, {NULL}, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {NULL}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {NULL}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_SHORT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, 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, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_CPU_LOAD, MAVLINK_MESSAGE_INFO_AIR_DATA, MAVLINK_MESSAGE_INFO_SENSOR_BIAS, MAVLINK_MESSAGE_INFO_DIAGNOSTIC, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SLUGS_NAVIGATION, MAVLINK_MESSAGE_INFO_DATA_LOG, {NULL}, MAVLINK_MESSAGE_INFO_GPS_DATE_TIME, MAVLINK_MESSAGE_INFO_MID_LVL_CMDS, MAVLINK_MESSAGE_INFO_CTRL_SRFC_PT, {NULL}, MAVLINK_MESSAGE_INFO_SLUGS_ACTION, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, MAVLINK_MESSAGE_INFO_EXTENDED_MESSAGE} -#endif - -#include "../protocol.h" - -#define MAVLINK_ENABLED_SLUGS - -#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 - -// ENUM DEFINITIONS - - - -// MESSAGE DEFINITIONS -#include "./mavlink_msg_cpu_load.h" -#include "./mavlink_msg_air_data.h" -#include "./mavlink_msg_sensor_bias.h" -#include "./mavlink_msg_diagnostic.h" -#include "./mavlink_msg_slugs_navigation.h" -#include "./mavlink_msg_data_log.h" -#include "./mavlink_msg_gps_date_time.h" -#include "./mavlink_msg_mid_lvl_cmds.h" -#include "./mavlink_msg_ctrl_srfc_pt.h" -#include "./mavlink_msg_slugs_action.h" - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // SLUGS_H diff --git a/libraries/GCS_MAVLink/include_v1.0/slugs/testsuite.h b/libraries/GCS_MAVLink/include_v1.0/slugs/testsuite.h deleted file mode 100644 index 4593235a4a..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/slugs/testsuite.h +++ /dev/null @@ -1,552 +0,0 @@ -/** @file - * @brief MAVLink comm protocol testsuite generated from slugs.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef SLUGS_TESTSUITE_H -#define SLUGS_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_slugs(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_slugs(system_id, component_id, last_msg); -} -#endif - -#include "../common/testsuite.h" - - -static void mavlink_test_cpu_load(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_cpu_load_t packet_in = { - 17235, - 139, - 206, - }; - mavlink_cpu_load_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.batVolt = packet_in.batVolt; - packet1.sensLoad = packet_in.sensLoad; - packet1.ctrlLoad = packet_in.ctrlLoad; - - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_cpu_load_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_cpu_load_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_cpu_load_pack(system_id, component_id, &msg , packet1.sensLoad , packet1.ctrlLoad , packet1.batVolt ); - mavlink_msg_cpu_load_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_cpu_load_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.sensLoad , packet1.ctrlLoad , packet1.batVolt ); - mavlink_msg_cpu_load_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_TEST_TYPES; - return mavlink_finalize_message(msg, system_id, component_id, 179, 103); -} - -/** - * @brief Pack a test_types 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param c char - * @param s string - * @param u8 uint8_t - * @param u16 uint16_t - * @param u32 uint32_t - * @param u64 uint64_t - * @param s8 int8_t - * @param s16 int16_t - * @param s32 int32_t - * @param s64 int64_t - * @param f float - * @param d double - * @param u8_array uint8_t_array - * @param u16_array uint16_t_array - * @param u32_array uint32_t_array - * @param u64_array uint64_t_array - * @param s8_array int8_t_array - * @param s16_array int16_t_array - * @param s32_array int32_t_array - * @param s64_array int64_t_array - * @param f_array float_array - * @param d_array double_array - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_test_types_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - char c,const char *s,uint8_t u8,uint16_t u16,uint32_t u32,uint64_t u64,int8_t s8,int16_t s16,int32_t s32,int64_t s64,float f,double d,const uint8_t *u8_array,const uint16_t *u16_array,const uint32_t *u32_array,const uint64_t *u64_array,const int8_t *s8_array,const int16_t *s16_array,const int32_t *s32_array,const int64_t *s64_array,const float *f_array,const double *d_array) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[179]; - _mav_put_uint64_t(buf, 0, u64); - _mav_put_int64_t(buf, 8, s64); - _mav_put_double(buf, 16, d); - _mav_put_uint32_t(buf, 96, u32); - _mav_put_int32_t(buf, 100, s32); - _mav_put_float(buf, 104, f); - _mav_put_uint16_t(buf, 144, u16); - _mav_put_int16_t(buf, 146, s16); - _mav_put_char(buf, 160, c); - _mav_put_uint8_t(buf, 171, u8); - _mav_put_int8_t(buf, 172, s8); - _mav_put_uint64_t_array(buf, 24, u64_array, 3); - _mav_put_int64_t_array(buf, 48, s64_array, 3); - _mav_put_double_array(buf, 72, d_array, 3); - _mav_put_uint32_t_array(buf, 108, u32_array, 3); - _mav_put_int32_t_array(buf, 120, s32_array, 3); - _mav_put_float_array(buf, 132, f_array, 3); - _mav_put_uint16_t_array(buf, 148, u16_array, 3); - _mav_put_int16_t_array(buf, 154, s16_array, 3); - _mav_put_char_array(buf, 161, s, 10); - _mav_put_uint8_t_array(buf, 173, u8_array, 3); - _mav_put_int8_t_array(buf, 176, s8_array, 3); - memcpy(_MAV_PAYLOAD(msg), buf, 179); -#else - mavlink_test_types_t packet; - packet.u64 = u64; - packet.s64 = s64; - packet.d = d; - packet.u32 = u32; - packet.s32 = s32; - packet.f = f; - packet.u16 = u16; - packet.s16 = s16; - packet.c = c; - packet.u8 = u8; - packet.s8 = s8; - memcpy(packet.u64_array, u64_array, sizeof(uint64_t)*3); - memcpy(packet.s64_array, s64_array, sizeof(int64_t)*3); - memcpy(packet.d_array, d_array, sizeof(double)*3); - memcpy(packet.u32_array, u32_array, sizeof(uint32_t)*3); - memcpy(packet.s32_array, s32_array, sizeof(int32_t)*3); - memcpy(packet.f_array, f_array, sizeof(float)*3); - memcpy(packet.u16_array, u16_array, sizeof(uint16_t)*3); - memcpy(packet.s16_array, s16_array, sizeof(int16_t)*3); - memcpy(packet.s, s, sizeof(char)*10); - memcpy(packet.u8_array, u8_array, sizeof(uint8_t)*3); - memcpy(packet.s8_array, s8_array, sizeof(int8_t)*3); - memcpy(_MAV_PAYLOAD(msg), &packet, 179); -#endif - - msg->msgid = MAVLINK_MSG_ID_TEST_TYPES; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 179, 103); -} - -/** - * @brief Encode a test_types struct into 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 test_types C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_test_types_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_test_types_t* test_types) -{ - return mavlink_msg_test_types_pack(system_id, component_id, msg, test_types->c, test_types->s, test_types->u8, test_types->u16, test_types->u32, test_types->u64, test_types->s8, test_types->s16, test_types->s32, test_types->s64, test_types->f, test_types->d, test_types->u8_array, test_types->u16_array, test_types->u32_array, test_types->u64_array, test_types->s8_array, test_types->s16_array, test_types->s32_array, test_types->s64_array, test_types->f_array, test_types->d_array); -} - -/** - * @brief Send a test_types message - * @param chan MAVLink channel to send the message - * - * @param c char - * @param s string - * @param u8 uint8_t - * @param u16 uint16_t - * @param u32 uint32_t - * @param u64 uint64_t - * @param s8 int8_t - * @param s16 int16_t - * @param s32 int32_t - * @param s64 int64_t - * @param f float - * @param d double - * @param u8_array uint8_t_array - * @param u16_array uint16_t_array - * @param u32_array uint32_t_array - * @param u64_array uint64_t_array - * @param s8_array int8_t_array - * @param s16_array int16_t_array - * @param s32_array int32_t_array - * @param s64_array int64_t_array - * @param f_array float_array - * @param d_array double_array - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_test_types_send(mavlink_channel_t chan, char c, const char *s, uint8_t u8, uint16_t u16, uint32_t u32, uint64_t u64, int8_t s8, int16_t s16, int32_t s32, int64_t s64, float f, double d, const uint8_t *u8_array, const uint16_t *u16_array, const uint32_t *u32_array, const uint64_t *u64_array, const int8_t *s8_array, const int16_t *s16_array, const int32_t *s32_array, const int64_t *s64_array, const float *f_array, const double *d_array) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[179]; - _mav_put_uint64_t(buf, 0, u64); - _mav_put_int64_t(buf, 8, s64); - _mav_put_double(buf, 16, d); - _mav_put_uint32_t(buf, 96, u32); - _mav_put_int32_t(buf, 100, s32); - _mav_put_float(buf, 104, f); - _mav_put_uint16_t(buf, 144, u16); - _mav_put_int16_t(buf, 146, s16); - _mav_put_char(buf, 160, c); - _mav_put_uint8_t(buf, 171, u8); - _mav_put_int8_t(buf, 172, s8); - _mav_put_uint64_t_array(buf, 24, u64_array, 3); - _mav_put_int64_t_array(buf, 48, s64_array, 3); - _mav_put_double_array(buf, 72, d_array, 3); - _mav_put_uint32_t_array(buf, 108, u32_array, 3); - _mav_put_int32_t_array(buf, 120, s32_array, 3); - _mav_put_float_array(buf, 132, f_array, 3); - _mav_put_uint16_t_array(buf, 148, u16_array, 3); - _mav_put_int16_t_array(buf, 154, s16_array, 3); - _mav_put_char_array(buf, 161, s, 10); - _mav_put_uint8_t_array(buf, 173, u8_array, 3); - _mav_put_int8_t_array(buf, 176, s8_array, 3); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TEST_TYPES, buf, 179, 103); -#else - mavlink_test_types_t packet; - packet.u64 = u64; - packet.s64 = s64; - packet.d = d; - packet.u32 = u32; - packet.s32 = s32; - packet.f = f; - packet.u16 = u16; - packet.s16 = s16; - packet.c = c; - packet.u8 = u8; - packet.s8 = s8; - memcpy(packet.u64_array, u64_array, sizeof(uint64_t)*3); - memcpy(packet.s64_array, s64_array, sizeof(int64_t)*3); - memcpy(packet.d_array, d_array, sizeof(double)*3); - memcpy(packet.u32_array, u32_array, sizeof(uint32_t)*3); - memcpy(packet.s32_array, s32_array, sizeof(int32_t)*3); - memcpy(packet.f_array, f_array, sizeof(float)*3); - memcpy(packet.u16_array, u16_array, sizeof(uint16_t)*3); - memcpy(packet.s16_array, s16_array, sizeof(int16_t)*3); - memcpy(packet.s, s, sizeof(char)*10); - memcpy(packet.u8_array, u8_array, sizeof(uint8_t)*3); - memcpy(packet.s8_array, s8_array, sizeof(int8_t)*3); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TEST_TYPES, (const char *)&packet, 179, 103); -#endif -} - -#endif - -// MESSAGE TEST_TYPES UNPACKING - - -/** - * @brief Get field c from test_types message - * - * @return char - */ -static inline char mavlink_msg_test_types_get_c(const mavlink_message_t* msg) -{ - return _MAV_RETURN_char(msg, 160); -} - -/** - * @brief Get field s from test_types message - * - * @return string - */ -static inline uint16_t mavlink_msg_test_types_get_s(const mavlink_message_t* msg, char *s) -{ - return _MAV_RETURN_char_array(msg, s, 10, 161); -} - -/** - * @brief Get field u8 from test_types message - * - * @return uint8_t - */ -static inline uint8_t mavlink_msg_test_types_get_u8(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 171); -} - -/** - * @brief Get field u16 from test_types message - * - * @return uint16_t - */ -static inline uint16_t mavlink_msg_test_types_get_u16(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 144); -} - -/** - * @brief Get field u32 from test_types message - * - * @return uint32_t - */ -static inline uint32_t mavlink_msg_test_types_get_u32(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 96); -} - -/** - * @brief Get field u64 from test_types message - * - * @return uint64_t - */ -static inline uint64_t mavlink_msg_test_types_get_u64(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field s8 from test_types message - * - * @return int8_t - */ -static inline int8_t mavlink_msg_test_types_get_s8(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int8_t(msg, 172); -} - -/** - * @brief Get field s16 from test_types message - * - * @return int16_t - */ -static inline int16_t mavlink_msg_test_types_get_s16(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 146); -} - -/** - * @brief Get field s32 from test_types message - * - * @return int32_t - */ -static inline int32_t mavlink_msg_test_types_get_s32(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 100); -} - -/** - * @brief Get field s64 from test_types message - * - * @return int64_t - */ -static inline int64_t mavlink_msg_test_types_get_s64(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int64_t(msg, 8); -} - -/** - * @brief Get field f from test_types message - * - * @return float - */ -static inline float mavlink_msg_test_types_get_f(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 104); -} - -/** - * @brief Get field d from test_types message - * - * @return double - */ -static inline double mavlink_msg_test_types_get_d(const mavlink_message_t* msg) -{ - return _MAV_RETURN_double(msg, 16); -} - -/** - * @brief Get field u8_array from test_types message - * - * @return uint8_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_u8_array(const mavlink_message_t* msg, uint8_t *u8_array) -{ - return _MAV_RETURN_uint8_t_array(msg, u8_array, 3, 173); -} - -/** - * @brief Get field u16_array from test_types message - * - * @return uint16_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_u16_array(const mavlink_message_t* msg, uint16_t *u16_array) -{ - return _MAV_RETURN_uint16_t_array(msg, u16_array, 3, 148); -} - -/** - * @brief Get field u32_array from test_types message - * - * @return uint32_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_u32_array(const mavlink_message_t* msg, uint32_t *u32_array) -{ - return _MAV_RETURN_uint32_t_array(msg, u32_array, 3, 108); -} - -/** - * @brief Get field u64_array from test_types message - * - * @return uint64_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_u64_array(const mavlink_message_t* msg, uint64_t *u64_array) -{ - return _MAV_RETURN_uint64_t_array(msg, u64_array, 3, 24); -} - -/** - * @brief Get field s8_array from test_types message - * - * @return int8_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_s8_array(const mavlink_message_t* msg, int8_t *s8_array) -{ - return _MAV_RETURN_int8_t_array(msg, s8_array, 3, 176); -} - -/** - * @brief Get field s16_array from test_types message - * - * @return int16_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_s16_array(const mavlink_message_t* msg, int16_t *s16_array) -{ - return _MAV_RETURN_int16_t_array(msg, s16_array, 3, 154); -} - -/** - * @brief Get field s32_array from test_types message - * - * @return int32_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_s32_array(const mavlink_message_t* msg, int32_t *s32_array) -{ - return _MAV_RETURN_int32_t_array(msg, s32_array, 3, 120); -} - -/** - * @brief Get field s64_array from test_types message - * - * @return int64_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_s64_array(const mavlink_message_t* msg, int64_t *s64_array) -{ - return _MAV_RETURN_int64_t_array(msg, s64_array, 3, 48); -} - -/** - * @brief Get field f_array from test_types message - * - * @return float_array - */ -static inline uint16_t mavlink_msg_test_types_get_f_array(const mavlink_message_t* msg, float *f_array) -{ - return _MAV_RETURN_float_array(msg, f_array, 3, 132); -} - -/** - * @brief Get field d_array from test_types message - * - * @return double_array - */ -static inline uint16_t mavlink_msg_test_types_get_d_array(const mavlink_message_t* msg, double *d_array) -{ - return _MAV_RETURN_double_array(msg, d_array, 3, 72); -} - -/** - * @brief Decode a test_types message into a struct - * - * @param msg The message to decode - * @param test_types C-struct to decode the message contents into - */ -static inline void mavlink_msg_test_types_decode(const mavlink_message_t* msg, mavlink_test_types_t* test_types) -{ -#if MAVLINK_NEED_BYTE_SWAP - test_types->u64 = mavlink_msg_test_types_get_u64(msg); - test_types->s64 = mavlink_msg_test_types_get_s64(msg); - test_types->d = mavlink_msg_test_types_get_d(msg); - mavlink_msg_test_types_get_u64_array(msg, test_types->u64_array); - mavlink_msg_test_types_get_s64_array(msg, test_types->s64_array); - mavlink_msg_test_types_get_d_array(msg, test_types->d_array); - test_types->u32 = mavlink_msg_test_types_get_u32(msg); - test_types->s32 = mavlink_msg_test_types_get_s32(msg); - test_types->f = mavlink_msg_test_types_get_f(msg); - mavlink_msg_test_types_get_u32_array(msg, test_types->u32_array); - mavlink_msg_test_types_get_s32_array(msg, test_types->s32_array); - mavlink_msg_test_types_get_f_array(msg, test_types->f_array); - test_types->u16 = mavlink_msg_test_types_get_u16(msg); - test_types->s16 = mavlink_msg_test_types_get_s16(msg); - mavlink_msg_test_types_get_u16_array(msg, test_types->u16_array); - mavlink_msg_test_types_get_s16_array(msg, test_types->s16_array); - test_types->c = mavlink_msg_test_types_get_c(msg); - mavlink_msg_test_types_get_s(msg, test_types->s); - test_types->u8 = mavlink_msg_test_types_get_u8(msg); - test_types->s8 = mavlink_msg_test_types_get_s8(msg); - mavlink_msg_test_types_get_u8_array(msg, test_types->u8_array); - mavlink_msg_test_types_get_s8_array(msg, test_types->s8_array); -#else - memcpy(test_types, _MAV_PAYLOAD(msg), 179); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/test/test.h b/libraries/GCS_MAVLink/include_v1.0/test/test.h deleted file mode 100644 index 4dc04f889f..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/test/test.h +++ /dev/null @@ -1,53 +0,0 @@ -/** @file - * @brief MAVLink comm protocol generated from test.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef TEST_H -#define TEST_H - -#ifdef __cplusplus -extern "C" { -#endif - -// MESSAGE LENGTHS AND CRCS - -#ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {179, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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 {103, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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_TEST_TYPES, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}} -#endif - -#include "../protocol.h" - -#define MAVLINK_ENABLED_TEST - - - -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 3 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 3 -#endif - -// ENUM DEFINITIONS - - - -// MESSAGE DEFINITIONS -#include "./mavlink_msg_test_types.h" - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // TEST_H diff --git a/libraries/GCS_MAVLink/include_v1.0/test/testsuite.h b/libraries/GCS_MAVLink/include_v1.0/test/testsuite.h deleted file mode 100644 index bfb269fb5f..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/test/testsuite.h +++ /dev/null @@ -1,120 +0,0 @@ -/** @file - * @brief MAVLink comm protocol testsuite generated from test.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef TEST_TESTSUITE_H -#define TEST_TESTSUITE_H - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef MAVLINK_TEST_ALL -#define MAVLINK_TEST_ALL - -static void mavlink_test_test(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_test(system_id, component_id, last_msg); -} -#endif - - - - -static void mavlink_test_test_types(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_test_types_t packet_in = { - 93372036854775807ULL, - 93372036854776311LL, - 235.0, - { 93372036854777319, 93372036854777320, 93372036854777321 }, - { 93372036854778831, 93372036854778832, 93372036854778833 }, - { 627.0, 628.0, 629.0 }, - 963502456, - 963502664, - 745.0, - { 963503080, 963503081, 963503082 }, - { 963503704, 963503705, 963503706 }, - { 941.0, 942.0, 943.0 }, - 24723, - 24827, - { 24931, 24932, 24933 }, - { 25243, 25244, 25245 }, - 'E', - "FGHIJKLMN", - 198, - 9, - { 76, 77, 78 }, - { 21, 22, 23 }, - }; - mavlink_test_types_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.u64 = packet_in.u64; - packet1.s64 = packet_in.s64; - packet1.d = packet_in.d; - packet1.u32 = packet_in.u32; - packet1.s32 = packet_in.s32; - packet1.f = packet_in.f; - packet1.u16 = packet_in.u16; - packet1.s16 = packet_in.s16; - packet1.c = packet_in.c; - packet1.u8 = packet_in.u8; - packet1.s8 = packet_in.s8; - - memcpy(packet1.u64_array, packet_in.u64_array, sizeof(uint64_t)*3); - memcpy(packet1.s64_array, packet_in.s64_array, sizeof(int64_t)*3); - memcpy(packet1.d_array, packet_in.d_array, sizeof(double)*3); - memcpy(packet1.u32_array, packet_in.u32_array, sizeof(uint32_t)*3); - memcpy(packet1.s32_array, packet_in.s32_array, sizeof(int32_t)*3); - memcpy(packet1.f_array, packet_in.f_array, sizeof(float)*3); - memcpy(packet1.u16_array, packet_in.u16_array, sizeof(uint16_t)*3); - memcpy(packet1.s16_array, packet_in.s16_array, sizeof(int16_t)*3); - memcpy(packet1.s, packet_in.s, sizeof(char)*10); - memcpy(packet1.u8_array, packet_in.u8_array, sizeof(uint8_t)*3); - memcpy(packet1.s8_array, packet_in.s8_array, sizeof(int8_t)*3); - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_test_types_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_test_types_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_test_types_pack(system_id, component_id, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array ); - mavlink_msg_test_types_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_test_types_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array ); - mavlink_msg_test_types_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_NAV_FILTER_BIAS; - return mavlink_finalize_message(msg, system_id, component_id, 32, 34); -} - -/** - * @brief Pack a nav_filter_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 was sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (microseconds) - * @param accel_0 b_f[0] - * @param accel_1 b_f[1] - * @param accel_2 b_f[2] - * @param gyro_0 b_f[0] - * @param gyro_1 b_f[1] - * @param gyro_2 b_f[2] - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_nav_filter_bias_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t usec,float accel_0,float accel_1,float accel_2,float gyro_0,float gyro_1,float gyro_2) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[32]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, accel_0); - _mav_put_float(buf, 12, accel_1); - _mav_put_float(buf, 16, accel_2); - _mav_put_float(buf, 20, gyro_0); - _mav_put_float(buf, 24, gyro_1); - _mav_put_float(buf, 28, gyro_2); - - memcpy(_MAV_PAYLOAD(msg), buf, 32); -#else - mavlink_nav_filter_bias_t packet; - packet.usec = usec; - packet.accel_0 = accel_0; - packet.accel_1 = accel_1; - packet.accel_2 = accel_2; - packet.gyro_0 = gyro_0; - packet.gyro_1 = gyro_1; - packet.gyro_2 = gyro_2; - - memcpy(_MAV_PAYLOAD(msg), &packet, 32); -#endif - - msg->msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 34); -} - -/** - * @brief Encode a nav_filter_bias struct into 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 nav_filter_bias C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_nav_filter_bias_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_nav_filter_bias_t* nav_filter_bias) -{ - return mavlink_msg_nav_filter_bias_pack(system_id, component_id, msg, nav_filter_bias->usec, nav_filter_bias->accel_0, nav_filter_bias->accel_1, nav_filter_bias->accel_2, nav_filter_bias->gyro_0, nav_filter_bias->gyro_1, nav_filter_bias->gyro_2); -} - -/** - * @brief Send a nav_filter_bias message - * @param chan MAVLink channel to send the message - * - * @param usec Timestamp (microseconds) - * @param accel_0 b_f[0] - * @param accel_1 b_f[1] - * @param accel_2 b_f[2] - * @param gyro_0 b_f[0] - * @param gyro_1 b_f[1] - * @param gyro_2 b_f[2] - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_nav_filter_bias_send(mavlink_channel_t chan, uint64_t usec, float accel_0, float accel_1, float accel_2, float gyro_0, float gyro_1, float gyro_2) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[32]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, accel_0); - _mav_put_float(buf, 12, accel_1); - _mav_put_float(buf, 16, accel_2); - _mav_put_float(buf, 20, gyro_0); - _mav_put_float(buf, 24, gyro_1); - _mav_put_float(buf, 28, gyro_2); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_FILTER_BIAS, buf, 32, 34); -#else - mavlink_nav_filter_bias_t packet; - packet.usec = usec; - packet.accel_0 = accel_0; - packet.accel_1 = accel_1; - packet.accel_2 = accel_2; - packet.gyro_0 = gyro_0; - packet.gyro_1 = gyro_1; - packet.gyro_2 = gyro_2; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_FILTER_BIAS, (const char *)&packet, 32, 34); -#endif -} - -#endif - -// MESSAGE NAV_FILTER_BIAS UNPACKING - - -/** - * @brief Get field usec from nav_filter_bias message - * - * @return Timestamp (microseconds) - */ -static inline uint64_t mavlink_msg_nav_filter_bias_get_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field accel_0 from nav_filter_bias message - * - * @return b_f[0] - */ -static inline float mavlink_msg_nav_filter_bias_get_accel_0(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field accel_1 from nav_filter_bias message - * - * @return b_f[1] - */ -static inline float mavlink_msg_nav_filter_bias_get_accel_1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field accel_2 from nav_filter_bias message - * - * @return b_f[2] - */ -static inline float mavlink_msg_nav_filter_bias_get_accel_2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field gyro_0 from nav_filter_bias message - * - * @return b_f[0] - */ -static inline float mavlink_msg_nav_filter_bias_get_gyro_0(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field gyro_1 from nav_filter_bias message - * - * @return b_f[1] - */ -static inline float mavlink_msg_nav_filter_bias_get_gyro_1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field gyro_2 from nav_filter_bias message - * - * @return b_f[2] - */ -static inline float mavlink_msg_nav_filter_bias_get_gyro_2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Decode a nav_filter_bias message into a struct - * - * @param msg The message to decode - * @param nav_filter_bias C-struct to decode the message contents into - */ -static inline void mavlink_msg_nav_filter_bias_decode(const mavlink_message_t* msg, mavlink_nav_filter_bias_t* nav_filter_bias) -{ -#if MAVLINK_NEED_BYTE_SWAP - nav_filter_bias->usec = mavlink_msg_nav_filter_bias_get_usec(msg); - nav_filter_bias->accel_0 = mavlink_msg_nav_filter_bias_get_accel_0(msg); - nav_filter_bias->accel_1 = mavlink_msg_nav_filter_bias_get_accel_1(msg); - nav_filter_bias->accel_2 = mavlink_msg_nav_filter_bias_get_accel_2(msg); - nav_filter_bias->gyro_0 = mavlink_msg_nav_filter_bias_get_gyro_0(msg); - nav_filter_bias->gyro_1 = mavlink_msg_nav_filter_bias_get_gyro_1(msg); - nav_filter_bias->gyro_2 = mavlink_msg_nav_filter_bias_get_gyro_2(msg); -#else - memcpy(nav_filter_bias, _MAV_PAYLOAD(msg), 32); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/ualberta/mavlink_msg_radio_calibration.h b/libraries/GCS_MAVLink/include_v1.0/ualberta/mavlink_msg_radio_calibration.h deleted file mode 100644 index ffe2b74856..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/ualberta/mavlink_msg_radio_calibration.h +++ /dev/null @@ -1,259 +0,0 @@ -// MESSAGE RADIO_CALIBRATION PACKING - -#define MAVLINK_MSG_ID_RADIO_CALIBRATION 221 - -typedef struct __mavlink_radio_calibration_t -{ - uint16_t aileron[3]; ///< Aileron setpoints: left, center, right - uint16_t elevator[3]; ///< Elevator setpoints: nose down, center, nose up - uint16_t rudder[3]; ///< Rudder setpoints: nose left, center, nose right - uint16_t gyro[2]; ///< Tail gyro mode/gain setpoints: heading hold, rate mode - uint16_t pitch[5]; ///< Pitch curve setpoints (every 25%) - uint16_t throttle[5]; ///< Throttle curve setpoints (every 25%) -} mavlink_radio_calibration_t; - -#define MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN 42 -#define MAVLINK_MSG_ID_221_LEN 42 - -#define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_AILERON_LEN 3 -#define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_ELEVATOR_LEN 3 -#define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_RUDDER_LEN 3 -#define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_GYRO_LEN 2 -#define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_PITCH_LEN 5 -#define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_THROTTLE_LEN 5 - -#define MAVLINK_MESSAGE_INFO_RADIO_CALIBRATION { \ - "RADIO_CALIBRATION", \ - 6, \ - { { "aileron", NULL, MAVLINK_TYPE_UINT16_T, 3, 0, offsetof(mavlink_radio_calibration_t, aileron) }, \ - { "elevator", NULL, MAVLINK_TYPE_UINT16_T, 3, 6, offsetof(mavlink_radio_calibration_t, elevator) }, \ - { "rudder", NULL, MAVLINK_TYPE_UINT16_T, 3, 12, offsetof(mavlink_radio_calibration_t, rudder) }, \ - { "gyro", NULL, MAVLINK_TYPE_UINT16_T, 2, 18, offsetof(mavlink_radio_calibration_t, gyro) }, \ - { "pitch", NULL, MAVLINK_TYPE_UINT16_T, 5, 22, offsetof(mavlink_radio_calibration_t, pitch) }, \ - { "throttle", NULL, MAVLINK_TYPE_UINT16_T, 5, 32, offsetof(mavlink_radio_calibration_t, throttle) }, \ - } \ -} - - -/** - * @brief Pack a radio_calibration 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 aileron Aileron setpoints: left, center, right - * @param elevator Elevator setpoints: nose down, center, nose up - * @param rudder Rudder setpoints: nose left, center, nose right - * @param gyro Tail gyro mode/gain setpoints: heading hold, rate mode - * @param pitch Pitch curve setpoints (every 25%) - * @param throttle Throttle curve setpoints (every 25%) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_radio_calibration_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const uint16_t *aileron, const uint16_t *elevator, const uint16_t *rudder, const uint16_t *gyro, const uint16_t *pitch, const uint16_t *throttle) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[42]; - - _mav_put_uint16_t_array(buf, 0, aileron, 3); - _mav_put_uint16_t_array(buf, 6, elevator, 3); - _mav_put_uint16_t_array(buf, 12, rudder, 3); - _mav_put_uint16_t_array(buf, 18, gyro, 2); - _mav_put_uint16_t_array(buf, 22, pitch, 5); - _mav_put_uint16_t_array(buf, 32, throttle, 5); - memcpy(_MAV_PAYLOAD(msg), buf, 42); -#else - mavlink_radio_calibration_t packet; - - memcpy(packet.aileron, aileron, sizeof(uint16_t)*3); - memcpy(packet.elevator, elevator, sizeof(uint16_t)*3); - memcpy(packet.rudder, rudder, sizeof(uint16_t)*3); - memcpy(packet.gyro, gyro, sizeof(uint16_t)*2); - memcpy(packet.pitch, pitch, sizeof(uint16_t)*5); - memcpy(packet.throttle, throttle, sizeof(uint16_t)*5); - memcpy(_MAV_PAYLOAD(msg), &packet, 42); -#endif - - msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; - return mavlink_finalize_message(msg, system_id, component_id, 42, 71); -} - -/** - * @brief Pack a radio_calibration 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 was sent over - * @param msg The MAVLink message to compress the data into - * @param aileron Aileron setpoints: left, center, right - * @param elevator Elevator setpoints: nose down, center, nose up - * @param rudder Rudder setpoints: nose left, center, nose right - * @param gyro Tail gyro mode/gain setpoints: heading hold, rate mode - * @param pitch Pitch curve setpoints (every 25%) - * @param throttle Throttle curve setpoints (every 25%) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_radio_calibration_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const uint16_t *aileron,const uint16_t *elevator,const uint16_t *rudder,const uint16_t *gyro,const uint16_t *pitch,const uint16_t *throttle) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[42]; - - _mav_put_uint16_t_array(buf, 0, aileron, 3); - _mav_put_uint16_t_array(buf, 6, elevator, 3); - _mav_put_uint16_t_array(buf, 12, rudder, 3); - _mav_put_uint16_t_array(buf, 18, gyro, 2); - _mav_put_uint16_t_array(buf, 22, pitch, 5); - _mav_put_uint16_t_array(buf, 32, throttle, 5); - memcpy(_MAV_PAYLOAD(msg), buf, 42); -#else - mavlink_radio_calibration_t packet; - - memcpy(packet.aileron, aileron, sizeof(uint16_t)*3); - memcpy(packet.elevator, elevator, sizeof(uint16_t)*3); - memcpy(packet.rudder, rudder, sizeof(uint16_t)*3); - memcpy(packet.gyro, gyro, sizeof(uint16_t)*2); - memcpy(packet.pitch, pitch, sizeof(uint16_t)*5); - memcpy(packet.throttle, throttle, sizeof(uint16_t)*5); - memcpy(_MAV_PAYLOAD(msg), &packet, 42); -#endif - - msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 42, 71); -} - -/** - * @brief Encode a radio_calibration struct into 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 radio_calibration C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_radio_calibration_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_radio_calibration_t* radio_calibration) -{ - return mavlink_msg_radio_calibration_pack(system_id, component_id, msg, radio_calibration->aileron, radio_calibration->elevator, radio_calibration->rudder, radio_calibration->gyro, radio_calibration->pitch, radio_calibration->throttle); -} - -/** - * @brief Send a radio_calibration message - * @param chan MAVLink channel to send the message - * - * @param aileron Aileron setpoints: left, center, right - * @param elevator Elevator setpoints: nose down, center, nose up - * @param rudder Rudder setpoints: nose left, center, nose right - * @param gyro Tail gyro mode/gain setpoints: heading hold, rate mode - * @param pitch Pitch curve setpoints (every 25%) - * @param throttle Throttle curve setpoints (every 25%) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_radio_calibration_send(mavlink_channel_t chan, const uint16_t *aileron, const uint16_t *elevator, const uint16_t *rudder, const uint16_t *gyro, const uint16_t *pitch, const uint16_t *throttle) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[42]; - - _mav_put_uint16_t_array(buf, 0, aileron, 3); - _mav_put_uint16_t_array(buf, 6, elevator, 3); - _mav_put_uint16_t_array(buf, 12, rudder, 3); - _mav_put_uint16_t_array(buf, 18, gyro, 2); - _mav_put_uint16_t_array(buf, 22, pitch, 5); - _mav_put_uint16_t_array(buf, 32, throttle, 5); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_CALIBRATION, buf, 42, 71); -#else - mavlink_radio_calibration_t packet; - - memcpy(packet.aileron, aileron, sizeof(uint16_t)*3); - memcpy(packet.elevator, elevator, sizeof(uint16_t)*3); - memcpy(packet.rudder, rudder, sizeof(uint16_t)*3); - memcpy(packet.gyro, gyro, sizeof(uint16_t)*2); - memcpy(packet.pitch, pitch, sizeof(uint16_t)*5); - memcpy(packet.throttle, throttle, sizeof(uint16_t)*5); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_CALIBRATION, (const char *)&packet, 42, 71); -#endif -} - -#endif - -// MESSAGE RADIO_CALIBRATION UNPACKING - - -/** - * @brief Get field aileron from radio_calibration message - * - * @return Aileron setpoints: left, center, right - */ -static inline uint16_t mavlink_msg_radio_calibration_get_aileron(const mavlink_message_t* msg, uint16_t *aileron) -{ - return _MAV_RETURN_uint16_t_array(msg, aileron, 3, 0); -} - -/** - * @brief Get field elevator from radio_calibration message - * - * @return Elevator setpoints: nose down, center, nose up - */ -static inline uint16_t mavlink_msg_radio_calibration_get_elevator(const mavlink_message_t* msg, uint16_t *elevator) -{ - return _MAV_RETURN_uint16_t_array(msg, elevator, 3, 6); -} - -/** - * @brief Get field rudder from radio_calibration message - * - * @return Rudder setpoints: nose left, center, nose right - */ -static inline uint16_t mavlink_msg_radio_calibration_get_rudder(const mavlink_message_t* msg, uint16_t *rudder) -{ - return _MAV_RETURN_uint16_t_array(msg, rudder, 3, 12); -} - -/** - * @brief Get field gyro from radio_calibration message - * - * @return Tail gyro mode/gain setpoints: heading hold, rate mode - */ -static inline uint16_t mavlink_msg_radio_calibration_get_gyro(const mavlink_message_t* msg, uint16_t *gyro) -{ - return _MAV_RETURN_uint16_t_array(msg, gyro, 2, 18); -} - -/** - * @brief Get field pitch from radio_calibration message - * - * @return Pitch curve setpoints (every 25%) - */ -static inline uint16_t mavlink_msg_radio_calibration_get_pitch(const mavlink_message_t* msg, uint16_t *pitch) -{ - return _MAV_RETURN_uint16_t_array(msg, pitch, 5, 22); -} - -/** - * @brief Get field throttle from radio_calibration message - * - * @return Throttle curve setpoints (every 25%) - */ -static inline uint16_t mavlink_msg_radio_calibration_get_throttle(const mavlink_message_t* msg, uint16_t *throttle) -{ - return _MAV_RETURN_uint16_t_array(msg, throttle, 5, 32); -} - -/** - * @brief Decode a radio_calibration message into a struct - * - * @param msg The message to decode - * @param radio_calibration C-struct to decode the message contents into - */ -static inline void mavlink_msg_radio_calibration_decode(const mavlink_message_t* msg, mavlink_radio_calibration_t* radio_calibration) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_radio_calibration_get_aileron(msg, radio_calibration->aileron); - mavlink_msg_radio_calibration_get_elevator(msg, radio_calibration->elevator); - mavlink_msg_radio_calibration_get_rudder(msg, radio_calibration->rudder); - mavlink_msg_radio_calibration_get_gyro(msg, radio_calibration->gyro); - mavlink_msg_radio_calibration_get_pitch(msg, radio_calibration->pitch); - mavlink_msg_radio_calibration_get_throttle(msg, radio_calibration->throttle); -#else - memcpy(radio_calibration, _MAV_PAYLOAD(msg), 42); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/ualberta/mavlink_msg_ualberta_sys_status.h b/libraries/GCS_MAVLink/include_v1.0/ualberta/mavlink_msg_ualberta_sys_status.h deleted file mode 100644 index 69c4599e13..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/ualberta/mavlink_msg_ualberta_sys_status.h +++ /dev/null @@ -1,188 +0,0 @@ -// MESSAGE UALBERTA_SYS_STATUS PACKING - -#define MAVLINK_MSG_ID_UALBERTA_SYS_STATUS 222 - -typedef struct __mavlink_ualberta_sys_status_t -{ - uint8_t mode; ///< System mode, see UALBERTA_AUTOPILOT_MODE ENUM - uint8_t nav_mode; ///< Navigation mode, see UALBERTA_NAV_MODE ENUM - uint8_t pilot; ///< Pilot mode, see UALBERTA_PILOT_MODE -} mavlink_ualberta_sys_status_t; - -#define MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN 3 -#define MAVLINK_MSG_ID_222_LEN 3 - - - -#define MAVLINK_MESSAGE_INFO_UALBERTA_SYS_STATUS { \ - "UALBERTA_SYS_STATUS", \ - 3, \ - { { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_ualberta_sys_status_t, mode) }, \ - { "nav_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_ualberta_sys_status_t, nav_mode) }, \ - { "pilot", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_ualberta_sys_status_t, pilot) }, \ - } \ -} - - -/** - * @brief Pack a ualberta_sys_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 mode System mode, see UALBERTA_AUTOPILOT_MODE ENUM - * @param nav_mode Navigation mode, see UALBERTA_NAV_MODE ENUM - * @param pilot Pilot mode, see UALBERTA_PILOT_MODE - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ualberta_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t mode, uint8_t nav_mode, uint8_t pilot) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[3]; - _mav_put_uint8_t(buf, 0, mode); - _mav_put_uint8_t(buf, 1, nav_mode); - _mav_put_uint8_t(buf, 2, pilot); - - memcpy(_MAV_PAYLOAD(msg), buf, 3); -#else - mavlink_ualberta_sys_status_t packet; - packet.mode = mode; - packet.nav_mode = nav_mode; - packet.pilot = pilot; - - memcpy(_MAV_PAYLOAD(msg), &packet, 3); -#endif - - msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; - return mavlink_finalize_message(msg, system_id, component_id, 3, 15); -} - -/** - * @brief Pack a ualberta_sys_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 was sent over - * @param msg The MAVLink message to compress the data into - * @param mode System mode, see UALBERTA_AUTOPILOT_MODE ENUM - * @param nav_mode Navigation mode, see UALBERTA_NAV_MODE ENUM - * @param pilot Pilot mode, see UALBERTA_PILOT_MODE - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ualberta_sys_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t mode,uint8_t nav_mode,uint8_t pilot) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[3]; - _mav_put_uint8_t(buf, 0, mode); - _mav_put_uint8_t(buf, 1, nav_mode); - _mav_put_uint8_t(buf, 2, pilot); - - memcpy(_MAV_PAYLOAD(msg), buf, 3); -#else - mavlink_ualberta_sys_status_t packet; - packet.mode = mode; - packet.nav_mode = nav_mode; - packet.pilot = pilot; - - memcpy(_MAV_PAYLOAD(msg), &packet, 3); -#endif - - msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 15); -} - -/** - * @brief Encode a ualberta_sys_status struct into 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 ualberta_sys_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ualberta_sys_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ualberta_sys_status_t* ualberta_sys_status) -{ - return mavlink_msg_ualberta_sys_status_pack(system_id, component_id, msg, ualberta_sys_status->mode, ualberta_sys_status->nav_mode, ualberta_sys_status->pilot); -} - -/** - * @brief Send a ualberta_sys_status message - * @param chan MAVLink channel to send the message - * - * @param mode System mode, see UALBERTA_AUTOPILOT_MODE ENUM - * @param nav_mode Navigation mode, see UALBERTA_NAV_MODE ENUM - * @param pilot Pilot mode, see UALBERTA_PILOT_MODE - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_ualberta_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t pilot) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[3]; - _mav_put_uint8_t(buf, 0, mode); - _mav_put_uint8_t(buf, 1, nav_mode); - _mav_put_uint8_t(buf, 2, pilot); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UALBERTA_SYS_STATUS, buf, 3, 15); -#else - mavlink_ualberta_sys_status_t packet; - packet.mode = mode; - packet.nav_mode = nav_mode; - packet.pilot = pilot; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UALBERTA_SYS_STATUS, (const char *)&packet, 3, 15); -#endif -} - -#endif - -// MESSAGE UALBERTA_SYS_STATUS UNPACKING - - -/** - * @brief Get field mode from ualberta_sys_status message - * - * @return System mode, see UALBERTA_AUTOPILOT_MODE ENUM - */ -static inline uint8_t mavlink_msg_ualberta_sys_status_get_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field nav_mode from ualberta_sys_status message - * - * @return Navigation mode, see UALBERTA_NAV_MODE ENUM - */ -static inline uint8_t mavlink_msg_ualberta_sys_status_get_nav_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field pilot from ualberta_sys_status message - * - * @return Pilot mode, see UALBERTA_PILOT_MODE - */ -static inline uint8_t mavlink_msg_ualberta_sys_status_get_pilot(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Decode a ualberta_sys_status message into a struct - * - * @param msg The message to decode - * @param ualberta_sys_status C-struct to decode the message contents into - */ -static inline void mavlink_msg_ualberta_sys_status_decode(const mavlink_message_t* msg, mavlink_ualberta_sys_status_t* ualberta_sys_status) -{ -#if MAVLINK_NEED_BYTE_SWAP - ualberta_sys_status->mode = mavlink_msg_ualberta_sys_status_get_mode(msg); - ualberta_sys_status->nav_mode = mavlink_msg_ualberta_sys_status_get_nav_mode(msg); - ualberta_sys_status->pilot = mavlink_msg_ualberta_sys_status_get_pilot(msg); -#else - memcpy(ualberta_sys_status, _MAV_PAYLOAD(msg), 3); -#endif -} diff --git a/libraries/GCS_MAVLink/include_v1.0/ualberta/testsuite.h b/libraries/GCS_MAVLink/include_v1.0/ualberta/testsuite.h deleted file mode 100644 index 11ba27a3cf..0000000000 --- a/libraries/GCS_MAVLink/include_v1.0/ualberta/testsuite.h +++ /dev/null @@ -1,192 +0,0 @@ -/** @file - * @brief MAVLink comm protocol testsuite generated from ualberta.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef UALBERTA_TESTSUITE_H -#define UALBERTA_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_ualberta(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_ualberta(system_id, component_id, last_msg); -} -#endif - -#include "../common/testsuite.h" - - -static void mavlink_test_nav_filter_bias(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_nav_filter_bias_t packet_in = { - 93372036854775807ULL, - 73.0, - 101.0, - 129.0, - 157.0, - 185.0, - 213.0, - }; - mavlink_nav_filter_bias_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.usec = packet_in.usec; - packet1.accel_0 = packet_in.accel_0; - packet1.accel_1 = packet_in.accel_1; - packet1.accel_2 = packet_in.accel_2; - packet1.gyro_0 = packet_in.gyro_0; - packet1.gyro_1 = packet_in.gyro_1; - packet1.gyro_2 = packet_in.gyro_2; - - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_nav_filter_bias_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_nav_filter_bias_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_nav_filter_bias_pack(system_id, component_id, &msg , packet1.usec , packet1.accel_0 , packet1.accel_1 , packet1.accel_2 , packet1.gyro_0 , packet1.gyro_1 , packet1.gyro_2 ); - mavlink_msg_nav_filter_bias_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_nav_filter_bias_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.accel_0 , packet1.accel_1 , packet1.accel_2 , packet1.gyro_0 , packet1.gyro_1 , packet1.gyro_2 ); - mavlink_msg_nav_filter_bias_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 + + + + + + Enumeration of possible mount operation modes + Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization + Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM. + Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization + Load neutral position and start RC Roll,Pitch,Yaw control with stabilization + Load neutral position and start to point to Lat,Lon,Alt + + + + + + 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) + + + + 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 + + + + + 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 + + + + 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 + + + + Offsets and calibrations values for hardware @@ -51,5 +113,65 @@ ADC output 6 + + + Configure on-board Camera Control System. + System ID + Component ID + Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) + Divisor number //e.g. 1000 means 1/1000 (0 means ignore) + F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) + ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) + Exposure type enumeration from 1 to N (0 means ignore) + Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once + Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) + Extra parameters enumeration (0 means ignore) + Correspondent value to given extra_param + + + + Control on-board Camera Control System to take shots. + System ID + Component ID + 0: stop, 1: start or keep it up //Session control e.g. show/hide lens + 1 to N //Zoom's absolute position (0 means ignore) + -100 to 100 //Zooming step value to offset zoom from the current position + 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus + 0: ignore, 1: shot or start filming + Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once + Extra parameters enumeration (0 means ignore) + Correspondent value to given extra_param + + + + + Message to configure a camera mount, directional antenna, etc. + System ID + Component ID + mount operating mode (see MAV_MOUNT_MODE enum) + (1 = yes, 0 = no) + (1 = yes, 0 = no) + (1 = yes, 0 = no) + + + + Message to control a camera mount, directional antenna, etc. + System ID + Component ID + pitch(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 + if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) + + + + Message with some status from APM to GCS about camera or antenna mount + System ID + Component ID + 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 + + diff --git a/libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml b/libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml index 33fc5b8de5..60d86024ac 100644 --- a/libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml +++ b/libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml @@ -8,6 +8,68 @@ If you prototype a message here, then you should consider if it is general enough to move into common.xml later --> + + + + + + Enumeration of possible mount operation modes + Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization + Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM. + Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization + Load neutral position and start RC Roll,Pitch,Yaw control with stabilization + Load neutral position and start to point to Lat,Lon,Alt + + + + + + 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) + + + + 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 + + + + + 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 + + + + 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 + + + + Offsets and calibrations values for hardware @@ -51,5 +113,65 @@ ADC output 6 + + + Configure on-board Camera Control System. + System ID + Component ID + Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) + Divisor number //e.g. 1000 means 1/1000 (0 means ignore) + F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) + ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) + Exposure type enumeration from 1 to N (0 means ignore) + Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once + Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) + Extra parameters enumeration (0 means ignore) + Correspondent value to given extra_param + + + + Control on-board Camera Control System to take shots. + System ID + Component ID + 0: stop, 1: start or keep it up //Session control e.g. show/hide lens + 1 to N //Zoom's absolute position (0 means ignore) + -100 to 100 //Zooming step value to offset zoom from the current position + 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus + 0: ignore, 1: shot or start filming + Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once + Extra parameters enumeration (0 means ignore) + Correspondent value to given extra_param + + + + + Message to configure a camera mount, directional antenna, etc. + System ID + Component ID + mount operating mode (see MAV_MOUNT_MODE enum) + (1 = yes, 0 = no) + (1 = yes, 0 = no) + (1 = yes, 0 = no) + + + + Message to control a camera mount, directional antenna, etc. + System ID + Component ID + pitch(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 + if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) + + + + Message with some status from APM to GCS about camera or antenna mount + System ID + Component ID + 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 + + diff --git a/libraries/GCS_MAVLink/message_definitions_v1.0/common.xml b/libraries/GCS_MAVLink/message_definitions_v1.0/common.xml index 066d2dbc78..de53b7e6a0 100644 --- a/libraries/GCS_MAVLink/message_definitions_v1.0/common.xml +++ b/libraries/GCS_MAVLink/message_definitions_v1.0/common.xml @@ -65,7 +65,7 @@ 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. - + 0b00000001 Reserved for future use. @@ -93,7 +93,7 @@ Seventh bit: 00000010 - + Eighth bit: 00000001 @@ -103,7 +103,7 @@ Hold at the current position. - Continue with the mission execution. + Continue with the next item in mission execution. Hold at the current position of the system @@ -656,7 +656,7 @@ Hold / continue the current action - MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with mission plan + 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 @@ -664,6 +664,11 @@ Longitude / Y position Altitude / Z position + + 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) + Data stream IDs. A data stream is not a fixed set of messages, but rather a @@ -748,7 +753,101 @@ The Z or altitude value is out of range. - + + + + type of a mavlink parameter + + 32 bit float + + + 8 bit unsigned integer + + + 8 bit signed integer + + + 16 bit unsigned integer + + + 16 bit signed integer + + + 32 bit unsigned integer + + + 32 bit signed integer + + + + + result from a mavlink command + + Command ACCEPTED and EXECUTED + + + Command TEMPORARY REJECTED/DENIED + + + Command PERMANENTLY DENIED + + + Command UNKNOWN/UNSUPPORTED + + + Command executed, but failed + + + + + result in a mavlink mission ack + + mission accepted OK + + + generic error / not accepting mission commands at all right now + + + coordinate frame is not supported + + + command is not supported + + + mission item exceeds storage space + + + one of the parameters has an invalid value + + + param1 has an invalid value + + + param2 has an invalid value + + + param3 has an invalid value + + + param4 has an invalid value + + + x/param5 has an invalid value + + + y/param6 has an invalid value + + + param7 has an invalid value + + + received waypoint out of sequence + + + not accepting any mission commands from this communication partner + + + @@ -811,7 +910,7 @@ Set the system mode, as defined by enum MAV_MODE. There is no target component id as the mode is by definition for the overall aircraft, not only for one component. The system setting the mode The new base mode - The new autopilot-specific mode. This field can be ignored by an autopilot. + The new autopilot-specific mode. This field can be ignored by an autopilot. Request to read the onboard parameter with the param_id string id. Onboard parameters are stored as key[const char*] -> value[float]. This allows to send a parameter to any other component (such as the GCS) without the need of previous knowledge of possible parameter names. Thus the same GCS can store different parameters for different autopilots. See also http://qgroundcontrol.org/parameter_interface for a full documentation of QGroundControl and IMU code. @@ -829,7 +928,7 @@ Emit the value of a onboard parameter. The inclusion of param_count and param_index in the message allows the recipient to keep track of received parameters and allows him to re-request missing parameters after a loss or timeout. Onboard parameter id Onboard parameter value - Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t + Onboard parameter type: see MAV_VAR enum Total number of onboard parameters Index of this onboard parameter @@ -839,7 +938,7 @@ Component ID Onboard parameter id Onboard parameter value - Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t + Onboard parameter type: see MAV_VAR enum The global position, as returned by the Global Positioning System (GPS). This is @@ -937,7 +1036,7 @@ Y Speed Z Speed - + The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up). It is designed as scaled integer message since the resolution of float is not sufficient. Timestamp (milliseconds since system boot) @@ -949,21 +1048,8 @@ Ground Y Speed (Longitude), expressed as m/s * 100 Ground Z Speed (Altitude), expressed as m/s * 100 Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - - The RAW values of the RC channels received. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. - Timestamp (milliseconds since system boot) - Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. - RC channel 1 value, in microseconds - RC channel 2 value, in microseconds - RC channel 3 value, in microseconds - RC channel 4 value, in microseconds - RC channel 5 value, in microseconds - RC channel 6 value, in microseconds - RC channel 7 value, in microseconds - RC channel 8 value, in microseconds - Receive signal strength indicator, 0: 0%, 255: 100% - + The scaled values of the RC channels received. (-100%) -10000, (0%) 0, (100%) 10000 Timestamp (milliseconds since system boot) Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. @@ -977,7 +1063,21 @@ RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 Receive signal strength indicator, 0: 0%, 255: 100% - + + The RAW values of the RC channels received. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. + Timestamp (milliseconds since system boot) + Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. + RC channel 1 value, in microseconds + RC channel 2 value, in microseconds + RC channel 3 value, in microseconds + RC channel 4 value, in microseconds + RC channel 5 value, in microseconds + RC channel 6 value, in microseconds + RC channel 7 value, in microseconds + RC channel 8 value, in microseconds + Receive signal strength indicator, 0: 0%, 255: 100% + + The RAW values of the servo outputs (for RC input from the remote, use the RC_CHANNELS messages). The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Timestamp (since UNIX epoch or microseconds since system boot) Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. @@ -990,15 +1090,29 @@ Servo output 7 value, in microseconds Servo output 8 value, in microseconds + + Request the overall list of MISSIONs from the system/component. http://qgroundcontrol.org/mavlink/waypoint_protocol + System ID + Component ID + Start index, 0 by default + End index, -1 by default (-1: send list to end). Else a valid index of the list + + + This message is sent to the MAV to write a partial list. If start index == end index, only one item will be transmitted / updated. If the start index is NOT 0 and above the current list size, this request should be REJECTED! + System ID + Component ID + Start index, 0 by default and smaller / equal to the largest index of the current onboard list. + End index, equal or greater than start index. + - Message encoding a MISSION. This message is emitted to announce - the presence of a MISSION and to set a MISSION on the system. The MISSION can be either in x, y, z meters (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is Z-down, right handed, global frame is Z-up, right handed + Message encoding a mission item. This message is emitted to announce + the presence of a mission item and to set a mission item on the system. The mission item can be either in x, y, z meters (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is Z-down, right handed (NED), global frame is Z-up, right handed (ENU). http://qgroundcontrol.org/mavlink/waypoint_protocol System ID Component ID Sequence The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h - The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs + The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs false:0, true:1 autocontinue to next wp PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters @@ -1010,31 +1124,31 @@ PARAM7 / z position: global: altitude - Request the information of the MISSION with the sequence number seq. The response of the system to this message should be a MISSION message. + Request the information of the mission item with the sequence number seq. The response of the system to this message should be a MISSION_ITEM message. http://qgroundcontrol.org/mavlink/waypoint_protocol System ID Component ID Sequence - Set the MISSION with sequence number seq as current MISSION. This means that the MAV will continue to this MISSION on the shortest path (not following the MISSIONs in-between). + Set the mission item with sequence number seq as current item. This means that the MAV will continue to this mission item on the shortest path (not following the mission items in-between). System ID Component ID Sequence - Message that announces the sequence number of the current active MISSION. The MAV will fly towards this MISSION. + Message that announces the sequence number of the current active mission item. The MAV will fly towards this mission item. Sequence - Request the overall list of MISSIONs from the system/component. + Request the overall list of mission items from the system/component. System ID Component ID - This message is emitted as response to MISSION_REQUEST_LIST by the MAV. The GCS can then request the individual MISSIONs based on the knowledge of the total number of MISSIONs. + This message is emitted as response to MISSION_REQUEST_LIST by the MAV and to initiate a write transaction. The GCS can then request the individual mission item based on the knowledge of the total number of MISSIONs. System ID Component ID - Number of MISSIONs in the Sequence + Number of mission items in the sequence Delete all mission items at once. @@ -1049,7 +1163,7 @@ Ack message during MISSION handling. The type field states if this message is a positive ack (type=0) or if an error happened (type=non-zero). System ID Component ID - 0: OK, 1: generic error / not accepting mission commands at all right now, 2: coordinate frame is not supported, 3: command is not supported, 4: mission item exceeds storage space, 5: one of the parameters has an invalid value, 6: param1 has an invalid value, 7: param2 has an invalid value, 8: param3 has an invalid value, 9: param4 has an invalid value, 10: x/param5 has an invalid value, 11: y:param6 has an invalid value, 12: z:param7 has an invalid value, 13: received waypoint out of sequence, 14: not accepting any mission commands from this communication partner + See MAV_MISSION_RESULT enum As local MISSIONs exist, the global MISSION reference allows to transform between the local coordinate frame and the global (GPS) coordinate frame. This can be necessary when e.g. in- and outdoor settings are connected and the MAV should move from in- to outdoor. @@ -1224,22 +1338,11 @@ Current altitude (MSL), in meters Current climb rate in meters/second - - Send a command with up to four parameters to the MAV - System which should execute the command - Component which should execute the command, 0 for all components - Command ID, as defined by MAV_CMD enum. - 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - Parameter 1, as defined by MAV_CMD enum. - Parameter 2, as defined by MAV_CMD enum. - Parameter 3, as defined by MAV_CMD enum. - Parameter 4, as defined by MAV_CMD enum. - Send a command with up to four parameters to the MAV System which should execute the command Component which should execute the command, 0 for all components - Command ID, as defined by MAV_CMD enum. + Command ID, as defined by MAV_CMD enum. 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) Parameter 1, as defined by MAV_CMD enum. Parameter 2, as defined by MAV_CMD enum. @@ -1252,7 +1355,7 @@ Report status of a command. Includes feedback wether the command was executed Command ID, as defined by MAV_CMD enum. - 1: Command ACCEPTED and EXECUTED, 1: Command TEMPORARY REJECTED/DENIED, 2: Command PERMANENTLY DENIED, 3: Command UNKNOWN/UNSUPPORTED, 4: Command executed, but failed + See MAV_RESULT enum