fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

move vehicle_attitude_setpoint to msg format

Browse Source
This commit is contained in:
Thomas Gubler 2014-12-08 10:37:01 +01:00
parent 65629d09d5
commit 87df7c3243
11 changed files with 66 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
msg/px4_msgs/vehicle_attitude_setpoint.msg Normal file
View File

@ -0,0 +1,21 @@
uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
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[9] R_body # Rotation matrix describing the setpoint as rotation from the current body frame
bool R_valid # Set to true if rotation matrix is valid
# 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[4] q_e # Attitude error in quaternion
bool q_e_valid # Set to true if quaternion error 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)

3
src/modules/fw_att_control/fw_att_control_base.cpp
View File

@ -31,7 +31,7 @@
/**
* @file mc_att_control_base.cpp
*
*
* @author Roman Bapst <bapstr@ethz.ch>
*
*/
@ -40,7 +40,6 @@
#include <math.h>
#include <mathlib/mathlib.h>
#include <drivers/drv_hrt.h>
#include <systemlib/err.h>
using namespace std;

2
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -778,7 +778,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
att_sp.timestamp = hrt_absolute_time();
mavlink_quaternion_to_euler(set_attitude_target.q,
&att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body);
mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body);
att_sp.R_valid = true;
att_sp.thrust = set_attitude_target.thrust;
memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d));

4
src/modules/mc_att_control/mc_att_control_base.cpp
View File

@ -172,7 +172,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
if (_v_att_sp.R_valid) {
/* rotation matrix in _att_sp is valid, use it */
R_sp.set(&_v_att_sp.R_body[0][0]);
R_sp.set(&_v_att_sp.R_body[0]);
} else {
/* rotation matrix in _att_sp is not valid, use euler angles instead */
@ -180,7 +180,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
_v_att_sp.yaw_body);
/* copy rotation matrix back to setpoint struct */
memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0],
memcpy(&_v_att_sp.R_body[0], &R_sp.data[0][0],
sizeof(_v_att_sp.R_body));
_v_att_sp.R_valid = true;
}

7
src/modules/mc_att_control/mc_att_control_base.h
View File

@ -34,7 +34,7 @@
/**
* @file mc_att_control_base.h
*
*
* MC Attitude Controller
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
@ -45,23 +45,20 @@
* @author Roman Bapst <bapstr@ethz.ch>
*
*/
#include <px4.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <math.h>
#include <drivers/drv_hrt.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/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <lib/mathlib/mathlib.h>

6
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -52,8 +52,7 @@
* If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <px4.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
@ -62,8 +61,6 @@
#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>
@ -78,6 +75,7 @@
#include <systemlib/circuit_breaker.h>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include "mc_att_control_base.h"
/**

22
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -43,8 +43,9 @@
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <px4.h>
#include <functional>
#include <cstdio>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
@ -54,8 +55,7 @@
#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>
@ -67,8 +67,8 @@
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
// #include <systemlib/param/param.h>
// #include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
#include <lib/geo/geo.h>
@ -532,9 +532,9 @@ MulticopterPositionControl::reset_pos_sp()
if (_reset_pos_sp) {
_reset_pos_sp = false;
/* shift position setpoint to make attitude setpoint continuous */
_pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0)
_pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0)
- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
_pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1)
_pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1)
- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
}
@ -987,7 +987,7 @@ MulticopterPositionControl::task_main()
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */
R.identity();
memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
_att_sp.roll_body = 0.0f;
@ -1260,7 +1260,7 @@ MulticopterPositionControl::task_main()
}
/* copy rotation matrix to attitude setpoint topic */
memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
/* calculate euler angles, for logging only, must not be used for control */
@ -1275,7 +1275,7 @@ MulticopterPositionControl::task_main()
R.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
/* copy rotation matrix to attitude setpoint topic */
memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
_att_sp.roll_body = 0.0f;

59
src/modules/uORB/topics/vehicle_attitude_setpoint.h
View File

@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* Copyright (C) 2013-2014 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
@ -32,49 +31,37 @@
*
****************************************************************************/
/**
* @file vehicle_attitude_setpoint.h
* Definition of the vehicle attitude setpoint uORB topic.
*/
/* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/vehicle_attitude_setpoint.msg */
#ifndef TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_
#define TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_
#pragma once
#include <stdint.h>
#include <stdbool.h>
#include <platforms/px4_defines.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
/**
* vehicle attitude setpoint.
*/
struct vehicle_attitude_setpoint_s {
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
float roll_body; /**< body angle in NED frame */
float pitch_body; /**< body angle in NED frame */
float yaw_body; /**< body angle in NED frame */
//float body_valid; /**< Set to true if body angles are valid */
float R_body[3][3]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */
bool R_valid; /**< Set to true if rotation matrix is valid */
//! For quaternion-based attitude control
float q_d[4]; /** Desired quaternion for quaternion control */
bool q_d_valid; /**< Set to true if quaternion vector is valid */
float q_e[4]; /** Attitude error in quaternion */
bool q_e_valid; /**< Set to true if quaternion error vector is valid */
float 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) */
uint64_t timestamp;
float roll_body;
float pitch_body;
float yaw_body;
float R_body[9];
bool R_valid;
float q_d[4];
bool q_d_valid;
float q_e[4];
bool q_e_valid;
float thrust;
bool roll_reset_integral;
bool pitch_reset_integral;
bool yaw_reset_integral;
};
/**
@ -83,5 +70,3 @@ struct vehicle_attitude_setpoint_s {
/* register this as object request broker structure */
ORB_DECLARE(vehicle_attitude_setpoint);
#endif /* TOPIC_ARDRONE_CONTROL_H_ */

1
src/platforms/px4_defines.h
View File

@ -115,6 +115,7 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval)
/* Parameter handle datatype */
#include <systemlib/param/param.h>
typedef param_t px4_param_t;
/* Initialize a param, get param handle */

1
src/platforms/px4_includes.h
View File

@ -55,6 +55,7 @@
#include <nuttx/config.h>
#include <uORB/uORB.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>

4
src/platforms/px4_middleware.h
View File

@ -53,13 +53,13 @@ __EXPORT uint64_t get_time_micros();
/**
* Returns true if the app/task should continue to run
*/
bool ok() { return ros::ok(); }
inline bool ok() { return ros::ok(); }
#else
extern bool task_should_exit;
/**
* Returns true if the app/task should continue to run
*/
bool ok() { return !task_should_exit; }
inline bool ok() { return !task_should_exit; }
#endif
class Rate