From e5d4b9bb19623c9e643faaaa26e064c891b22afe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Atle=20H=2E=20Havs=C3=B8?= Date: Sat, 28 Jun 2014 18:40:37 +0200 Subject: Initial commit with working USB sending of data to host computer. --- SConstruct | 11 +- laks | 2 +- main.cpp | 410 ++++--------------------------------------------------------- 3 files changed, 34 insertions(+), 389 deletions(-) diff --git a/SConstruct b/SConstruct index d67ac16..73bc8f7 100644 --- a/SConstruct +++ b/SConstruct @@ -2,14 +2,21 @@ import os env = Environment( ENV = os.environ, + tools = ['gnulink', 'gcc', 'g++', 'gas', 'ar'] ) SConscript('laks/build_rules') -env.SelectMCU('stm32f405rg') +env.SelectMCU('stm32f303cc') env.Append( CPPPATH = ['drivers'], ) -env.Firmware('suzumebachi.elf', Glob('*.cpp') + Glob('drivers/*.cpp')) +#env.Firmware('suzumebachi.elf', Glob('*.cpp') + Glob('drivers/*.cpp')) +env.Firmware('suzumebachi.elf', Glob('*.cpp')) + +env.Command('suzumebachi.bin', 'suzumebachi.elf', '${TOOLCHAIN}objcopy -O binary $SOURCE $TARGET') +env.Command('flash', ['suzumebachi.bin'], 'dfu-util.exe -d 0483:df11 -c 1 -a 0 -s 0x08000000 -D suzumebachi.bin') + +Default('suzumebachi.bin') \ No newline at end of file diff --git a/laks b/laks index c982426..fab8321 160000 --- a/laks +++ b/laks @@ -1 +1 @@ -Subproject commit c982426fd492b8645a04698f5a2b24e242902100 +Subproject commit fab8321b68896a8a9ff085586652ed4d834ba173 diff --git a/main.cpp b/main.cpp index aaa3661..24962f4 100644 --- a/main.cpp +++ b/main.cpp @@ -1,430 +1,68 @@ #include -#include -#include #include - -#include - +#include #include #include -#include - -#include - -#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 -- cgit v1.2.3