aboutsummaryrefslogtreecommitdiff
path: root/src/modules
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules')
-rw-r--r--src/modules/bottle_drop/bottle_drop.cpp1
-rw-r--r--src/modules/mc_att_control/mc_att_control.cpp75
-rw-r--r--src/modules/mc_att_control/mc_att_control.h9
-rw-r--r--src/modules/mc_att_control/mc_att_control_main.cpp4
-rw-r--r--src/modules/systemlib/circuit_breaker.c17
-rw-r--r--src/modules/systemlib/circuit_breaker.h8
-rw-r--r--src/modules/systemlib/circuit_breaker_params.h7
-rw-r--r--src/modules/systemlib/perf_counter.h1
8 files changed, 56 insertions, 66 deletions
diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp
index e0bcbc6e9..53ab74305 100644
--- a/src/modules/bottle_drop/bottle_drop.cpp
+++ b/src/modules/bottle_drop/bottle_drop.cpp
@@ -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>
diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp
index 13bff9561..906bd0a47 100644
--- a/src/modules/mc_att_control/mc_att_control.cpp
+++ b/src/modules/mc_att_control/mc_att_control.cpp
@@ -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) {
diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h
index 24009a5e6..1da738408 100644
--- a/src/modules/mc_att_control/mc_att_control.h
+++ b/src/modules/mc_att_control/mc_att_control.h
@@ -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 */
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 a1dca8a8c..be627866e 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -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;
}
diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c
index 12187d70e..1b3ffd59f 100644
--- a/src/modules/systemlib/circuit_breaker.c
+++ b/src/modules/systemlib/circuit_breaker.c
@@ -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)
{
diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h
index b3431722f..012d8cb61 100644
--- a/src/modules/systemlib/circuit_breaker.h
+++ b/src/modules/systemlib/circuit_breaker.h
@@ -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_ */
diff --git a/src/modules/systemlib/circuit_breaker_params.h b/src/modules/systemlib/circuit_breaker_params.h
new file mode 100644
index 000000000..768bf7f53
--- /dev/null
+++ b/src/modules/systemlib/circuit_breaker_params.h
@@ -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
diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h
index 668d9dfdf..a9dfb13f8 100644
--- a/src/modules/systemlib/perf_counter.h
+++ b/src/modules/systemlib/perf_counter.h
@@ -40,6 +40,7 @@
#define _SYSTEMLIB_PERF_COUNTER_H value
#include <stdint.h>
+#include <platforms/px4_defines.h>
/**
* Counter types.
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-11 17:59:42 +0000