summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--main.cpp43
-rw-r--r--ppmsum.cpp39
-rw-r--r--ppmsum.h13
3 files changed, 60 insertions, 35 deletions
diff --git a/main.cpp b/main.cpp
index 4ce3f66..3b37344 100644
--- a/main.cpp
+++ b/main.cpp
@@ -3,6 +3,7 @@
#include "itg3200.h"
#include "bma150.h"
#include "ak8975.h"
+#include "ppmsum.h"
#include <ch.h>
#include <hal.h>
@@ -26,6 +27,8 @@ class LEDThread : public BaseThread<LEDThread, 128> {
LEDThread led_thread;
+PPMSum ppmsum;
+
class USBThread : public BaseThread<USBThread, 256> {
private:
typedef enum {W_S, W_N, W_V} w_s_t;
@@ -138,35 +141,6 @@ class I2CThread : public BaseThread<I2CThread, 256> {
I2CThread i2c_thread;
-icucnt_t last_width, last_period;
-
-int16_t ppm_input[4];
-
-int ppm_n = 0;
-
-static void icuwidthcb(ICUDriver *icup) {
-
- last_width = icuGetWidthI(icup);
- ppm_input[ppm_n] = last_width - 1050;
-}
-
-static void icuperiodcb(ICUDriver *icup) {
-
- last_period = icuGetPeriodI(icup);
- if(last_period > 5000) {
- ppm_n = 0;
- } else {
- ppm_n = (ppm_n + 1) % 4;
- }
-}
-
-static ICUConfig icucfg = {
- ICU_INPUT_ACTIVE_HIGH,
- 1000000,
- icuwidthcb,
- icuperiodcb
-};
-
static const ADCConversionGroup adcgrpcfg = {
FALSE,
2,
@@ -206,7 +180,7 @@ void foo(PWMDriver*) {
int16_t thrust, pitch, roll, yaw;
- thrust = ppm_input[2];
+ thrust = ppmsum.data[2];
if(thrust < 0) {
thrust = 0;
}
@@ -214,9 +188,9 @@ void foo(PWMDriver*) {
thrust = 1000;
}
- pitch = ((ppm_input[1] - 500) * thrust) >> 10;
- roll = ((ppm_input[0] - 500) * thrust) >> 10;
- yaw = ((ppm_input[3] - 500) * thrust) >> 10;
+ pitch = ((ppmsum.data[1] - 500) * thrust) >> 10;
+ roll = ((ppmsum.data[0] - 500) * thrust) >> 10;
+ yaw = ((ppmsum.data[3] - 500) * thrust) >> 10;
//pitch = (-i2c_thread.x) >> 5;
//roll = i2c_thread.y >> 5;
@@ -257,8 +231,7 @@ int main(void) {
led_thread.start();
- icuStart(&ICUD4, &icucfg);
- icuEnable(&ICUD4);
+ ppmsum.start();
usbs.init();
diff --git a/ppmsum.cpp b/ppmsum.cpp
new file mode 100644
index 0000000..ce601e5
--- /dev/null
+++ b/ppmsum.cpp
@@ -0,0 +1,39 @@
+#include "ppmsum.h"
+
+#include <ch.h>
+#include <hal.h>
+
+namespace {
+ icucnt_t last_width, last_period;
+
+ PPMSum* ppmsum;
+ int ppm_n = 0;
+
+ static void icuwidthcb(ICUDriver *icup) {
+ last_width = icuGetWidthI(icup);
+ ppmsum->data[ppm_n] = last_width - 1050;
+ }
+
+ static void icuperiodcb(ICUDriver *icup) {
+ last_period = icuGetPeriodI(icup);
+ if(last_period > 5000) {
+ ppm_n = 0;
+ } else {
+ ppm_n = (ppm_n + 1) % 4;
+ }
+ }
+
+ static ICUConfig icucfg = {
+ ICU_INPUT_ACTIVE_HIGH,
+ 1000000,
+ icuwidthcb,
+ icuperiodcb
+ };
+};
+
+void PPMSum::start() {
+ ppmsum = this;
+
+ icuStart(&ICUD4, &icucfg);
+ icuEnable(&ICUD4);
+}
diff --git a/ppmsum.h b/ppmsum.h
new file mode 100644
index 0000000..82b62df
--- /dev/null
+++ b/ppmsum.h
@@ -0,0 +1,13 @@
+#ifndef PPMSUM_H
+#define PPMSUM_H
+
+#include <stdint.h>
+
+class PPMSum {
+ public:
+ int16_t data[4];
+
+ void start();
+};
+
+#endif