uORB combine VTOL FW and MC virtual topics (#7008)

This commit is contained in:
Daniel Agar 2017-04-09 13:17:20 -04:00 committed by GitHub
parent 19f0b0be3a
commit c5e0bf1c2f
17 changed files with 74 additions and 263 deletions

View File

@ -41,8 +41,8 @@ set(msg_file_names
att_pos_mocap.msg
battery_status.msg
camera_trigger.msg
commander_state.msg
collision_report.msg
commander_state.msg
control_state.msg
cpuload.msg
debug_key_value.msg
@ -59,8 +59,6 @@ set(msg_file_names
filtered_bottom_flow.msg
follow_target.msg
fw_pos_ctrl_status.msg
fw_virtual_attitude_setpoint.msg
fw_virtual_rates_setpoint.msg
geofence_result.msg
gps_dump.msg
gps_inject_data.msg
@ -72,8 +70,6 @@ set(msg_file_names
manual_control_setpoint.msg
mavlink_log.msg
mc_att_ctrl_status.msg
mc_virtual_attitude_setpoint.msg
mc_virtual_rates_setpoint.msg
mission.msg
mission_result.msg
mount_orientation.msg
@ -112,8 +108,8 @@ set(msg_file_names
ulog_stream_ack.msg
vehicle_attitude.msg
vehicle_attitude_setpoint.msg
vehicle_command_ack.msg
vehicle_command.msg
vehicle_command_ack.msg
vehicle_control_mode.msg
vehicle_force_setpoint.msg
vehicle_global_position.msg

View File

@ -1,36 +0,0 @@
###############################################################################################
# The vehicle_attitude_setpoint.msg needs to be in sync with the virtual setpoint messages
#
# Please keep the following messages identical;
# vehicle_attitude_setpoint.msg
# mc_virtual_attitude_setpoint.msg
# fw_virtual_attitude_setpoint.msg
#
###############################################################################################
float32 roll_body # body angle in NED frame
float32 pitch_body # body angle in NED frame
float32 yaw_body # body angle in NED frame
float32 yaw_sp_move_rate # rad/s (commanded by user)
# For quaternion-based attitude control
float32[4] q_d # Desired quaternion for quaternion control
bool q_d_valid # Set to true if quaternion vector is valid
float32 thrust # Thrust in Newton the power system should generate
bool roll_reset_integral # Reset roll integral part (navigation logic change)
bool pitch_reset_integral # Reset pitch integral part (navigation logic change)
bool yaw_reset_integral # Reset yaw integral part (navigation logic change)
bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway)
bool disable_mc_yaw_control # control yaw for mc (used for vtol weather-vane mode)
bool apply_flaps
float32 landing_gear
# WAS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint
# TOPICS fw_virtual_attitude_setpoint

View File

@ -1,15 +0,0 @@
###############################################################################################
# The vehicle_rates_setpoint.msg needs to be in sync with the virtual setpoint messages
#
# Please keep the following messages identical;
# vehicle_rates_setpoint.msg
# mc_virtual_rates_setpoint.msg
# fw_virtual_rates_setpoint.msg
#
###############################################################################################
float32 roll # body angular rates in NED frame
float32 pitch # body angular rates in NED frame
float32 yaw # body angular rates in NED frame
float32 thrust # thrust normalized to 0..1

View File

@ -1,36 +0,0 @@
###############################################################################################
# The vehicle_attitude_setpoint.msg needs to be in sync with the virtual setpoint messages
#
# Please keep the following messages identical;
# vehicle_attitude_setpoint.msg
# mc_virtual_attitude_setpoint.msg
# fw_virtual_attitude_setpoint.msg
#
###############################################################################################
float32 roll_body # body angle in NED frame
float32 pitch_body # body angle in NED frame
float32 yaw_body # body angle in NED frame
float32 yaw_sp_move_rate # rad/s (commanded by user)
# For quaternion-based attitude control
float32[4] q_d # Desired quaternion for quaternion control
bool q_d_valid # Set to true if quaternion vector is valid
float32 thrust # Thrust in Newton the power system should generate
bool roll_reset_integral # Reset roll integral part (navigation logic change)
bool pitch_reset_integral # Reset pitch integral part (navigation logic change)
bool yaw_reset_integral # Reset yaw integral part (navigation logic change)
bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway)
bool disable_mc_yaw_control # control yaw for mc (used for vtol weather-vane mode)
bool apply_flaps
float32 landing_gear
# WAS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint
# TOPICS mc_virtual_attitude_setpoint

View File

@ -1,15 +0,0 @@
###############################################################################################
# The vehicle_rates_setpoint.msg needs to be in sync with the virtual setpoint messages
#
# Please keep the following messages identical;
# vehicle_rates_setpoint.msg
# mc_virtual_rates_setpoint.msg
# fw_virtual_rates_setpoint.msg
#
###############################################################################################
float32 roll # body angular rates in NED frame
float32 pitch # body angular rates in NED frame
float32 yaw # body angular rates in NED frame
float32 thrust # thrust normalized to 0..1

View File

@ -4,5 +4,3 @@
px4/position_setpoint previous
px4/position_setpoint current
px4/position_setpoint next
uint8 nav_state # report the navigation state

View File

@ -1,13 +1,3 @@
###############################################################################################
# The vehicle_attitude_setpoint.msg needs to be in sync with the virtual setpoint messages
#
# Please keep the following messages identical;
# vehicle_attitude_setpoint.msg
# mc_virtual_attitude_setpoint.msg
# fw_virtual_attitude_setpoint.msg
#
###############################################################################################
float32 roll_body # body angle in NED frame
float32 pitch_body # body angle in NED frame
@ -32,5 +22,4 @@ bool apply_flaps
float32 landing_gear
# WAS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint
# TOPICS vehicle_attitude_setpoint
# TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint

View File

@ -1,15 +1,7 @@
###############################################################################################
# The vehicle_rates_setpoint.msg needs to be in sync with the virtual setpoint messages
#
# Please keep the following messages identical;
# vehicle_rates_setpoint.msg
# mc_virtual_rates_setpoint.msg
# fw_virtual_rates_setpoint.msg
#
###############################################################################################
float32 roll # body angular rates in NED frame
float32 pitch # body angular rates in NED frame
float32 yaw # body angular rates in NED frame
float32 thrust # thrust normalized to 0..1
# TOPICS vehicle_rates_setpoint mc_virtual_rates_setpoint fw_virtual_rates_setpoint

View File

@ -58,8 +58,6 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/fw_virtual_attitude_setpoint.h>
#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>

View File

@ -88,7 +88,6 @@
#include <systemlib/systemlib.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <uORB/topics/fw_virtual_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>

View File

@ -53,45 +53,35 @@
* If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
*/
#include <conversion/rotation.h>
#include <drivers/drv_hrt.h>
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <lib/tailsitter_recovery/tailsitter_recovery.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/multirotor_motor_limits.h>
#include <uORB/topics/mc_att_ctrl_status.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_correction.h>
#include <systemlib/param/param.h>
#include <px4_tasks.h>
#include <systemlib/circuit_breaker.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <systemlib/circuit_breaker.h>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <lib/tailsitter_recovery/tailsitter_recovery.h>
#include <conversion/rotation.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/mc_att_ctrl_status.h>
#include <uORB/topics/multirotor_motor_limits.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_correction.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/uORB.h>
/**
* Multicopter attitude control app start / stop handling function

View File

@ -53,36 +53,26 @@
#include <px4_defines.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/mc_virtual_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <float.h>
#include <systemlib/mavlink_log.h>
#include <mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <mathlib/mathlib.h>
#include <platforms/px4_defines.h>
#include <systemlib/mavlink_log.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>

View File

@ -43,46 +43,33 @@
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include "navigator.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <float.h>
#include <poll.h>
#include <time.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <cfloat>
#include <arch/board/board.h>
#include <dataman/dataman.h>
#include <drivers/device/device.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/mission.h>
#include <geo/geo.h>
#include <mathlib/mathlib.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_posix.h>
#include <px4_tasks.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/systemlib.h>
#include <uORB/topics/fence.h>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/vehicle_command.h>
#include <drivers/drv_baro.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <geo/geo.h>
#include <dataman/dataman.h>
#include <mathlib/mathlib.h>
#include <systemlib/mavlink_log.h>
#include "navigator.h"
#include <uORB/topics/vehicle_status.h>
#include <uORB/uORB.h>
/**
* navigator app start / stop handling function
@ -778,9 +765,6 @@ Navigator::status()
void
Navigator::publish_position_setpoint_triplet()
{
/* update navigation state */
_pos_sp_triplet.nav_state = _vstatus.nav_state;
/* do not publish an empty triplet */
if (!_pos_sp_triplet.current.valid) {
return;

View File

@ -57,18 +57,10 @@
#else
#include <sys/statfs.h>
#endif
#include <fcntl.h>
#include <errno.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#include <systemlib/err.h>
#include <unistd.h>
#include <drivers/drv_hrt.h>
#include <math.h>
#include <time.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
@ -1965,7 +1957,6 @@ int sdlog2_thread_main(int argc, char *argv[])
if (buf.triplet.current.valid) {
log_msg.msg_type = LOG_GPSP_MSG;
log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * (double)1e7);
log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * (double)1e7);
log_msg.body.log_GPSP.alt = buf.triplet.current.alt;

View File

@ -248,7 +248,6 @@ struct log_GPOS_s {
/* --- GPSP - GLOBAL POSITION SETPOINT --- */
#define LOG_GPSP_MSG 17
struct log_GPSP_s {
uint8_t nav_state;
int32_t lat;
int32_t lon;
float alt;
@ -693,7 +692,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "BffffffLLHhB", "ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"),
LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"),
LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
LOG_FORMAT(GPSP, "LLffBfbf", "Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
LOG_FORMAT(ESC, "HBBBHHffiffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
LOG_FORMAT(BATT, "fffffffB", "V,VFilt,C,CFilt,Discharged,Remaining,Scale,Warning"),

View File

@ -54,15 +54,6 @@
#include <px4_tasks.h>
#include <px4_posix.h>
#include <errno.h>
#include <fcntl.h>
#include <math.h>
#include <poll.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
@ -77,11 +68,7 @@
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/fw_virtual_attitude_setpoint.h>
#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/mc_virtual_attitude_setpoint.h>
#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/vehicle_attitude.h>
@ -115,11 +102,11 @@ public:
struct vehicle_attitude_s *get_att() {return &_v_att;}
struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;}
struct mc_virtual_attitude_setpoint_s *get_mc_virtual_att_sp() {return &_mc_virtual_att_sp;}
struct fw_virtual_attitude_setpoint_s *get_fw_virtual_att_sp() {return &_fw_virtual_att_sp;}
struct vehicle_attitude_setpoint_s *get_mc_virtual_att_sp() {return &_mc_virtual_att_sp;}
struct vehicle_attitude_setpoint_s *get_fw_virtual_att_sp() {return &_fw_virtual_att_sp;}
struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;}
struct mc_virtual_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;}
struct fw_virtual_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;}
struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;}
struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;}
struct manual_control_setpoint_s *get_manual_control_sp() {return &_manual_control_sp;}
struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;}
struct vtol_vehicle_status_s *get_vtol_vehicle_status() {return &_vtol_vehicle_status;}
@ -174,11 +161,11 @@ private:
//*******************data containers***********************************************************
struct vehicle_attitude_s _v_att; //vehicle attitude
struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint
struct mc_virtual_attitude_setpoint_s _mc_virtual_att_sp; // virtual mc attitude setpoint
struct fw_virtual_attitude_setpoint_s _fw_virtual_att_sp; // virtual fw attitude setpoint
struct vehicle_attitude_setpoint_s _mc_virtual_att_sp; // virtual mc attitude setpoint
struct vehicle_attitude_setpoint_s _fw_virtual_att_sp; // virtual fw attitude setpoint
struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint
struct mc_virtual_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint
struct fw_virtual_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint
struct vehicle_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint
struct vehicle_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint
struct manual_control_setpoint_s _manual_control_sp; //manual control setpoint
struct vehicle_control_mode_s _v_control_mode; //vehicle control mode
struct vtol_vehicle_status_s _vtol_vehicle_status;

