aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--msg/px4_msgs/vehicle_attitude_setpoint.msg21
-rw-r--r--src/modules/fw_att_control/fw_att_control_base.cpp3
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp2
-rw-r--r--src/modules/mc_att_control/mc_att_control_base.cpp4
-rw-r--r--src/modules/mc_att_control/mc_att_control_base.h7
-rw-r--r--src/modules/mc_att_control/mc_att_control_main.cpp6
-rw-r--r--src/modules/mc_pos_control/mc_pos_control_main.cpp22
-rw-r--r--src/modules/uORB/topics/vehicle_attitude_setpoint.h59
-rw-r--r--src/platforms/px4_defines.h1
-rw-r--r--src/platforms/px4_includes.h1
-rw-r--r--src/platforms/px4_middleware.h4
11 files changed, 66 insertions, 64 deletions
diff --git a/msg/px4_msgs/vehicle_attitude_setpoint.msg b/msg/px4_msgs/vehicle_attitude_setpoint.msg
new file mode 100644
index 000000000..1a8e6e3d5
--- /dev/null
+++ b/msg/px4_msgs/vehicle_attitude_setpoint.msg
@@ -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)
diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp
index 46fef3e67..f543c02f9 100644
--- a/src/modules/fw_att_control/fw_att_control_base.cpp
+++ b/src/modules/fw_att_control/fw_att_control_base.cpp
@@ -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;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index e98d72afe..cb55a25aa 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -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));
diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index 5e65e6508..f6b3268b5 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -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;
}
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 1c2f3fb6f..c1ea52f63 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -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>
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 887a53508..c0e8957e0 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -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"
/**
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index cea847603..4ea5fdfb6 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -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;
diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
index a503aa0c6..29fb104df 100644
--- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
@@ -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) */
+struct vehicle_attitude_setpoint_s {
+ 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_ */
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index af7188f32..ff302448a 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -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 */
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index a3b59b766..a9425077e 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -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>
diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h
index c1465b4b1..b0bc40417 100644
--- a/src/platforms/px4_middleware.h
+++ b/src/platforms/px4_middleware.h
@@ -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
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-01 06:10:40 +0000