Browse Source

extended uORB structs with VTOL specific control topics

sbg
Roman Bapst 10 years ago
parent
commit
35c985e523
  1. 10
      src/modules/uORB/objects_common.cpp
  2. 3
      src/modules/uORB/topics/actuator_controls.h
  3. 17
      src/modules/uORB/topics/vehicle_attitude.h
  4. 2
      src/modules/uORB/topics/vehicle_attitude_setpoint.h
  5. 3
      src/modules/uORB/topics/vehicle_rates_setpoint.h
  6. 5
      src/modules/uORB/topics/vehicle_status.h
  7. 66
      src/modules/uORB/topics/vtol_vehicle_status.h

10
src/modules/uORB/objects_common.cpp

@ -91,6 +91,9 @@ ORB_DEFINE(home_position, struct home_position_s); @@ -91,6 +91,9 @@ ORB_DEFINE(home_position, struct home_position_s);
#include "topics/vehicle_status.h"
ORB_DEFINE(vehicle_status, struct vehicle_status_s);
#include "topics/vtol_vehicle_status.h"
ORB_DEFINE(vtol_vehicle_status, struct vtol_vehicle_status_s);
#include "topics/safety.h"
ORB_DEFINE(safety, struct safety_s);
@ -114,6 +117,8 @@ ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s); @@ -114,6 +117,8 @@ ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s);
#include "topics/vehicle_rates_setpoint.h"
ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s);
ORB_DEFINE(mc_virtual_rates_setpoint, struct vehicle_rates_setpoint_s);
ORB_DEFINE(fw_virtual_rates_setpoint, struct vehicle_rates_setpoint_s);
#include "topics/rc_channels.h"
ORB_DEFINE(rc_channels, struct rc_channels_s);
@ -148,6 +153,8 @@ ORB_DEFINE(fence, unsigned); @@ -148,6 +153,8 @@ ORB_DEFINE(fence, unsigned);
#include "topics/vehicle_attitude_setpoint.h"
ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
ORB_DEFINE(mc_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s);
ORB_DEFINE(fw_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s);
#include "topics/manual_control_setpoint.h"
ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
@ -182,6 +189,9 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s); @@ -182,6 +189,9 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
//Virtual control groups, used for VTOL operation
ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_s);
#include "topics/actuator_armed.h"
ORB_DEFINE(actuator_armed, struct actuator_armed_s);

3
src/modules/uORB/topics/actuator_controls.h

@ -74,5 +74,8 @@ ORB_DECLARE(actuator_controls_0); @@ -74,5 +74,8 @@ ORB_DECLARE(actuator_controls_0);
ORB_DECLARE(actuator_controls_1);
ORB_DECLARE(actuator_controls_2);
ORB_DECLARE(actuator_controls_3);
ORB_DECLARE(actuator_controls_virtual_mc);
ORB_DECLARE(actuator_controls_virtual_fw);
#endif

17
src/modules/uORB/topics/vehicle_attitude.h

@ -80,6 +80,23 @@ struct vehicle_attitude_s { @@ -80,6 +80,23 @@ struct vehicle_attitude_s {
bool R_valid; /**< Rotation matrix valid */
bool q_valid; /**< Quaternion valid */
// secondary attitude, use for VTOL
float roll_sec; /**< Roll angle (rad, Tait-Bryan, NED) */
float pitch_sec; /**< Pitch angle (rad, Tait-Bryan, NED) */
float yaw_sec; /**< Yaw angle (rad, Tait-Bryan, NED) */
float rollspeed_sec; /**< Roll angular speed (rad/s, Tait-Bryan, NED) */
float pitchspeed_sec; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) */
float yawspeed_sec; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) */
float rollacc_sec; /**< Roll angular accelration (rad/s, Tait-Bryan, NED) */
float pitchacc_sec; /**< Pitch angular acceleration (rad/s, Tait-Bryan, NED) */
float yawacc_sec; /**< Yaw angular acceleration (rad/s, Tait-Bryan, NED) */
float rate_offsets_sec[3]; /**< Offsets of the body angular rates from zero */
float R_sec[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */
float q_sec[4]; /**< Quaternion (NED) */
float g_comp_sec[3]; /**< Compensated gravity vector */
bool R_valid_sec; /**< Rotation matrix valid */
bool q_valid_sec; /**< Quaternion valid */
};
/**

2
src/modules/uORB/topics/vehicle_attitude_setpoint.h

@ -83,5 +83,7 @@ struct vehicle_attitude_setpoint_s { @@ -83,5 +83,7 @@ struct vehicle_attitude_setpoint_s {
/* register this as object request broker structure */
ORB_DECLARE(vehicle_attitude_setpoint);
ORB_DECLARE(mc_virtual_attitude_setpoint);
ORB_DECLARE(fw_virtual_attitude_setpoint);
#endif /* TOPIC_ARDRONE_CONTROL_H_ */

3
src/modules/uORB/topics/vehicle_rates_setpoint.h

@ -63,5 +63,6 @@ struct vehicle_rates_setpoint_s { @@ -63,5 +63,6 @@ struct vehicle_rates_setpoint_s {
/* register this as object request broker structure */
ORB_DECLARE(vehicle_rates_setpoint);
ORB_DECLARE(mc_virtual_rates_setpoint);
ORB_DECLARE(fw_virtual_rates_setpoint);
#endif

5
src/modules/uORB/topics/vehicle_status.h

@ -147,7 +147,10 @@ enum VEHICLE_TYPE { @@ -147,7 +147,10 @@ enum VEHICLE_TYPE {
VEHICLE_TYPE_TRICOPTER = 15, /* Octorotor | */
VEHICLE_TYPE_FLAPPING_WING = 16, /* Flapping wing | */
VEHICLE_TYPE_KITE = 17, /* Kite | */
VEHICLE_TYPE_ENUM_END = 18, /* | */
VEHICLE_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */
VEHICLE_TYPE_VTOL_DUOROTOR = 19, /* Vtol with two engines */
VEHICLE_TYPE_VTOL_QUADROTOR = 20, /* Vtol with four engines*/
VEHICLE_TYPE_ENUM_END = 21 /* | */
};
enum VEHICLE_BATTERY_WARNING {

66
src/modules/uORB/topics/vtol_vehicle_status.h

@ -0,0 +1,66 @@ @@ -0,0 +1,66 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name 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 vtol_status.h
*
* Vtol status topic
*
*/
#ifndef TOPIC_VTOL_STATUS_H
#define TOPIC_VTOL_STATUS_H
#include <stdint.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
/* Indicates in which mode the vtol aircraft is in */
struct vtol_vehicle_status_s {
uint64_t timestamp; /**< Microseconds since system boot */
bool vtol_in_rw_mode; /*true: vtol vehicle is in rotating wing mode */
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(vtol_vehicle_status);
#endif
Loading…
Cancel
Save