summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--main.cpp98
-rw-r--r--telemetry.cpp5
2 files changed, 61 insertions, 42 deletions
diff --git a/main.cpp b/main.cpp
index 309ff60..47e7c51 100644
--- a/main.cpp
+++ b/main.cpp
@@ -65,15 +65,15 @@ AHRS ahrs(I2C1);
volatile uint16_t motors[4];
-GPS gps;
+//GPS gps;
void gps_thread_main() {
while(1) {
- P<GPSMsg> msg = gps.read();
+ //P<GPSMsg> msg = gps.read();
- if(msg->n < 128) {
- xbee_send(2, msg->n, msg->buf);
- }
+ //if(msg->n < 128) {
+ // xbee_send(2, msg->n, msg->buf);
+ //}
}
}
@@ -100,9 +100,9 @@ int main() {
led_red.set_mode(Pin::Output);
led_blue.set_mode(Pin::Output);
+ //RCC.enable(RCC.DMA1);
+ //RCC.enable(RCC.ADC1);
- RCC.enable(RCC.DMA1);
- RCC.enable(RCC.ADC1);
PB6.set_af(7);
PB7.set_af(7);
PB6.set_mode(Pin::Output);
@@ -114,58 +114,76 @@ int main() {
I2C1.enable(PB8, PB9);
ahrs.init();
- PPMSum ppmsum;
- ppmsum.enable();
+ //PPMSum ppmsum;
+ //ppmsum.enable();
+
+ //RCC.enable(RCC.TIM2);
+ //TIM2.PSC = 72;
+ //TIM2.ARR = 5000;
+ //TIM2.CCER = 0x1111;
+ //TIM2.CCMR1 = 0x6868;
+ //TIM2.CCMR2 = 0x6868;
- RCC.enable(RCC.TIM2);
- TIM2.PSC = 72;
- TIM2.ARR = 5000;
- TIM2.CCER = 0x1111;
- TIM2.CCMR1 = 0x6868;
- TIM2.CCMR2 = 0x6868;
+ //TIM2.CR1 = 0x05;
- TIM2.CR1 = 0x05;
+ //PID pid_pitch(6000, 0, 0);
+ //PID pid_roll(6000, 0, 0);
+ //PID pid_yaw(6000, 0, 0);
- PID pid_pitch(6000, 0, 0);
- PID pid_roll(6000, 0, 0);
- PID pid_yaw(6000, 0, 0);
+ RCC.enable(RCC.USART1);
usart_enable();
- gps.enable();
+ //gps.enable();
telemetry_thread.start();
//gps_thread.start();
while(1) {
+ led_green.on();
+ Time::sleep(100);
+ led_green.off();
+
+ led_yellow.on();
+ Time::sleep(100);
+ led_yellow.off();
+
+ led_red.on();
+ Time::sleep(100);
+ led_red.off();
+
+ led_blue.on();
+ Time::sleep(100);
+ led_blue.off();
+
// Wait for a new update.
- while(!(TIM2.SR & 0x01)) {
- Thread::yield();
- }
- TIM2.SR = 0;
+ //while(!(TIM2.SR & 0x01)) {
+ // Thread::yield();
+ //}
+ //TIM2.SR = 0;
// Update AHRS.
ahrs.update();
// Update filter.
- int16_t throttle = ppmsum.channels[2] - 1000;
- int16_t pitch = pid_pitch.update((ppmsum.channels[1] - 1500) * 1 - ahrs.gyro.x);
- int16_t roll = pid_roll.update((ppmsum.channels[0] - 1500) * 1 - ahrs.gyro.y);
- int16_t yaw = pid_yaw.update((ppmsum.channels[3] - 1500) * -1 - ahrs.gyro.z);
+ //int16_t throttle = ppmsum.channels[2] - 1000;
+ //int16_t pitch = pid_pitch.update((ppmsum.channels[1] - 1500) * 1 - ahrs.gyro.x);
+ //int16_t roll = pid_roll.update((ppmsum.channels[0] - 1500) * 1 - ahrs.gyro.y);
+ //int16_t yaw = pid_yaw.update((ppmsum.channels[3] - 1500) * -1 - ahrs.gyro.z);
- int16_t max = throttle > 250 ? 250 : throttle;
- saturate(pitch, max);
- saturate(roll, max);
- saturate(yaw, max);
+ //int16_t max = throttle > 250 ? 250 : throttle;
+ //saturate(pitch, max);
+ //saturate(roll, max);
+ //saturate(yaw, max);
// Generate motor mix.
- motors[0] = limit(throttle + pitch - roll + yaw, 0, 1000);
- motors[1] = limit(throttle - pitch - roll - yaw, 0, 1000);
- motors[2] = limit(throttle - pitch + roll + yaw, 0, 1000);
- motors[3] = limit(throttle + pitch + roll - yaw, 0, 1000);
+ //motors[0] = limit(throttle + pitch - roll + yaw, 0, 1000);
+ //motors[1] = limit(throttle - pitch - roll - yaw, 0, 1000);
+ //motors[2] = limit(throttle - pitch + roll + yaw, 0, 1000);
+ //motors[3] = limit(throttle + pitch + roll - yaw, 0, 1000);
- TIM2.CCR1 = 1000 + motors[0];
- TIM2.CCR2 = 1000 + motors[1];
- TIM2.CCR3 = 1000 + motors[2];
- TIM2.CCR4 = 1000 + motors[3];
+ //TIM2.CCR1 = 1000 + motors[0];
+ //TIM2.CCR2 = 1000 + motors[1];
+ //TIM2.CCR3 = 1000 + motors[2];
+ //TIM2.CCR4 = 1000 + motors[3];
}
}
diff --git a/telemetry.cpp b/telemetry.cpp
index d44412f..781fb60 100644
--- a/telemetry.cpp
+++ b/telemetry.cpp
@@ -12,6 +12,7 @@ extern AHRS ahrs;
volatile uint16_t dmabuf[2];
void telemetry_main() {
+ /*
ADC1.CR2 = 0x9;
while(ADC1.CR2 & 0x8);
ADC1.CR2 = 0x5;
@@ -28,7 +29,7 @@ void telemetry_main() {
ADC1.CR1 = 0x100;
ADC1.CR2 = 0x103;
ADC1.CR2 = 0x103;
-
+ */
while(1) {
uint16_t buf[] = {
ahrs.gyro.x,
@@ -52,4 +53,4 @@ void telemetry_main() {
uint32_t telemetry_stack[1024];
-Thread telemetry_thread(telemetry_stack, sizeof(telemetry_stack), telemetry_main); \ No newline at end of file
+Thread telemetry_thread(telemetry_stack, sizeof(telemetry_stack), telemetry_main);