diff options
| -rw-r--r-- | main.cpp | 43 | ||||
| -rw-r--r-- | ppmsum.cpp | 39 | ||||
| -rw-r--r-- | ppmsum.h | 13 |
3 files changed, 60 insertions, 35 deletions
@@ -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 |