View File

@ -152,11 +152,11 @@ protected:
struct vehicle_attitude_s *_v_att; //vehicle attitude
struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint
struct mc_virtual_attitude_setpoint_s *_mc_virtual_att_sp; // virtual mc attitude setpoint
struct fw_virtual_attitude_setpoint_s *_fw_virtual_att_sp; // virtual fw attitude setpoint
struct vehicle_attitude_setpoint_s *_mc_virtual_att_sp; // virtual mc attitude setpoint
struct vehicle_attitude_setpoint_s *_fw_virtual_att_sp; // virtual fw attitude setpoint
struct vehicle_rates_setpoint_s *_v_rates_sp; //vehicle rates setpoint
struct mc_virtual_rates_setpoint_s *_mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint
struct fw_virtual_rates_setpoint_s *_fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint
struct vehicle_rates_setpoint_s *_mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint
struct vehicle_rates_setpoint_s *_fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint
struct manual_control_setpoint_s *_manual_control_sp; //manual control setpoint
struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode
struct vtol_vehicle_status_s *_vtol_vehicle_status;
@ -180,7 +180,7 @@ protected:
float _mc_pitch_weight = 1.0f; // weight for multicopter attitude controller pitch output
float _mc_yaw_weight = 1.0f; // weight for multicopter attitude controller yaw output
float _mc_throttle_weight = 1.0f; // weight for multicopter throttle command. Used to avoid
// motors spinning up or cutting too fast whend doing transitions.
// motors spinning up or cutting too fast when doing transitions.
float _thrust_transition = 0.0f; // thrust value applied during a front transition (tailsitter & tiltrotor only)
bool _flag_was_in_trans_mode = false; // true if mode has just switched to transition