summaryrefslogtreecommitdiff
path: root/main.cpp
diff options
context:
space:
mode:
authorAtle H. Havsø <atle@havso.net>2014-06-28 18:40:37 +0200
committerAtle H. Havsø <atle@havso.net>2014-06-28 18:40:37 +0200
commite5d4b9bb19623c9e643faaaa26e064c891b22afe (patch)
tree69ef690b1482bc8bf3ac1758f651173e6a7010a4 /main.cpp
parentc91fecf862df22568b55093ffa1fac9edd254142 (diff)
Initial commit with working USB sending of data to host computer.
Diffstat (limited to 'main.cpp')
-rw-r--r--main.cpp410
1 files changed, 24 insertions, 386 deletions
diff --git a/main.cpp b/main.cpp
index aaa3661..24962f4 100644
--- a/main.cpp
+++ b/main.cpp
@@ -1,430 +1,68 @@
#include <rcc/rcc.h>
-#include <interrupt/interrupt.h>
-#include <os/thread.h>
#include <os/time.h>
-
-#include <gpio/pin.h>
-
+#include <gpio/gpio.h>
#include <usb/usb.h>
#include <usb/descriptor.h>
-#include <i2c/i2c.h>
-
-#include <timer/timer.h>
-
-#include "lsm303dlm.h"
-#include "l3gd20.h"
-
-#include "ppmsum.h"
-
-static Pin& led_status = PA4;
-static Pin& led_error = PC4;
-
-static Pin& usb_vbus = PB13;
-static Pin& usb_dm = PB14;
-static Pin& usb_dp = PB15;
-
-static Pin& jtag_tdo = PC3;
-static Pin& jtag_tms = PC13;
-static Pin& jtag_tck = PC14;
-static Pin& jtag_tdi = PC15;
-
-static Pin& i2c_scl = PB10;
-static Pin& i2c_sda = PB11;
-
-static Pin& cs_pressure = PB4;
-static Pin& cs_gyro = PB5;
-
-static Pin& esc_power = PA1;
-static Pin pwm_outputs[] = {PC6, PC7, PC8, PC9};
-
-volatile float g_throttle = 0;
-
-class PID {
- float i_acc;
- float last_error;
-
- const float dt = (1.0 / 400);
-
- public:
- float Kp;
- float Ki;
- float Kd;
-
- PID() : i_acc(0), last_error(0), Kp(0), Ki(0), Kd(0) {}
-
- float update(float error) {
- float corr = Kp * error + (i_acc += Ki * error * dt) + Kd * (error - last_error) / dt;
-
- last_error = error;
-
- return corr;
- }
-
- void reset() {
- i_acc = 0;
- last_error = 0;
- }
-};
-
-PID pid_x;
-PID pid_y;
-PID pid_z;
-
-volatile uint32_t gyro_calib = 0;
-float gyro_calib_x = 0;
-float gyro_calib_y = 0;
-float gyro_calib_z = 0;
+Pin usb_vbus = GPIOB[5];
+Pin usb_dm = GPIOA[11];
+Pin usb_dp = GPIOA[12];
auto dev_desc = device_desc(0x200, 0, 0, 0, 64, 0x1234, 0x5678, 0, 0, 0, 0, 1);
-auto conf_desc = configuration_desc(3, 1, 0, 0xc0, 0,
- interface_desc(0, 0, 3, 0xff, 0x00, 0x00, 0,
- endpoint_desc(0x01, 0x02, 64, 0), // OUT
- endpoint_desc(0x81, 0x02, 64, 0), // IN
- endpoint_desc(0x82, 0x01, 64, 1) // IN ISO
+auto conf_desc = configuration_desc(1, 1, 0, 0xc0, 0,
+ interface_desc(1, 0, 1, 0xff, 0x00, 0x00, 0,
+ endpoint_desc(0x81, 0x02, 64, 0) // IN
)
);
desc_t dev_desc_p = {sizeof(dev_desc), (void*)&dev_desc};
desc_t conf_desc_p = {sizeof(conf_desc), (void*)&conf_desc};
-USB_otg usb(OTG_HS, dev_desc_p, conf_desc_p);
-
-class USB_I2C : public USB_class_driver {
- private:
- USB_generic& usb;
- I2C_t& i2c;
-
- bool i2c_read(uint16_t wValue, uint16_t wIndex, uint16_t wLength) {
- uint8_t buf[wLength];
- i2c.read_reg(wValue, wIndex, wLength, buf);
-
- usb.write(0, (uint32_t*)buf, wLength);
- return true;
- }
-
- public:
- USB_I2C(USB_generic& usbd, I2C_t& i2c_bus) : usb(usbd), i2c(i2c_bus) {
- usb.register_driver(this);
- }
-
- protected:
- virtual SetupStatus handle_setup(uint8_t bmRequestType, uint8_t bRequest, uint16_t wValue, uint16_t wIndex, uint16_t wLength) {
- if(bmRequestType == 0xc0 && bRequest == 0xf0) {
- return i2c_read(wValue, wIndex, wLength) ? SetupStatus::Ok : SetupStatus::Stall;
- }
-
- return SetupStatus::Unhandled;
- }
-};
-
-USB_I2C usb_i2c(usb, I2C2);
-
-class USB_JTAG : public USB_class_driver {
- private:
- USB_generic& usb;
-
- bool jtag_tick(bool tdi, bool tms) {
- bool tdo = jtag_tdo.get();
- jtag_tdi.set(tdi);
- jtag_tms.set(tms);
- jtag_tck.on();
- for(uint32_t i = 0; i < 1000; i++) {
- asm volatile("nop");
- }
- jtag_tck.off();
- for(uint32_t i = 0; i < 1000; i++) {
- asm volatile("nop");
- }
- return tdo;
- }
-
- bool jtag_shift(uint16_t wValue, uint16_t wIndex, uint16_t wLength) {
- if(wLength > 16) {
- return false;
- }
-
- uint32_t tdo = 0;
-
- for(int16_t i = 0; i < wLength; i++) {
- tdo |= jtag_tick(wValue & 1, wIndex & 1) ? 1 << i : 0;
- wValue >>= 1;
- wIndex >>= 1;
- }
-
- usb.write(0, &tdo, (wLength + 7) >> 3);
- return true;
- }
-
- public:
- USB_JTAG(USB_generic& usbd) : usb(usbd) {
- usb.register_driver(this);
- }
-
- protected:
- virtual SetupStatus handle_setup(uint8_t bmRequestType, uint8_t bRequest, uint16_t wValue, uint16_t wIndex, uint16_t wLength) {
- if(bmRequestType == 0xc0 && bRequest == 0xff) {
- return jtag_shift(wValue, wIndex, wLength) ? SetupStatus::Ok : SetupStatus::Stall;
- }
-
- return SetupStatus::Unhandled;
- }
-};
-
-USB_JTAG usb_jtag(usb);
+USB_f1 usb(USB, dev_desc_p, conf_desc_p);
class USB_TM : public USB_class_driver {
private:
USB_generic& usb;
-
uint32_t buf[16];
public:
USB_TM(USB_generic& usbd) : usb(usbd) {
usb.register_driver(this);
}
-
+
protected:
virtual void handle_set_configuration(uint8_t configuration) {
- if(configuration) {
- usb.hw_conf_ep(0x01, EPType::Bulk, 64);
+ if (configuration) {
usb.hw_conf_ep(0x81, EPType::Bulk, 64);
- usb.hw_conf_ep(0x82, EPType::Isochronous, 64);
usb.register_out_handler(this, 1);
}
}
-
- virtual void handle_out(uint8_t ep, uint32_t len) {
- if(ep == 1 && len) {
- usb.read(ep, buf, len);
-
- if(len == 2) {
- uint8_t* bufp = (uint8_t*)buf;
-
- switch(bufp[0]) {
- case 2:
- g_throttle = float(bufp[1]) / 128.0f * 0.7;
- break;
-
- case 3:
- pid_x.Kp = float(bufp[1]) / 128.0f * 0.4;
- pid_y.Kp = float(bufp[1]) / 128.0f * 0.4;
- break;
-
- case 4:
- pid_x.Ki = float(bufp[1]) / 128.0f * 0.4;
- pid_y.Ki = float(bufp[1]) / 128.0f * 0.4;
- break;
-
- case 5:
- pid_x.Kd = float(bufp[1]) / 128.0f * 0.4;
- pid_y.Kd = float(bufp[1]) / 128.0f * 0.4;
- break;
-
- case 6:
- pid_z.Kp = float(bufp[1]) / 128.0f * 0.4;
- break;
-
- case 7:
- pid_z.Kp = float(bufp[1]) / 128.0f * 0.4;
- break;
-
- case 8:
- pid_z.Kp = float(bufp[1]) / 128.0f * 0.4;
- break;
-
- case 23:
- if(bufp[1]) {
- gyro_calib = 256;
- gyro_calib_x = 0;
- gyro_calib_y = 0;
- gyro_calib_z = 0;
- }
- break;
- }
- }
- }
- }
};
USB_TM usb_tm(usb);
-template<>
-void interrupt<(Interrupt::IRQ)77>() {
- usb.process();
-}
-
-void usb_main() {
- usb_vbus.set_mode(Pin::Input);
- usb_dm.set_mode(Pin::AF);
- usb_dm.set_pull(Pin::PullNone);
- usb_dm.set_af(12);
- usb_dp.set_mode(Pin::AF);
- usb_dp.set_pull(Pin::PullNone);
- usb_dp.set_af(12);
-
- RCC.enable(RCC.OTGHS);
- //Interrupt::enable((Interrupt::IRQ)77);
-
- usb.init();
-
- while(1) {
- usb.process();
- Thread::yield();
- }
-}
-
-uint32_t usb_stack[1024];
-
-Thread usb_thread(usb_stack, sizeof(usb_stack), usb_main);
-
-L3GD20 gyro(cs_gyro, SPI1);
-LSM303DLM_A accel(I2C2);
-LSM303DLM_M magn(I2C2);
-
-PPMSum ppmsum;
-
int main() {
+ rcc_init();
+
// Initialize system timer.
- STK.LOAD = 168000000 / 8 / 1000; // 1000 Hz.
+ STK.LOAD = 72000000 / 8 / 1000; // 1000 Hz
STK.CTRL = 0x03;
RCC.enable(RCC.GPIOA);
- RCC.enable(RCC.GPIOB);
- RCC.enable(RCC.GPIOC);
- RCC.enable(RCC.GPIOD);
-
- led_status.set_mode(Pin::Output);
- led_status.off();
- led_error.set_mode(Pin::Output);
- led_error.off();
-
- jtag_tdi.set_mode(Pin::Output);
- jtag_tms.set_mode(Pin::Output);
- jtag_tck.set_mode(Pin::Output);
- jtag_tdo.set_mode(Pin::Input);
-
- RCC.enable(RCC.I2C2);
- I2C2.enable(i2c_scl, i2c_sda);
-
- usb_thread.start();
-
- RCC.enable(RCC.SPI1);
- PA5.set_mode(Pin::AF);
- PA5.set_af(5);
- PA6.set_mode(Pin::AF);
- PA6.set_af(5);
- PA7.set_mode(Pin::AF);
- PA7.set_af(5);
-
- cs_gyro.on();
- cs_gyro.set_mode(Pin::Output);
- cs_pressure.on();
- cs_pressure.set_mode(Pin::Output);
-
- SPI1.reg.CR1 = (1 << 9) | (1 << 8) | (1 << 6) | (3 << 3) | (1 << 2) | (1 << 1) | (1 << 0); // SSM, SSI, SPE, 84/16MHz, MSTR, CPOL, CPHA
-
- Time::sleep(1000);
-
- gyro.init();
- accel.init();
- magn.init();
-
- PA0.set_mode(Pin::AF);
- PA0.set_af(1);
- ppmsum.enable();
-
- for(Pin& p : pwm_outputs) {
- p.set_af(3);
- p.set_mode(Pin::AF);
- }
-
- RCC.enable(RCC.TIM8);
- TIM8.PSC = 168 - 1;
- TIM8.ARR = 2500;
- TIM8.CCER = 0x1111;
- TIM8.CCMR1 = 0x6868;
- TIM8.CCMR2 = 0x6868;
- TIM8.BDTR = (1 << 15);
- TIM8.CCR1 = 1000;
- TIM8.CCR2 = 1000;
- TIM8.CCR3 = 1000;
- TIM8.CCR4 = 1000;
- TIM8.CR1 = 0x05;
+ usb_dm.set_mode(Pin::AF);
+ usb_dm.set_af(14);
+ usb_dp.set_mode(Pin::AF);
+ usb_dp.set_af(14);
- float abst = 0;
+ RCC.enable(RCC.USB);
+ usb.init();
while(1) {
- led_error.toggle();
-
- // Wait for new period.
- while(!(TIM8.SR & 1)) {
- Thread::yield();
- }
-
- TIM8.SR = 0;
-
- float throttle = g_throttle;
-
- gyro.update();
- //accel.update();
- //magn.update();
-
- if(gyro_calib) {
- gyro_calib--;
- gyro_calib_x += gyro.x / 256;
- gyro_calib_y += gyro.y / 256;
- gyro_calib_z += gyro.z / 256;
- }
-
- gyro.x -= gyro_calib_x;
- gyro.y -= gyro_calib_y;
- gyro.z -= gyro_calib_z;
-
- float stab_x = pid_x.update(-gyro.x);
- float stab_y = pid_y.update(-gyro.y);
- float stab_z = pid_z.update(-gyro.z);
-
- if(throttle < 0.05) {
- stab_x = 0;
- stab_y = 0;
- stab_z = 0;
- pid_x.reset();
- pid_y.reset();
- pid_z.reset();
- }
-
- float motors[] = {
- throttle - stab_x - stab_y + stab_z,
- throttle - stab_x + stab_y - stab_z,
- throttle + stab_x + stab_y + stab_z,
- throttle + stab_x - stab_y - stab_z
- };
-
- TIM8.CCR1 = 1000 + motors[0] * 1000;
- TIM8.CCR2 = 1000 + motors[1] * 1000;
- TIM8.CCR3 = 1000 + motors[2] * 1000;
- TIM8.CCR4 = 1000 + motors[3] * 1000;
-
- float buf[] = {
- abst,
- gyro.x,
- gyro.y,
- gyro.z,
- stab_x,
- stab_y,
- stab_z,
- throttle,
- 0,
- };
-
- if(usb.ep_ready(2)) {
- usb.write(2, (uint32_t*)buf, sizeof(buf));
- } else {
- usb_rblog.log("Busy.");
+ usb.process();
+ if(usb.ep_ready(1)) {
+ char a = 'A';
+ usb.write(1, (uint32_t*)&a, sizeof(a));
}
-
- abst += 1.0 / 400.0;
}
-}
+} \ No newline at end of file