mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-22 22:32:11 +08:00
Fixed correct RC loss detection, AR.Drone is now shutting down motors after 1 s of RC loss. Added debug topic.
This commit is contained in:
@@ -289,7 +289,10 @@ int ardrone_interface_thread_main(int argc, char *argv[])
|
||||
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
|
||||
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
|
||||
|
||||
if (armed.armed) {
|
||||
/* for now only spin if armed and immediately shut down
|
||||
* if in failsafe
|
||||
*/
|
||||
if (armed.armed && !armed.failsafe) {
|
||||
ardrone_mixing_and_output(ardrone_write, &actuator_controls);
|
||||
} else {
|
||||
/* Silently lock down motor speeds to zero */
|
||||
|
||||
@@ -69,6 +69,7 @@
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/subsystem_info.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
@@ -1174,6 +1175,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
/* End battery voltage check */
|
||||
|
||||
/* Start RC state check */
|
||||
bool prev_lost = current_status.rc_signal_lost;
|
||||
|
||||
if (rc.chan_count > 4 && (hrt_absolute_time() - rc.timestamp) < 100000) {
|
||||
|
||||
@@ -1238,10 +1240,18 @@ int commander_thread_main(int argc, char *argv[])
|
||||
/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
|
||||
current_status.rc_signal_cutting_off = true;
|
||||
current_status.rc_signal_lost_interval = hrt_absolute_time() - rc.timestamp;
|
||||
|
||||
/* if the RC signal is gone for a full second, consider it lost */
|
||||
if (current_status.rc_signal_lost_interval > 1000000) current_status.rc_signal_lost = true;
|
||||
}
|
||||
|
||||
/* Check if this is the first loss or first gain*/
|
||||
if ((!prev_lost && current_status.rc_signal_lost) ||
|
||||
prev_lost && !current_status.rc_signal_lost) {
|
||||
/* publish rc lost */
|
||||
publish_armed_status(¤t_status);
|
||||
}
|
||||
|
||||
/* End mode switch */
|
||||
|
||||
/* END RC state check */
|
||||
|
||||
@@ -199,10 +199,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
|
||||
if (invalid_state == false || old_state != new_state) {
|
||||
current_status->state_machine = new_state;
|
||||
state_machine_publish(status_pub, current_status, mavlink_fd);
|
||||
struct actuator_armed_s armed;
|
||||
armed.armed = current_status->flag_system_armed;
|
||||
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
|
||||
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
|
||||
publish_armed_status(current_status);
|
||||
ret = OK;
|
||||
}
|
||||
if (invalid_state) {
|
||||
@@ -220,6 +217,14 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
|
||||
printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]);
|
||||
}
|
||||
|
||||
void publish_armed_status(const struct vehicle_status_s *current_status) {
|
||||
struct actuator_armed_s armed;
|
||||
armed.armed = current_status->flag_system_armed;
|
||||
armed.failsafe = current_status->rc_signal_lost;
|
||||
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
|
||||
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Private functions, update the state machine
|
||||
|
||||
@@ -188,6 +188,12 @@ void state_machine_emergency_always_critical(int status_pub, struct vehicle_stat
|
||||
*/
|
||||
void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
|
||||
|
||||
/**
|
||||
* Publish the armed state depending on the current system state
|
||||
*
|
||||
* @param current_status the current system status
|
||||
*/
|
||||
void publish_armed_status(const struct vehicle_status_s *current_status);
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1120,5 +1120,5 @@ mpu6000_main(int argc, char *argv[])
|
||||
if (!strcmp(argv[1], "info"))
|
||||
mpu6000::info();
|
||||
|
||||
errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
|
||||
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
|
||||
}
|
||||
|
||||
+38
-7
@@ -75,6 +75,7 @@
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
#include "waypoints.h"
|
||||
@@ -166,6 +167,7 @@ static struct mavlink_subscriptions {
|
||||
int spa_sub;
|
||||
int spl_sub;
|
||||
int spg_sub;
|
||||
int debug_key_value;
|
||||
bool initialized;
|
||||
} mavlink_subs = {
|
||||
.sensor_sub = 0,
|
||||
@@ -183,6 +185,7 @@ static struct mavlink_subscriptions {
|
||||
.spa_sub = 0,
|
||||
.spl_sub = 0,
|
||||
.spg_sub = 0,
|
||||
.debug_key_value = 0,
|
||||
.initialized = false
|
||||
};
|
||||
|
||||
@@ -543,6 +546,10 @@ static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int ma
|
||||
case MAVLINK_MSG_ID_MANUAL_CONTROL:
|
||||
/* manual_control_setpoint triggers this message */
|
||||
if (subs->man_control_sp_sub) orb_set_interval(subs->man_control_sp_sub, min_interval);
|
||||
break;
|
||||
case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
|
||||
if (subs->debug_key_value) orb_set_interval(subs->debug_key_value, min_interval);
|
||||
break;
|
||||
default:
|
||||
/* not found */
|
||||
ret = ERROR;
|
||||
@@ -588,6 +595,7 @@ static void *uorb_receiveloop(void *arg)
|
||||
struct actuator_outputs_s act_outputs;
|
||||
struct manual_control_setpoint_s man_control;
|
||||
struct actuator_controls_s actuators;
|
||||
struct debug_key_value_s debug;
|
||||
} buf;
|
||||
|
||||
/* --- SENSORS RAW VALUE --- */
|
||||
@@ -709,6 +717,13 @@ static void *uorb_receiveloop(void *arg)
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* --- DEBUG VALUE OUTPUT --- */
|
||||
/* subscribe to ORB for debug value outputs */
|
||||
subs->debug_key_value = orb_subscribe(ORB_ID(debug_key_value));
|
||||
fds[fdsc_count].fd = subs->debug_key_value;
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* all subscriptions initialized, return success */
|
||||
subs->initialized = true;
|
||||
|
||||
@@ -1060,10 +1075,17 @@ static void *uorb_receiveloop(void *arg)
|
||||
/* --- ACTUATOR CONTROL --- */
|
||||
if (fds[ifds++].revents & POLLIN) {
|
||||
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs->actuators_sub, &buf.actuators);
|
||||
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl0", buf.actuators.control[0]);
|
||||
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl1", buf.actuators.control[1]);
|
||||
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl2", buf.actuators.control[2]);
|
||||
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl3", buf.actuators.control[3]);
|
||||
/* send, add spaces so that string buffer is at least 10 chars long */
|
||||
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl0 ", buf.actuators.control[0]);
|
||||
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl1 ", buf.actuators.control[1]);
|
||||
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl2 ", buf.actuators.control[2]);
|
||||
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl3 ", buf.actuators.control[3]);
|
||||
}
|
||||
|
||||
/* --- DEBUG KEY/VALUE --- */
|
||||
if (fds[ifds++].revents & POLLIN) {
|
||||
orb_copy(ORB_ID(debug_key_value), subs->debug_key_value, &buf.debug);
|
||||
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, buf.debug.key, buf.debug.value);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1501,6 +1523,7 @@ int mavlink_thread_main(int argc, char *argv[])
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 2);
|
||||
/* 200 Hz / 5 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 5);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 5);
|
||||
/* 5 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200);
|
||||
} else if (baudrate >= 460800) {
|
||||
@@ -1508,15 +1531,19 @@ int mavlink_thread_main(int argc, char *argv[])
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 5);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 5);
|
||||
/* 100 Hz / 10 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
|
||||
/* 50 Hz / 20 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 20);
|
||||
/* 2 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
|
||||
} else if (baudrate >= 115200) {
|
||||
/* 50 Hz / 20 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 50);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 20);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 20);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
|
||||
/* 20 Hz / 50 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
|
||||
/* 10 Hz / 100 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 100);
|
||||
/* 1 Hz */
|
||||
@@ -1527,6 +1554,8 @@ int mavlink_thread_main(int argc, char *argv[])
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 100);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 100);
|
||||
/* 5 Hz / 200 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
|
||||
/* 5 Hz / 200 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
|
||||
/* 0.2 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 5000);
|
||||
@@ -1535,6 +1564,8 @@ int mavlink_thread_main(int argc, char *argv[])
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 1000);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
|
||||
/* 1 Hz / 1000 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000);
|
||||
/* 0.5 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
|
||||
/* 0.1 Hz */
|
||||
|
||||
@@ -123,3 +123,6 @@ ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s);
|
||||
ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
|
||||
ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
|
||||
ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
|
||||
|
||||
#include "topics/debug_key_value.h"
|
||||
ORB_DEFINE(debug_key_value, struct debug_key_value_s);
|
||||
|
||||
@@ -67,7 +67,8 @@ ORB_DECLARE(actuator_controls_3);
|
||||
|
||||
/** global 'actuator output is live' control. */
|
||||
struct actuator_armed_s {
|
||||
bool armed;
|
||||
bool armed; /**< Set to true if system is armed */
|
||||
bool failsafe; /**< Set to true if no valid control input is available */
|
||||
};
|
||||
|
||||
ORB_DECLARE(actuator_armed);
|
||||
|
||||
@@ -0,0 +1,75 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file debug_key_value.h
|
||||
* Definition of the debug named value uORB topic. Allows to send a 10-char key
|
||||
* with a float as value.
|
||||
*/
|
||||
|
||||
#ifndef TOPIC_DEBUG_KEY_VALUE_H_
|
||||
#define TOPIC_DEBUG_KEY_VALUE_H_
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
*/
|
||||
|
||||
/**
|
||||
* Key/value pair for debugging
|
||||
*/
|
||||
struct debug_key_value_s {
|
||||
|
||||
/*
|
||||
* Actual data, this is specific to the type of data which is stored in this struct
|
||||
* A line containing L0GME will be added by the Python logging code generator to the
|
||||
* logged dataset.
|
||||
*/
|
||||
uint32_t timestamp_ms; /**< in milliseconds since system start */
|
||||
char key[10]; /**< max. 10 characters as key / name */
|
||||
float value; /**< the value to send as debug output */
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(debug_key_value);
|
||||
|
||||
#endif
|
||||
@@ -16,7 +16,7 @@ extern "C" {
|
||||
#endif
|
||||
|
||||
#ifndef MAVLINK_MESSAGE_CRCS
|
||||
#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 106, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 78, 68, 189, 127, 111, 21, 21, 144, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
|
||||
#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 148, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 78, 68, 189, 127, 111, 21, 21, 144, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
|
||||
#endif
|
||||
|
||||
#ifndef MAVLINK_MESSAGE_INFO
|
||||
|
||||
@@ -5,7 +5,7 @@
|
||||
#ifndef MAVLINK_VERSION_H
|
||||
#define MAVLINK_VERSION_H
|
||||
|
||||
#define MAVLINK_BUILD_DATE "Mon Sep 3 14:55:54 2012"
|
||||
#define MAVLINK_BUILD_DATE "Wed Sep 5 11:04:20 2012"
|
||||
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
|
||||
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101
|
||||
|
||||
|
||||
@@ -16,7 +16,7 @@ extern "C" {
|
||||
#endif
|
||||
|
||||
#ifndef MAVLINK_MESSAGE_CRCS
|
||||
#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 106, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
|
||||
#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 148, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
|
||||
#endif
|
||||
|
||||
#ifndef MAVLINK_MESSAGE_INFO
|
||||
|
||||
@@ -4,7 +4,7 @@
|
||||
|
||||
typedef struct __mavlink_global_vision_position_estimate_t
|
||||
{
|
||||
uint64_t usec; ///< Timestamp (milliseconds)
|
||||
uint64_t usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
float x; ///< Global X position
|
||||
float y; ///< Global Y position
|
||||
float z; ///< Global Z position
|
||||
@@ -38,7 +38,7 @@ typedef struct __mavlink_global_vision_position_estimate_t
|
||||
* @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 usec Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
* @param x Global X position
|
||||
* @param y Global Y position
|
||||
* @param z Global Z position
|
||||
@@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack(uint8_t
|
||||
* @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 usec Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
* @param x Global X position
|
||||
* @param y Global Y position
|
||||
* @param z Global Z position
|
||||
@@ -142,7 +142,7 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_encode(uint8_
|
||||
* @brief Send a global_vision_position_estimate message
|
||||
* @param chan MAVLink channel to send the message
|
||||
*
|
||||
* @param usec Timestamp (milliseconds)
|
||||
* @param usec Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
* @param x Global X position
|
||||
* @param y Global Y position
|
||||
* @param z Global Z position
|
||||
@@ -187,7 +187,7 @@ static inline void mavlink_msg_global_vision_position_estimate_send(mavlink_chan
|
||||
/**
|
||||
* @brief Get field usec from global_vision_position_estimate message
|
||||
*
|
||||
* @return Timestamp (milliseconds)
|
||||
* @return Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
*/
|
||||
static inline uint64_t mavlink_msg_global_vision_position_estimate_get_usec(const mavlink_message_t* msg)
|
||||
{
|
||||
|
||||
@@ -4,7 +4,7 @@
|
||||
|
||||
typedef struct __mavlink_highres_imu_t
|
||||
{
|
||||
uint64_t time_boot_ms; ///< Timestamp (milliseconds since system boot)
|
||||
uint64_t time_usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
float xacc; ///< X acceleration (m/s^2)
|
||||
float yacc; ///< Y acceleration (m/s^2)
|
||||
float zacc; ///< Z acceleration (m/s^2)
|
||||
@@ -14,8 +14,8 @@ typedef struct __mavlink_highres_imu_t
|
||||
float xmag; ///< X Magnetic field (Gauss)
|
||||
float ymag; ///< Y Magnetic field (Gauss)
|
||||
float zmag; ///< Z Magnetic field (Gauss)
|
||||
float abs_pressure; ///< Absolute pressure in hectopascal
|
||||
float diff_pressure; ///< Differential pressure in hectopascal
|
||||
float abs_pressure; ///< Absolute pressure in millibar
|
||||
float diff_pressure; ///< Differential pressure in millibar
|
||||
float pressure_alt; ///< Altitude calculated from pressure
|
||||
float temperature; ///< Temperature in degrees celsius
|
||||
} mavlink_highres_imu_t;
|
||||
@@ -28,7 +28,7 @@ typedef struct __mavlink_highres_imu_t
|
||||
#define MAVLINK_MESSAGE_INFO_HIGHRES_IMU { \
|
||||
"HIGHRES_IMU", \
|
||||
14, \
|
||||
{ { "time_boot_ms", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_highres_imu_t, time_boot_ms) }, \
|
||||
{ { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_highres_imu_t, time_usec) }, \
|
||||
{ "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_highres_imu_t, xacc) }, \
|
||||
{ "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_highres_imu_t, yacc) }, \
|
||||
{ "zacc", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_highres_imu_t, zacc) }, \
|
||||
@@ -52,7 +52,7 @@ typedef struct __mavlink_highres_imu_t
|
||||
* @param component_id ID of this component (e.g. 200 for IMU)
|
||||
* @param msg The MAVLink message to compress the data into
|
||||
*
|
||||
* @param time_boot_ms Timestamp (milliseconds since system boot)
|
||||
* @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
* @param xacc X acceleration (m/s^2)
|
||||
* @param yacc Y acceleration (m/s^2)
|
||||
* @param zacc Z acceleration (m/s^2)
|
||||
@@ -62,18 +62,18 @@ typedef struct __mavlink_highres_imu_t
|
||||
* @param xmag X Magnetic field (Gauss)
|
||||
* @param ymag Y Magnetic field (Gauss)
|
||||
* @param zmag Z Magnetic field (Gauss)
|
||||
* @param abs_pressure Absolute pressure in hectopascal
|
||||
* @param diff_pressure Differential pressure in hectopascal
|
||||
* @param abs_pressure Absolute pressure in millibar
|
||||
* @param diff_pressure Differential pressure in millibar
|
||||
* @param pressure_alt Altitude calculated from pressure
|
||||
* @param temperature Temperature in degrees celsius
|
||||
* @return length of the message in bytes (excluding serial stream start sign)
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
|
||||
uint64_t time_boot_ms, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature)
|
||||
uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature)
|
||||
{
|
||||
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
||||
char buf[60];
|
||||
_mav_put_uint64_t(buf, 0, time_boot_ms);
|
||||
_mav_put_uint64_t(buf, 0, time_usec);
|
||||
_mav_put_float(buf, 8, xacc);
|
||||
_mav_put_float(buf, 12, yacc);
|
||||
_mav_put_float(buf, 16, zacc);
|
||||
@@ -91,7 +91,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t c
|
||||
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 60);
|
||||
#else
|
||||
mavlink_highres_imu_t packet;
|
||||
packet.time_boot_ms = time_boot_ms;
|
||||
packet.time_usec = time_usec;
|
||||
packet.xacc = xacc;
|
||||
packet.yacc = yacc;
|
||||
packet.zacc = zacc;
|
||||
@@ -110,7 +110,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t c
|
||||
#endif
|
||||
|
||||
msg->msgid = MAVLINK_MSG_ID_HIGHRES_IMU;
|
||||
return mavlink_finalize_message(msg, system_id, component_id, 60, 106);
|
||||
return mavlink_finalize_message(msg, system_id, component_id, 60, 148);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -119,7 +119,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t c
|
||||
* @param component_id ID of this component (e.g. 200 for IMU)
|
||||
* @param chan The MAVLink channel this message was sent over
|
||||
* @param msg The MAVLink message to compress the data into
|
||||
* @param time_boot_ms Timestamp (milliseconds since system boot)
|
||||
* @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
* @param xacc X acceleration (m/s^2)
|
||||
* @param yacc Y acceleration (m/s^2)
|
||||
* @param zacc Z acceleration (m/s^2)
|
||||
@@ -129,19 +129,19 @@ static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t c
|
||||
* @param xmag X Magnetic field (Gauss)
|
||||
* @param ymag Y Magnetic field (Gauss)
|
||||
* @param zmag Z Magnetic field (Gauss)
|
||||
* @param abs_pressure Absolute pressure in hectopascal
|
||||
* @param diff_pressure Differential pressure in hectopascal
|
||||
* @param abs_pressure Absolute pressure in millibar
|
||||
* @param diff_pressure Differential pressure in millibar
|
||||
* @param pressure_alt Altitude calculated from pressure
|
||||
* @param temperature Temperature in degrees celsius
|
||||
* @return length of the message in bytes (excluding serial stream start sign)
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_highres_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
|
||||
mavlink_message_t* msg,
|
||||
uint64_t time_boot_ms,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float xmag,float ymag,float zmag,float abs_pressure,float diff_pressure,float pressure_alt,float temperature)
|
||||
uint64_t time_usec,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float xmag,float ymag,float zmag,float abs_pressure,float diff_pressure,float pressure_alt,float temperature)
|
||||
{
|
||||
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
||||
char buf[60];
|
||||
_mav_put_uint64_t(buf, 0, time_boot_ms);
|
||||
_mav_put_uint64_t(buf, 0, time_usec);
|
||||
_mav_put_float(buf, 8, xacc);
|
||||
_mav_put_float(buf, 12, yacc);
|
||||
_mav_put_float(buf, 16, zacc);
|
||||
@@ -159,7 +159,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack_chan(uint8_t system_id, uint
|
||||
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 60);
|
||||
#else
|
||||
mavlink_highres_imu_t packet;
|
||||
packet.time_boot_ms = time_boot_ms;
|
||||
packet.time_usec = time_usec;
|
||||
packet.xacc = xacc;
|
||||
packet.yacc = yacc;
|
||||
packet.zacc = zacc;
|
||||
@@ -178,7 +178,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack_chan(uint8_t system_id, uint
|
||||
#endif
|
||||
|
||||
msg->msgid = MAVLINK_MSG_ID_HIGHRES_IMU;
|
||||
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 60, 106);
|
||||
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 60, 148);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -191,14 +191,14 @@ static inline uint16_t mavlink_msg_highres_imu_pack_chan(uint8_t system_id, uint
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_highres_imu_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_highres_imu_t* highres_imu)
|
||||
{
|
||||
return mavlink_msg_highres_imu_pack(system_id, component_id, msg, highres_imu->time_boot_ms, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature);
|
||||
return mavlink_msg_highres_imu_pack(system_id, component_id, msg, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Send a highres_imu message
|
||||
* @param chan MAVLink channel to send the message
|
||||
*
|
||||
* @param time_boot_ms Timestamp (milliseconds since system boot)
|
||||
* @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
* @param xacc X acceleration (m/s^2)
|
||||
* @param yacc Y acceleration (m/s^2)
|
||||
* @param zacc Z acceleration (m/s^2)
|
||||
@@ -208,18 +208,18 @@ static inline uint16_t mavlink_msg_highres_imu_encode(uint8_t system_id, uint8_t
|
||||
* @param xmag X Magnetic field (Gauss)
|
||||
* @param ymag Y Magnetic field (Gauss)
|
||||
* @param zmag Z Magnetic field (Gauss)
|
||||
* @param abs_pressure Absolute pressure in hectopascal
|
||||
* @param diff_pressure Differential pressure in hectopascal
|
||||
* @param abs_pressure Absolute pressure in millibar
|
||||
* @param diff_pressure Differential pressure in millibar
|
||||
* @param pressure_alt Altitude calculated from pressure
|
||||
* @param temperature Temperature in degrees celsius
|
||||
*/
|
||||
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
||||
|
||||
static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t time_boot_ms, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature)
|
||||
static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature)
|
||||
{
|
||||
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
||||
char buf[60];
|
||||
_mav_put_uint64_t(buf, 0, time_boot_ms);
|
||||
_mav_put_uint64_t(buf, 0, time_usec);
|
||||
_mav_put_float(buf, 8, xacc);
|
||||
_mav_put_float(buf, 12, yacc);
|
||||
_mav_put_float(buf, 16, zacc);
|
||||
@@ -234,10 +234,10 @@ static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t
|
||||
_mav_put_float(buf, 52, pressure_alt);
|
||||
_mav_put_float(buf, 56, temperature);
|
||||
|
||||
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, 60, 106);
|
||||
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, 60, 148);
|
||||
#else
|
||||
mavlink_highres_imu_t packet;
|
||||
packet.time_boot_ms = time_boot_ms;
|
||||
packet.time_usec = time_usec;
|
||||
packet.xacc = xacc;
|
||||
packet.yacc = yacc;
|
||||
packet.zacc = zacc;
|
||||
@@ -252,7 +252,7 @@ static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t
|
||||
packet.pressure_alt = pressure_alt;
|
||||
packet.temperature = temperature;
|
||||
|
||||
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)&packet, 60, 106);
|
||||
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)&packet, 60, 148);
|
||||
#endif
|
||||
}
|
||||
|
||||
@@ -262,11 +262,11 @@ static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t
|
||||
|
||||
|
||||
/**
|
||||
* @brief Get field time_boot_ms from highres_imu message
|
||||
* @brief Get field time_usec from highres_imu message
|
||||
*
|
||||
* @return Timestamp (milliseconds since system boot)
|
||||
* @return Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
*/
|
||||
static inline uint64_t mavlink_msg_highres_imu_get_time_boot_ms(const mavlink_message_t* msg)
|
||||
static inline uint64_t mavlink_msg_highres_imu_get_time_usec(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_uint64_t(msg, 0);
|
||||
}
|
||||
@@ -364,7 +364,7 @@ static inline float mavlink_msg_highres_imu_get_zmag(const mavlink_message_t* ms
|
||||
/**
|
||||
* @brief Get field abs_pressure from highres_imu message
|
||||
*
|
||||
* @return Absolute pressure in hectopascal
|
||||
* @return Absolute pressure in millibar
|
||||
*/
|
||||
static inline float mavlink_msg_highres_imu_get_abs_pressure(const mavlink_message_t* msg)
|
||||
{
|
||||
@@ -374,7 +374,7 @@ static inline float mavlink_msg_highres_imu_get_abs_pressure(const mavlink_messa
|
||||
/**
|
||||
* @brief Get field diff_pressure from highres_imu message
|
||||
*
|
||||
* @return Differential pressure in hectopascal
|
||||
* @return Differential pressure in millibar
|
||||
*/
|
||||
static inline float mavlink_msg_highres_imu_get_diff_pressure(const mavlink_message_t* msg)
|
||||
{
|
||||
@@ -410,7 +410,7 @@ static inline float mavlink_msg_highres_imu_get_temperature(const mavlink_messag
|
||||
static inline void mavlink_msg_highres_imu_decode(const mavlink_message_t* msg, mavlink_highres_imu_t* highres_imu)
|
||||
{
|
||||
#if MAVLINK_NEED_BYTE_SWAP
|
||||
highres_imu->time_boot_ms = mavlink_msg_highres_imu_get_time_boot_ms(msg);
|
||||
highres_imu->time_usec = mavlink_msg_highres_imu_get_time_usec(msg);
|
||||
highres_imu->xacc = mavlink_msg_highres_imu_get_xacc(msg);
|
||||
highres_imu->yacc = mavlink_msg_highres_imu_get_yacc(msg);
|
||||
highres_imu->zacc = mavlink_msg_highres_imu_get_zacc(msg);
|
||||
|
||||
@@ -4,7 +4,7 @@
|
||||
|
||||
typedef struct __mavlink_vicon_position_estimate_t
|
||||
{
|
||||
uint64_t usec; ///< Timestamp (milliseconds)
|
||||
uint64_t usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
float x; ///< Global X position
|
||||
float y; ///< Global Y position
|
||||
float z; ///< Global Z position
|
||||
@@ -38,7 +38,7 @@ typedef struct __mavlink_vicon_position_estimate_t
|
||||
* @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 usec Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
* @param x Global X position
|
||||
* @param y Global Y position
|
||||
* @param z Global Z position
|
||||
@@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_i
|
||||
* @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 usec Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
* @param x Global X position
|
||||
* @param y Global Y position
|
||||
* @param z Global Z position
|
||||
@@ -142,7 +142,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system
|
||||
* @brief Send a vicon_position_estimate message
|
||||
* @param chan MAVLink channel to send the message
|
||||
*
|
||||
* @param usec Timestamp (milliseconds)
|
||||
* @param usec Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
* @param x Global X position
|
||||
* @param y Global Y position
|
||||
* @param z Global Z position
|
||||
@@ -187,7 +187,7 @@ static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t ch
|
||||
/**
|
||||
* @brief Get field usec from vicon_position_estimate message
|
||||
*
|
||||
* @return Timestamp (milliseconds)
|
||||
* @return Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
*/
|
||||
static inline uint64_t mavlink_msg_vicon_position_estimate_get_usec(const mavlink_message_t* msg)
|
||||
{
|
||||
|
||||
@@ -4,7 +4,7 @@
|
||||
|
||||
typedef struct __mavlink_vision_position_estimate_t
|
||||
{
|
||||
uint64_t usec; ///< Timestamp (milliseconds)
|
||||
uint64_t usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
float x; ///< Global X position
|
||||
float y; ///< Global Y position
|
||||
float z; ///< Global Z position
|
||||
@@ -38,7 +38,7 @@ typedef struct __mavlink_vision_position_estimate_t
|
||||
* @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 usec Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
* @param x Global X position
|
||||
* @param y Global Y position
|
||||
* @param z Global Z position
|
||||
@@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t 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 usec Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
* @param x Global X position
|
||||
* @param y Global Y position
|
||||
* @param z Global Z position
|
||||
@@ -142,7 +142,7 @@ static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t syste
|
||||
* @brief Send a vision_position_estimate message
|
||||
* @param chan MAVLink channel to send the message
|
||||
*
|
||||
* @param usec Timestamp (milliseconds)
|
||||
* @param usec Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
* @param x Global X position
|
||||
* @param y Global Y position
|
||||
* @param z Global Z position
|
||||
@@ -187,7 +187,7 @@ static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t c
|
||||
/**
|
||||
* @brief Get field usec from vision_position_estimate message
|
||||
*
|
||||
* @return Timestamp (milliseconds)
|
||||
* @return Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
*/
|
||||
static inline uint64_t mavlink_msg_vision_position_estimate_get_usec(const mavlink_message_t* msg)
|
||||
{
|
||||
|
||||
@@ -4,7 +4,7 @@
|
||||
|
||||
typedef struct __mavlink_vision_speed_estimate_t
|
||||
{
|
||||
uint64_t usec; ///< Timestamp (milliseconds)
|
||||
uint64_t usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
float x; ///< Global X speed
|
||||
float y; ///< Global Y speed
|
||||
float z; ///< Global Z speed
|
||||
@@ -32,7 +32,7 @@ typedef struct __mavlink_vision_speed_estimate_t
|
||||
* @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 usec Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
* @param x Global X speed
|
||||
* @param y Global Y speed
|
||||
* @param z Global Z speed
|
||||
@@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id,
|
||||
* @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 usec Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
* @param x Global X speed
|
||||
* @param y Global Y speed
|
||||
* @param z Global Z speed
|
||||
@@ -118,7 +118,7 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_i
|
||||
* @brief Send a vision_speed_estimate message
|
||||
* @param chan MAVLink channel to send the message
|
||||
*
|
||||
* @param usec Timestamp (milliseconds)
|
||||
* @param usec Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
* @param x Global X speed
|
||||
* @param y Global Y speed
|
||||
* @param z Global Z speed
|
||||
@@ -154,7 +154,7 @@ static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan
|
||||
/**
|
||||
* @brief Get field usec from vision_speed_estimate message
|
||||
*
|
||||
* @return Timestamp (milliseconds)
|
||||
* @return Timestamp (microseconds, synced to UNIX time or since system boot)
|
||||
*/
|
||||
static inline uint64_t mavlink_msg_vision_speed_estimate_get_usec(const mavlink_message_t* msg)
|
||||
{
|
||||
|
||||
@@ -3738,7 +3738,7 @@ static void mavlink_test_highres_imu(uint8_t system_id, uint8_t component_id, ma
|
||||
};
|
||||
mavlink_highres_imu_t packet1, packet2;
|
||||
memset(&packet1, 0, sizeof(packet1));
|
||||
packet1.time_boot_ms = packet_in.time_boot_ms;
|
||||
packet1.time_usec = packet_in.time_usec;
|
||||
packet1.xacc = packet_in.xacc;
|
||||
packet1.yacc = packet_in.yacc;
|
||||
packet1.zacc = packet_in.zacc;
|
||||
@@ -3761,12 +3761,12 @@ static void mavlink_test_highres_imu(uint8_t system_id, uint8_t component_id, ma
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_highres_imu_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature );
|
||||
mavlink_msg_highres_imu_pack(system_id, component_id, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature );
|
||||
mavlink_msg_highres_imu_decode(&msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_highres_imu_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature );
|
||||
mavlink_msg_highres_imu_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature );
|
||||
mavlink_msg_highres_imu_decode(&msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
@@ -3779,7 +3779,7 @@ static void mavlink_test_highres_imu(uint8_t system_id, uint8_t component_id, ma
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_highres_imu_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature );
|
||||
mavlink_msg_highres_imu_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature );
|
||||
mavlink_msg_highres_imu_decode(last_msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
}
|
||||
|
||||
@@ -5,7 +5,7 @@
|
||||
#ifndef MAVLINK_VERSION_H
|
||||
#define MAVLINK_VERSION_H
|
||||
|
||||
#define MAVLINK_BUILD_DATE "Mon Sep 3 14:56:15 2012"
|
||||
#define MAVLINK_BUILD_DATE "Wed Sep 5 11:04:39 2012"
|
||||
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
|
||||
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101
|
||||
|
||||
|
||||
@@ -16,7 +16,7 @@ extern "C" {
|
||||
#endif
|
||||
|
||||
#ifndef MAVLINK_MESSAGE_CRCS
|
||||
#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 106, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 0, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 249, 182, 0, 0, 0, 0, 0, 0, 0, 153, 16, 29, 162, 0, 0, 0, 0, 0, 0, 90, 95, 36, 23, 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, 0}
|
||||
#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 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, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 148, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 0, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 249, 182, 0, 0, 0, 0, 0, 0, 0, 153, 16, 29, 162, 0, 0, 0, 0, 0, 0, 90, 95, 36, 23, 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, 0}
|
||||
#endif
|
||||
|
||||
#ifndef MAVLINK_MESSAGE_INFO
|
||||
|
||||
@@ -5,7 +5,7 @@
|
||||
#ifndef MAVLINK_VERSION_H
|
||||
#define MAVLINK_VERSION_H
|
||||
|
||||
#define MAVLINK_BUILD_DATE "Mon Sep 3 14:56:05 2012"
|
||||
#define MAVLINK_BUILD_DATE "Wed Sep 5 11:04:29 2012"
|
||||
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
|
||||
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
|
||||
|
||||
|
||||
@@ -16,7 +16,7 @@ extern "C" {
|
||||
#endif
|
||||
|
||||
#ifndef MAVLINK_MESSAGE_CRCS
|
||||
#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 106, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 15, 0, 108, 0, 146, 0, 16, 0, 32, 0, 159, 0, 24, 248, 79, 0, 5, 0, 170, 0, 157, 0, 209, 0, 243, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
|
||||
#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 148, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 15, 0, 108, 0, 146, 0, 16, 0, 32, 0, 159, 0, 24, 248, 79, 0, 5, 0, 170, 0, 157, 0, 209, 0, 243, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
|
||||
#endif
|
||||
|
||||
#ifndef MAVLINK_MESSAGE_INFO
|
||||
|
||||
@@ -5,7 +5,7 @@
|
||||
#ifndef MAVLINK_VERSION_H
|
||||
#define MAVLINK_VERSION_H
|
||||
|
||||
#define MAVLINK_BUILD_DATE "Mon Sep 3 14:56:15 2012"
|
||||
#define MAVLINK_BUILD_DATE "Wed Sep 5 11:04:39 2012"
|
||||
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
|
||||
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101
|
||||
|
||||
|
||||
Reference in New Issue
Block a user