diff options
| -rw-r--r-- | main.cpp | 98 | ||||
| -rw-r--r-- | telemetry.cpp | 5 |
2 files changed, 61 insertions, 42 deletions
@@ -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); |