summaryrefslogtreecommitdiff
path: root/main.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'main.cpp')
-rw-r--r--main.cpp506
1 files changed, 253 insertions, 253 deletions
diff --git a/main.cpp b/main.cpp
index 3bebf74..4d3240c 100644
--- a/main.cpp
+++ b/main.cpp
@@ -1,253 +1,253 @@
-#include "thread.h"
-#include "usbserial.h"
-#include "itg3200.h"
-#include "bma150.h"
-#include "ak8975.h"
-#include "ppmsum.h"
-#include "motormixer.h"
-
-#include <ch.h>
-#include <hal.h>
-
-#include "IMU.h"
-
-class LEDThread : public BaseThread<LEDThread, 128> {
- public:
- noreturn_t thread_main() {
- systime_t time = chTimeNow(); // T0
- while (TRUE) {
- time += MS2ST(1000); // Next deadline
- palClearPad(GPIOA, 5);
- chThdSleepUntil(time);
- time += MS2ST(1000); // Next deadline
- palSetPad(GPIOA, 5);
- chThdSleepUntil(time);
- }
- }
-};
-
-LEDThread led_thread;
-
-PPMSum ppmsum;
-MotorMixer motors;
-
-class USBThread : public BaseThread<USBThread, 256> {
- private:
- typedef enum {W_S, W_N, W_V} w_s_t;
-
- public:
- USBSerial* usbs;
-
- uint8_t data[9];
-
- noreturn_t thread_main() {
- for(int i = 0; i < 9; i++) {
- data[i] = 0;
- }
-
- w_s_t w_s = W_S;
- uint8_t w_n = 0;
-
- while(1) {
- size_t buffer = usbs->getc();
- if(buffer >= 0 && buffer < 256) {
- if(w_s == W_S && buffer == 'S') {
- w_s = W_N;
- } else if(w_s == W_N && buffer >= '1' && buffer <= '9') {
- w_s = W_V;
- w_n = buffer - '1';
- } else if(w_s == W_V) {
- w_s = W_S;
- data[w_n] = buffer;
- } else {
- w_s = W_S;
- }
- }
- }
- }
-};
-
-USBThread usb_thread;
-USBSerial usbs;
-
-#include "foo.h"
-#include <cmath>
-
-uint8_t syncword[] = {0xff, 0x00, 0xaa, 0x55};
-uint8_t buf[64];
-int16_t* sensordata = (int16_t*)buf;
-
-template<class T>
-inline void saturate(T& var, int absmax) {
- if(var > absmax) {
- var = absmax;
- } else if(var < -absmax) {
- var = -absmax;
- }
-}
-
-class I2CThread : public BaseThread<I2CThread, 256> {
- public:
- ITG3200 gyro;
- BMA150 acc;
- AK8975 magn;
-
- int16_t x, y, z;
-
- noreturn_t thread_main() {
- I2CSensor::enable_bus();
-
- gyro.init();
- acc.init();
- magn.init();
-
- systime_t time = chTimeNow();
-
- int32_t pitch_angle_accum = 0;
- int32_t roll_angle_accum = 0;
-
- while (1) {
- gyro.update();
- acc.update();
- magn.update();
- x = gyro.x;
- y = gyro.y;
- z = gyro.z;
-
- IMUupdate(gyro.x * 0.0012141420883438813, gyro.y * 0.0012141420883438813, gyro.z * 0.0012141420883438813, acc.x, acc.y, acc.z);
-
- //float pitch = asinf(2*(q0*q2 - q3*q1));
- //int16_t pitch = atan2f(2*(q2*q3 + q0*q1), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767;
- //int16_t roll = atan2f(2*(-q1*q3 + q0*q2), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767;
- //int16_t yaw = atan2f(2*(q2*q1 + q0*q3), 1 - 2 * (q3*q3 + q2*q2)) / M_PI * 32767;
-
- float norm_x = 2*(q0*q2 - q1*q3);
- float norm_y = 2*(q0*q1 + q2*q3);
- float norm_z = (1 - 2*(q1*q1 + q2*q2));
-
- float elev = acosf(norm_z);
- float azim = atan2f(norm_y, norm_x);
-
- int16_t pitch = elev * sinf(azim) / M_PI * 32767;
- int16_t roll = elev * cosf(azim) / M_PI * 32767;
- int16_t yaw = 0;
-
- sensordata[0] = gyro.x;
- sensordata[1] = gyro.y;
- sensordata[2] = gyro.z;
- sensordata[3] = acc.x;
- sensordata[4] = acc.y;
- sensordata[5] = acc.z;
- sensordata[6] = magn.x;
- sensordata[7] = magn.y;
- sensordata[8] = magn.z;
- sensordata[9] = pitch;
- sensordata[10] = roll;
- sensordata[11] = yaw;
-
- usbs.write(syncword, sizeof(syncword));
- usbs.write(buf, sizeof(buf));
-
- /*usbprintf(usbs, "%6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d, %6d | %6d, %6d, %6d\r\n",
- gyro.x, gyro.y, gyro.z,
- acc.x, acc.y, acc.z,
- magn.x, magn.y, magn.z,
- int(q0 * 10000), int(q1 * 10000), int(q2 * 10000), int(q3 * 10000),
- int(pitch * 10000), int(roll * 10000), int(yaw * 10000));*/
-
- int16_t pitch_angle_target = (ppmsum.data[1] - 500) * 8;
- int16_t roll_angle_target = (ppmsum.data[0] - 500) * 8;
-
- int16_t pitch_angle_error = pitch_angle_target - pitch;
- int16_t roll_angle_error = roll_angle_target - roll;
-
- // 25 deg max error.
- saturate(pitch_angle_error, 4551);
- saturate(roll_angle_error, 4551);
-
- pitch_angle_accum += pitch_angle_error;
- roll_angle_accum += roll_angle_error;
-
- // 20 deg s max error.
- saturate(pitch_angle_error, 364088);
- saturate(roll_angle_error, 364088);
-
- int32_t pitch_rate_target = (pitch_angle_error * 4 * 65536 + pitch_angle_accum * 98) >> 16;
- int32_t roll_rate_target = (roll_angle_error * 4 * 65536 + roll_angle_accum * 98) >> 16;
-
- int16_t pitch_rate_comp = ((pitch_rate_target - (gyro.x * 4000 / 360)) * 6 * 36) >> 16;
- int16_t roll_rate_comp = ((roll_rate_target - (gyro.y * 4000 / 360)) * 6 * 36) >> 16;
-
- saturate(pitch_rate_comp, 250);
- saturate(roll_rate_comp, 250);
-
- motors.update(ppmsum.data[2], pitch_rate_comp, roll_rate_comp, 0);
-
- time += MS2ST(10);
- if(time > chTimeNow()) {
- chThdSleepUntil(time);
- }
- }
- }
-};
-
-I2CThread i2c_thread;
-
-static const ADCConversionGroup adcgrpcfg = {
- FALSE,
- 2,
- 0,
- 0,
- 0,
- 0,
- 0,
- ADC_SQR1_NUM_CH(2),
- 0,
- ADC_SQR3_SQ2_N(ADC_CHANNEL_IN14) | ADC_SQR3_SQ1_N(ADC_CHANNEL_IN15)
-};
-
-class ADCThread : public BaseThread<ADCThread, 128> {
- private:
- adcsample_t adc_samples[2];
-
- public:
- noreturn_t thread_main() {
- adcStart(&ADCD1, NULL);
-
- systime_t time = chTimeNow();
- while (TRUE) {
- adcStartConversion(&ADCD1, &adcgrpcfg, adc_samples, 1);
- sensordata[12] = adc_samples[0] * 1265 / 1000;
- sensordata[13] = adc_samples[1] * 2201 / 1000;
-
- time += MS2ST(1000);
- chThdSleepUntil(time);
- }
- }
-};
-
-ADCThread adc_thread;
-
-int main(void) {
- halInit();
- chSysInit();
-
- led_thread.start();
-
- ppmsum.start();
-
- usbs.init();
-
- i2c_thread.start();
-
- adc_thread.start();
-
- motors.start();
-
- usb_thread.usbs = &usbs;
- usb_thread.start();
-
- while (1) {
- chThdSleepMilliseconds(1000);
- }
-}
+#include "thread.h"
+#include "usbserial.h"
+#include "itg3200.h"
+#include "bma150.h"
+#include "ak8975.h"
+#include "ppmsum.h"
+#include "motormixer.h"
+
+#include <ch.h>
+#include <hal.h>
+
+#include "IMU.h"
+
+class LEDThread : public BaseThread<LEDThread, 128> {
+ public:
+ noreturn_t thread_main() {
+ systime_t time = chTimeNow(); // T0
+ while (TRUE) {
+ time += MS2ST(1000); // Next deadline
+ palClearPad(GPIOA, 5);
+ chThdSleepUntil(time);
+ time += MS2ST(1000); // Next deadline
+ palSetPad(GPIOA, 5);
+ chThdSleepUntil(time);
+ }
+ }
+};
+
+LEDThread led_thread;
+
+PPMSum ppmsum;
+MotorMixer motors;
+
+class USBThread : public BaseThread<USBThread, 256> {
+ private:
+ typedef enum {W_S, W_N, W_V} w_s_t;
+
+ public:
+ USBSerial* usbs;
+
+ uint8_t data[9];
+
+ noreturn_t thread_main() {
+ for(int i = 0; i < 9; i++) {
+ data[i] = 0;
+ }
+
+ w_s_t w_s = W_S;
+ uint8_t w_n = 0;
+
+ while(1) {
+ size_t buffer = usbs->getc();
+ if(buffer >= 0 && buffer < 256) {
+ if(w_s == W_S && buffer == 'S') {
+ w_s = W_N;
+ } else if(w_s == W_N && buffer >= '1' && buffer <= '9') {
+ w_s = W_V;
+ w_n = buffer - '1';
+ } else if(w_s == W_V) {
+ w_s = W_S;
+ data[w_n] = buffer;
+ } else {
+ w_s = W_S;
+ }
+ }
+ }
+ }
+};
+
+USBThread usb_thread;
+USBSerial usbs;
+
+#include "foo.h"
+#include <cmath>
+
+uint8_t syncword[] = {0xff, 0x00, 0xaa, 0x55};
+uint8_t buf[64];
+int16_t* sensordata = (int16_t*)buf;
+
+template<class T>
+inline void saturate(T& var, int absmax) {
+ if(var > absmax) {
+ var = absmax;
+ } else if(var < -absmax) {
+ var = -absmax;
+ }
+}
+
+class I2CThread : public BaseThread<I2CThread, 256> {
+ public:
+ ITG3200 gyro;
+ BMA150 acc;
+ AK8975 magn;
+
+ int16_t x, y, z;
+
+ noreturn_t thread_main() {
+ I2CSensor::enable_bus();
+
+ gyro.init();
+ acc.init();
+ magn.init();
+
+ systime_t time = chTimeNow();
+
+ int32_t pitch_angle_accum = 0;
+ int32_t roll_angle_accum = 0;
+
+ while (1) {
+ gyro.update();
+ acc.update();
+ magn.update();
+ x = gyro.x;
+ y = gyro.y;
+ z = gyro.z;
+
+ IMUupdate(gyro.x * 0.0012141420883438813, gyro.y * 0.0012141420883438813, gyro.z * 0.0012141420883438813, acc.x, acc.y, acc.z);
+
+ //float pitch = asinf(2*(q0*q2 - q3*q1));
+ //int16_t pitch = atan2f(2*(q2*q3 + q0*q1), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767;
+ //int16_t roll = atan2f(2*(-q1*q3 + q0*q2), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767;
+ //int16_t yaw = atan2f(2*(q2*q1 + q0*q3), 1 - 2 * (q3*q3 + q2*q2)) / M_PI * 32767;
+
+ float norm_x = 2*(q0*q2 - q1*q3);
+ float norm_y = 2*(q0*q1 + q2*q3);
+ float norm_z = (1 - 2*(q1*q1 + q2*q2));
+
+ float elev = acosf(norm_z);
+ float azim = atan2f(norm_y, norm_x);
+
+ int16_t pitch = elev * sinf(azim) / M_PI * 32767;
+ int16_t roll = elev * cosf(azim) / M_PI * 32767;
+ int16_t yaw = 0;
+
+ sensordata[0] = gyro.x;
+ sensordata[1] = gyro.y;
+ sensordata[2] = gyro.z;
+ sensordata[3] = acc.x;
+ sensordata[4] = acc.y;
+ sensordata[5] = acc.z;
+ sensordata[6] = magn.x;
+ sensordata[7] = magn.y;
+ sensordata[8] = magn.z;
+ sensordata[9] = pitch;
+ sensordata[10] = roll;
+ sensordata[11] = yaw;
+
+ usbs.write(syncword, sizeof(syncword));
+ usbs.write(buf, sizeof(buf));
+
+ /*usbprintf(usbs, "%6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d, %6d | %6d, %6d, %6d\r\n",
+ gyro.x, gyro.y, gyro.z,
+ acc.x, acc.y, acc.z,
+ magn.x, magn.y, magn.z,
+ int(q0 * 10000), int(q1 * 10000), int(q2 * 10000), int(q3 * 10000),
+ int(pitch * 10000), int(roll * 10000), int(yaw * 10000));*/
+
+ int16_t pitch_angle_target = (ppmsum.data[1] - 500) * 8;
+ int16_t roll_angle_target = (ppmsum.data[0] - 500) * 8;
+
+ int16_t pitch_angle_error = pitch_angle_target - pitch;
+ int16_t roll_angle_error = roll_angle_target - roll;
+
+ // 25 deg max error.
+ saturate(pitch_angle_error, 4551);
+ saturate(roll_angle_error, 4551);
+
+ pitch_angle_accum += pitch_angle_error;
+ roll_angle_accum += roll_angle_error;
+
+ // 20 deg s max error.
+ saturate(pitch_angle_error, 364088);
+ saturate(roll_angle_error, 364088);
+
+ int32_t pitch_rate_target = (pitch_angle_error * 4 * 65536 + pitch_angle_accum * 98) >> 16;
+ int32_t roll_rate_target = (roll_angle_error * 4 * 65536 + roll_angle_accum * 98) >> 16;
+
+ int16_t pitch_rate_comp = ((pitch_rate_target - (gyro.x * 4000 / 360)) * 6 * 36) >> 16;
+ int16_t roll_rate_comp = ((roll_rate_target - (gyro.y * 4000 / 360)) * 6 * 36) >> 16;
+
+ saturate(pitch_rate_comp, 250);
+ saturate(roll_rate_comp, 250);
+
+ motors.update(ppmsum.data[2], pitch_rate_comp, roll_rate_comp, 0);
+
+ time += MS2ST(10);
+ if(time > chTimeNow()) {
+ chThdSleepUntil(time);
+ }
+ }
+ }
+};
+
+I2CThread i2c_thread;
+
+static const ADCConversionGroup adcgrpcfg = {
+ FALSE,
+ 2,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ ADC_SQR1_NUM_CH(2),
+ 0,
+ ADC_SQR3_SQ2_N(ADC_CHANNEL_IN14) | ADC_SQR3_SQ1_N(ADC_CHANNEL_IN15)
+};
+
+class ADCThread : public BaseThread<ADCThread, 128> {
+ private:
+ adcsample_t adc_samples[2];
+
+ public:
+ noreturn_t thread_main() {
+ adcStart(&ADCD1, NULL);
+
+ systime_t time = chTimeNow();
+ while (TRUE) {
+ adcStartConversion(&ADCD1, &adcgrpcfg, adc_samples, 1);
+ sensordata[12] = adc_samples[0] * 1265 / 1000;
+ sensordata[13] = adc_samples[1] * 2201 / 1000;
+
+ time += MS2ST(1000);
+ chThdSleepUntil(time);
+ }
+ }
+};
+
+ADCThread adc_thread;
+
+int main(void) {
+ halInit();
+ chSysInit();
+
+ led_thread.start();
+
+ ppmsum.start();
+
+ usbs.init();
+
+ i2c_thread.start();
+
+ adc_thread.start();
+
+ motors.start();
+
+ usb_thread.usbs = &usbs;
+ usb_thread.start();
+
+ while (1) {
+ chThdSleepMilliseconds(1000);
+ }
+}
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-22 18:55:37 +0100