From 6ff228c2ff810a8068610e54612b77c0d94e5584 Mon Sep 17 00:00:00 2001
From: romain <romain.chiap@gmail.com>
Date: Thu, 7 Mar 2019 15:28:33 -0500
Subject: [PATCH] sih module implemented with hrt_call_every and semaphore_wait
---
.../init.d/airframes/1100_rc_quad_x_sih.hil | 2 +
src/modules/sih/CMakeLists.txt | 2 +-
src/modules/sih/sih.cpp | 49 +++++++++++--------
src/modules/sih/sih.hpp | 13 +++--
4 files changed, 41 insertions(+), 25 deletions(-)
diff --git a/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil
index 63fea2122b..03c3f2d5a7 100644
--- a/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil
+++ b/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil
@@ -17,3 +17,5 @@ set PWM_OUT 1234
param set SYS_HITL 1
param set SYS_SIH 1
+
+param set BAT_N_CELLS 4
diff --git a/src/modules/sih/CMakeLists.txt b/src/modules/sih/CMakeLists.txt
index ab77255e28..74c6d273d8 100644
--- a/src/modules/sih/CMakeLists.txt
+++ b/src/modules/sih/CMakeLists.txt
@@ -35,7 +35,7 @@ px4_add_module(
MODULE modules__sih
MAIN sih
STACK_MAIN 1200
- STACK_MAX 3500
+ STACK_MAX 1024
COMPILE_FLAGS
SRCS
sih.cpp
diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp
index b8d5a598f4..5aa3ba174a 100644
--- a/src/modules/sih/sih.cpp
+++ b/src/modules/sih/sih.cpp
@@ -44,7 +44,6 @@
#include <px4_getopt.h>
#include <px4_log.h>
-#include <px4_posix.h>
#include <drivers/drv_pwm_output.h> // to get PWM flags
#include <uORB/topics/vehicle_status.h> // to get the HIL status
@@ -86,7 +85,7 @@ Most of the variables are declared global in the .hpp file to avoid stack overfl
PRINT_MODULE_USAGE_NAME("sih", "sih");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAM_FLAG('f', "Optional example flag", true);
- PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 4096, "Optional example parameter", true);
+ PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 1024, "Optional example parameter", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
@@ -124,7 +123,7 @@ int Sih::task_spawn(int argc, char *argv[])
_task_id = px4_task_spawn_cmd("sih",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX, //SCHED_PRIORITY_DEFAULT
- 4096,
+ 1024,
(px4_main_t)&run_trampoline,
(char *const *)argv);
@@ -182,7 +181,9 @@ Sih *Sih::instantiate(int argc, char *argv[])
}
Sih::Sih(int example_param, bool example_flag)
- : ModuleParams(nullptr)
+ : ModuleParams(nullptr),
+ _loop_perf(perf_alloc(PC_ELAPSED, "sih_execution")),
+ _sampling_perf(perf_alloc(PC_ELAPSED, "sih_sampling"))
{
}
@@ -206,29 +207,40 @@ void Sih::run()
_gps_time = task_start;
_serial_time = task_start;
- // hrt_call_every(&_timer_call, LOOP_INTERVAL, LOOP_INTERVAL, timer_callback, this);
+ px4_sem_init(&_data_semaphore, 0, 0);
- while (!should_exit())
- {
- inner_loop();
-
- usleep(1000);
- }
+ hrt_call_every(&_timer_call, LOOP_INTERVAL, LOOP_INTERVAL, timer_callback, &_data_semaphore);
+
+ perf_begin(_sampling_perf);
+
+ while (!should_exit())
+ {
+ px4_sem_wait(&_data_semaphore); // periodic real time wakeup
+
+ perf_end(_sampling_perf);
+ perf_begin(_sampling_perf);
+
+ perf_begin(_loop_perf);
+
+ inner_loop(); // main execution function
+
+ perf_end(_loop_perf);
+ }
- // hrt_cancel(&_timer_call); // close the periodic timer interruption
+ hrt_cancel(&_timer_call); // close the periodic timer interruption
orb_unsubscribe(_actuator_out_sub);
orb_unsubscribe(_parameter_update_sub);
// close(serial_fd);
}
-// timer_callback() is used as a trampoline to inner_loop()
-void Sih::timer_callback(void *arg)
+// timer_callback() is used as a real time callback to post the semaphore
+void Sih::timer_callback(void *sem)
{
- (reinterpret_cast<Sih *>(arg))->inner_loop();
+ px4_sem_post((px4_sem_t *)sem);
}
-// This is the main execution called periodically by the timer callback
+// this is the main execution waken up periodically by the semaphore
void Sih::inner_loop()
{
_now = hrt_absolute_time();
@@ -262,11 +274,6 @@ void Sih::inner_loop()
parameters_update_poll(); // update the parameters if needed
}
- // else if (loop_count==5)
- // {
- // tcflush(serial_fd, TCOFLUSH); // flush output data
- // tcdrain(serial_fd);
- // }
_sih.te_us=hrt_absolute_time()-_now; // execution time (without delay)
diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp
index 55b08f1f09..8fcd770735 100644
--- a/src/modules/sih/sih.hpp
+++ b/src/modules/sih/sih.hpp
@@ -35,12 +35,13 @@
#include <px4_module.h>
#include <px4_module_params.h>
+#include <px4_posix.h>
#include <matrix/matrix/math.hpp> // matrix, vectors, dcm, quaterions
#include <conversion/rotation.h> // math::radians,
#include <ecl/geo/geo.h> // to get the physical constants
#include <drivers/drv_hrt.h> // to get the real time
-// #include <mathlib/math/filter/LowPassFilter2p.hpp>
+#include <perf/perf_counter.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/actuator_outputs.h>
@@ -87,7 +88,8 @@ public:
// generate white Gaussian noise sample as a 3D vector with specified std
static Vector3f noiseGauss3f(float stdx, float stdy, float stdz);
- static void timer_callback(void *arg);
+ // timer called periodically to post the semaphore
+ static void timer_callback(void *sem);
// static int pack_float(char* uart_msg, int index, void *value); // pack a float to a IEEE754
private:
@@ -134,7 +136,7 @@ private:
static constexpr float T1_K = T1_C - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // ground temperature in Kelvin
static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f; // temperature gradient in degrees per metre
static constexpr uint32_t BAUDS_RATE = 57600; // bauds rate of the serial port
- static constexpr hrt_abstime LOOP_INTERVAL = 10000; // 250 Hz real time
+ static constexpr hrt_abstime LOOP_INTERVAL = 4000; // 4ms => 250 Hz real-time
void init_variables();
void init_sensors();
@@ -149,6 +151,11 @@ private:
void send_serial_msg(int serial_fd, int64_t t_ms);
void inner_loop();
+ perf_counter_t _loop_perf;
+ perf_counter_t _sampling_perf;
+
+ px4_sem_t _data_semaphore;
+
int32_t _counter = 0;
hrt_call _timer_call;
hrt_abstime _last_run;
--
GitLab