aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorRoman Bapst <romanbapst@yahoo.de>2014-11-10 16:58:56 +0100
committerRoman Bapst <romanbapst@yahoo.de>2014-11-10 16:58:56 +0100
commita2ec2ec857f5b7584628c2a923c85d0b62b082dd (patch)
tree5c709b555bb90501662eed9685d478cb43672634
parentf72f0db9963049879eb0dce9a9ef37baadd45dd2 (diff)
downloadpx4-firmware-a2ec2ec857f5b7584628c2a923c85d0b62b082dd.tar.gz
px4-firmware-a2ec2ec857f5b7584628c2a923c85d0b62b082dd.tar.bz2
px4-firmware-a2ec2ec857f5b7584628c2a923c85d0b62b082dd.zip
cleaned up
-rw-r--r--src/modules/mc_att_control/mc_att_control_base.h58
1 files changed, 6 insertions, 52 deletions
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 5a08bba79..515fb0c14 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -1,14 +1,7 @@
-/*
- * mc_att_control_base.h
- *
- * Created on: Sep 25, 2014
- * Author: roman
- */
-
#ifndef MC_ATT_CONTROL_BASE_H_
#define MC_ATT_CONTROL_BASE_H_
-/* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+/* Copyright (c) 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
@@ -40,21 +33,10 @@
****************************************************************************/
/**
- * @file mc_att_control_main.cpp
- * Multicopter attitude controller.
- *
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
+ * @file mc_att_control_base.h
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
*
- * The controller has two loops: P loop for angular error and PD loop for angular rate error.
- * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
- * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
- * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis.
- * These two approaches fused seamlessly with weight depending on angular error.
- * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity.
- * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
- * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
*/
#include <stdio.h>
@@ -74,18 +56,10 @@
#include <uORB/topics/actuator_armed.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
-//#include <systemlib/systemlib.h>
#include <lib/mathlib/mathlib.h>
-//#include <Eigen/Eigen>
-/**
- * Multicopter attitude control app start / stop handling function
- *
- * @ingroup apps
- */
-
#define YAW_DEADZONE 0.05f
#define MIN_TAKEOFF_THRUST 0.2f
#define RATES_I_LIMIT 0.3f
@@ -98,7 +72,7 @@ public:
MulticopterAttitudeControlBase();
/**
- * Destructor, also kills the sensors task.
+ * Destructor
*/
~MulticopterAttitudeControlBase();
@@ -109,6 +83,7 @@ public:
*/
void control_attitude(float dt);
void control_attitude_rates(float dt);
+
// setters and getters for interface with euroc-gazebo simulator
void set_attitude(const Eigen::Quaternion<double> attitude);
void set_attitude_rates(const Eigen::Vector3d& angular_rate);
@@ -120,10 +95,6 @@ protected:
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 */
struct vehicle_attitude_s _v_att; /**< vehicle attitude */
@@ -146,8 +117,6 @@ protected:
bool _reset_yaw_sp; /**< reset yaw setpoint flag */
-
-
struct {
math::Vector<3> att_p; /**< P gain for angular error */
math::Vector<3> rate_p; /**< P gain for angular rate error */
@@ -161,24 +130,9 @@ protected:
float man_yaw_max;
math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
} _params;
-
-
- /**
- * Attitude controller.
- */
- //void control_attitude(float dt);
-
- /**
- * Attitude rates controller.
- */
void vehicle_attitude_setpoint_poll(); //provisional
-
-
-
-
-
};
#endif /* MC_ATT_CONTROL_BASE_H_ */