fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 07:03: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/bottle_drop/bottle_drop.cpp
+1
View File
@@ -59,6 +59,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_global_position.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
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;
}
src/modules/systemlib/circuit_breaker.c
+9 -8
View File
@@ -42,7 +42,8 @@
* parameter needs to set to the key (magic).
*/
#include <systemlib/param/param.h>
#include <px4.h>
#include <systemlib/circuit_breaker_params.h>
#include <systemlib/circuit_breaker.h>
/**
@@ -56,7 +57,7 @@
* @max 894281
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0);
PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK);
/**
* Circuit breaker for rate controller output
@@ -69,7 +70,7 @@ PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0);
* @max 140253
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL);
/**
* Circuit breaker for IO safety
@@ -81,7 +82,7 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
* @max 22027
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY);
/**
* Circuit breaker for airspeed sensor
@@ -93,7 +94,7 @@ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
* @max 162128
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK);
/**
* Circuit breaker for flight termination
@@ -106,7 +107,7 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
* @max 121212
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM);
/**
* Circuit breaker for engine failure detection
@@ -120,7 +121,7 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
* @max 284953
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
PX4_PARAM_DEFINE_INT32(CBRK_ENGINEFAIL);
/**
* Circuit breaker for gps failure detection
@@ -134,7 +135,7 @@ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
* @max 240024
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024);
PX4_PARAM_DEFINE_INT32(CBRK_GPSFAIL);
bool circuit_breaker_enabled(const char* breaker, int32_t magic)
{
src/modules/systemlib/circuit_breaker.h
+7 -1
View File
@@ -61,8 +61,14 @@
__BEGIN_DECLS
#ifdef __cplusplus
extern "C" {
#endif
__EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic);
#ifdef __cplusplus
}
#endif
__EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic);
__END_DECLS
#endif /* CIRCUIT_BREAKER_H_ */
src/modules/systemlib/circuit_breaker_params.h
+7
View File
@@ -0,0 +1,7 @@
#define PARAM_CBRK_SUPPLY_CHK_DEFAULT 0
#define PARAM_CBRK_RATE_CTRL_DEFAULT 0
#define PARAM_CBRK_IO_SAFETY_DEFAULT 0
#define PARAM_CBRK_AIRSPD_CHK_DEFAULT 0
#define PARAM_CBRK_FLIGHTTERM_DEFAULT 121212
#define PARAM_CBRK_ENGINEFAIL_DEFAULT 284953
#define PARAM_CBRK_GPSFAIL_DEFAULT 240024
src/modules/systemlib/perf_counter.h
+1
View File
@@ -40,6 +40,7 @@
#define _SYSTEMLIB_PERF_COUNTER_H value
#include <stdint.h>
#include <platforms/px4_defines.h>
/**
* Counter types.