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

lots' of header juggling and small changes to make mc att control compile for NuttX and ROS

Browse Source
This commit is contained in:
Thomas Gubler
2014-12-16 08:24:51 +01:00
parent 9980e44821
commit 9520983e08
20 changed files with 376 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control.cpp
+28 -47
View File
@@ -44,6 +44,8 @@
*/
#include "mc_att_control.h"
#include "mc_att_control_params.h"
#include "math.h"
#define YAW_DEADZONE 0.05f
#define MIN_TAKEOFF_THRUST 0.2f
@@ -63,7 +65,6 @@ static const int ERROR = -1;
MulticopterAttitudeControl::MulticopterAttitudeControl() :
MulticopterAttitudeControlBase(),
_task_should_exit(false),
_control_task(-1),
_actuators_0_circuit_breaker_enabled(false),
/* publications */
@@ -76,26 +77,26 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
{
_params_handles.roll_p = PX4_PARAM_INIT("MC_ROLL_P");
_params_handles.roll_rate_p = PX4_PARAM_INIT("MC_ROLLRATE_P");
_params_handles.roll_rate_i = PX4_PARAM_INIT("MC_ROLLRATE_I");
_params_handles.roll_rate_d = PX4_PARAM_INIT("MC_ROLLRATE_D");
_params_handles.pitch_p = PX4_PARAM_INIT("MC_PITCH_P");
_params_handles.pitch_rate_p = PX4_PARAM_INIT("MC_PITCHRATE_P");
_params_handles.pitch_rate_i = PX4_PARAM_INIT("MC_PITCHRATE_I");
_params_handles.pitch_rate_d = PX4_PARAM_INIT("MC_PITCHRATE_D");
_params_handles.yaw_p = PX4_PARAM_INIT("MC_YAW_P");
_params_handles.yaw_rate_p = PX4_PARAM_INIT("MC_YAWRATE_P");
_params_handles.yaw_rate_i = PX4_PARAM_INIT("MC_YAWRATE_I");
_params_handles.yaw_rate_d = PX4_PARAM_INIT("MC_YAWRATE_D");
_params_handles.yaw_ff = PX4_PARAM_INIT("MC_YAW_FF");
_params_handles.yaw_rate_max = PX4_PARAM_INIT("MC_YAWRATE_MAX");
_params_handles.man_roll_max = PX4_PARAM_INIT("MC_MAN_R_MAX");
_params_handles.man_pitch_max = PX4_PARAM_INIT("MC_MAN_P_MAX");
_params_handles.man_yaw_max = PX4_PARAM_INIT("MC_MAN_Y_MAX");
_params_handles.acro_roll_max = PX4_PARAM_INIT("MC_ACRO_R_MAX");
_params_handles.acro_pitch_max = PX4_PARAM_INIT("MC_ACRO_P_MAX");
_params_handles.acro_yaw_max = PX4_PARAM_INIT("MC_ACRO_Y_MAX");
_params_handles.roll_p = PX4_PARAM_INIT(MC_ROLL_P);
_params_handles.roll_rate_p = PX4_PARAM_INIT(MC_ROLLRATE_P);
_params_handles.roll_rate_i = PX4_PARAM_INIT(MC_ROLLRATE_I);
_params_handles.roll_rate_d = PX4_PARAM_INIT(MC_ROLLRATE_D);
_params_handles.pitch_p = PX4_PARAM_INIT(MC_PITCH_P);
_params_handles.pitch_rate_p = PX4_PARAM_INIT(MC_PITCHRATE_P);
_params_handles.pitch_rate_i = PX4_PARAM_INIT(MC_PITCHRATE_I);
_params_handles.pitch_rate_d = PX4_PARAM_INIT(MC_PITCHRATE_D);
_params_handles.yaw_p = PX4_PARAM_INIT(MC_YAW_P);
_params_handles.yaw_rate_p = PX4_PARAM_INIT(MC_YAWRATE_P);
_params_handles.yaw_rate_i = PX4_PARAM_INIT(MC_YAWRATE_I);
_params_handles.yaw_rate_d = PX4_PARAM_INIT(MC_YAWRATE_D);
_params_handles.yaw_ff = PX4_PARAM_INIT(MC_YAW_FF);
_params_handles.yaw_rate_max = PX4_PARAM_INIT(MC_YAWRATE_MAX);
_params_handles.man_roll_max = PX4_PARAM_INIT(MC_MAN_R_MAX);
_params_handles.man_pitch_max = PX4_PARAM_INIT(MC_MAN_P_MAX);
_params_handles.man_yaw_max = PX4_PARAM_INIT(MC_MAN_Y_MAX);
_params_handles.acro_roll_max = PX4_PARAM_INIT(MC_ACRO_R_MAX);
_params_handles.acro_pitch_max = PX4_PARAM_INIT(MC_ACRO_P_MAX);
_params_handles.acro_yaw_max = PX4_PARAM_INIT(MC_ACRO_Y_MAX);
/* fetch initial parameter values */
parameters_update();
@@ -115,26 +116,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
MulticopterAttitudeControl::~MulticopterAttitudeControl()
{
if (_control_task != -1) {
/* task wakes up every 100ms or so at the longest */
_task_should_exit = true;
/* wait for a second for the task to quit at our request */
unsigned i = 0;
do {
/* wait 20ms */
usleep(20000);
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_control_task);
break;
}
} while (_control_task != -1);
}
// mc_att_control::g_control = nullptr;
}
int
@@ -203,8 +184,8 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
/* run controller on attitude changes */
static uint64_t last_run = 0;
float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
float dt = (px4::get_time_micros() - last_run) / 1000000.0f;
last_run = px4::get_time_micros();
/* guard against too small (< 2ms) and too large (> 20ms) dt's */
if (dt < 0.002f) {
@@ -219,7 +200,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
/* publish the attitude setpoint if needed */
if (_publish_att_sp) {
_v_att_sp_mod.timestamp = hrt_absolute_time();
_v_att_sp_mod.timestamp = px4::get_time_micros();
if (_att_sp_pub != nullptr) {
_att_sp_pub->publish(_v_att_sp_mod);
@@ -234,7 +215,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
_v_rates_sp_mod.pitch = _rates_sp(1);
_v_rates_sp_mod.yaw = _rates_sp(2);
_v_rates_sp_mod.thrust = _thrust_sp;
_v_rates_sp_mod.timestamp = hrt_absolute_time();
_v_rates_sp_mod.timestamp = px4::get_time_micros();
if (_v_rates_sp_pub != nullptr) {
_v_rates_sp_pub->publish(_v_rates_sp_mod);
@@ -258,7 +239,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
_v_rates_sp_mod.pitch = _rates_sp(1);
_v_rates_sp_mod.yaw = _rates_sp(2);
_v_rates_sp_mod.thrust = _thrust_sp;
_v_rates_sp_mod.timestamp = hrt_absolute_time();
_v_rates_sp_mod.timestamp = px4::get_time_micros();
if (_v_rates_sp_pub != nullptr) {
_v_rates_sp_pub->publish(_v_rates_sp_mod);
@@ -285,7 +266,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp = px4::get_time_micros();
if (!_actuators_0_circuit_breaker_enabled) {
if (_actuators_0_pub != nullptr) {
src/modules/mc_att_control/mc_att_control.h
+1 -8
View File
@@ -52,22 +52,16 @@
* If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
*/
#include <px4.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 <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
// #include <systemlib/systemlib.h>
#include <systemlib/circuit_breaker.h>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include "mc_att_control_base.h"
@@ -91,7 +85,6 @@ public:
private:
bool _task_should_exit; /**< if true, sensor task should exit */
int _control_task; /**< task handle for sensor task */
bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
px4::Publisher * _att_sp_pub; /**< attitude setpoint publication */
src/modules/mc_att_control/mc_att_control_main.cpp
+2 -2
View File
@@ -129,12 +129,12 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[])
PX4_MAIN_FUNCTION(mc_att_control)
{
warnx("starting");
PX4_INFO("starting");
MulticopterAttitudeControl attctl;
thread_running = true;
attctl.spin();
warnx("exiting.");
PX4_INFO("exiting.");
thread_running = false;
return 0;
}