Added debug routines.

lib/debug.cc

@@ -0,0 +1,89 @@
+#include <debug.h>
+#include <cassert>
+#include <cstdarg>
+
+void Debug::info(const char* pmsg, ...)
+{
+    print("info: ");
+
+    va_list args;
+    va_start(args, pmsg);
+    vprint(pmsg, args);
+    print("\r\n");
+}
+
+void Debug::error(const char* pmsg, ...)
+{
+    print("error: ");
+
+    va_list args;
+    va_start(args, pmsg);
+    vprint(pmsg, args);
+    print("\r\n");
+}
+
+void Debug::print(const char* pmsg)
+{
+    const char* pstop = pmsg + 100;
+
+    for (; *pmsg != 0 && pmsg < pstop; pmsg++) {
+        putch(*pmsg);
+    }
+}
+
+void Debug::vprint(const char* pmsg, va_list args)
+{
+    const char* pstop = pmsg + 100;
+
+    for (; *pmsg != 0 && pmsg < pstop; pmsg++) {
+        char ch = *pmsg;
+
+        if (ch == '%') {
+            ch = *++pmsg;
+
+            switch (ch) {
+            case '%':
+                putch(ch);
+                break;
+            case 'd':
+                printn(va_arg(args, int), 10, true);
+                break;
+            case 'u':
+                printn(va_arg(args, int), 10, false);
+                break;
+            case 'x':
+                printn(va_arg(args, int), 16, false);
+                break;
+            default:
+                assert(false);
+                break;
+            }
+        } else {
+            putch(ch);
+        }
+    }
+}
+
+void Debug::printn(int value, unsigned int base, bool is_signed)
+{
+    static const char digits[] = "0123456789abcdef";
+
+    if (is_signed && value < 0) {
+        putch('-');
+        value = -value;
+    }
+
+    unsigned int v = value;
+    char stack[11];
+    char* p = stack + sizeof(stack);
+    *--p = '\0';
+
+    do {
+        unsigned int next = v / base;
+        int remainder = v - next * base;
+        *--p = digits[remainder];
+        v = next;
+    } while (v != 0);
+
+    print(p);
+}

lib/debug.h

@@ -0,0 +1,21 @@
+/*
+ * Debug message and assertion support.
+ */
+
+#pragma once
+
+#include <cstdarg>
+
+class Debug
+{
+public:
+    static void info(const char* pmsg, ...);
+    static void error(const char* pmsg, ...);
+
+private:
+    static void print(const char* pmsg);
+    static void printn(int value, unsigned int base, bool is_signed);
+    static void vprint(const char* pmsg, va_list args);
+
+    static void putch(char ch);
+};

platform/posix/timer.cc

@@ -0,0 +1,30 @@
+#include "timer.h"
+#include "debug.h"
+
+struct Binding;
+
+struct Binding
+{
+    Timer* ptimer;
+    const Timer::Fixed* pfixed;
+};
+
+Binding _bindings[50];
+
+void Timer::tick_all()
+{
+    for (Binding* pbinding = _bindings; pbinding->ptimer != nullptr; pbinding++) {
+        pbinding->ptimer->tick(*pbinding->pfixed);
+    }
+}
+
+void Timer::bind(Timer& timer, const Timer::Fixed& fixed)
+{
+    for (Binding* pbinding = _bindings; true; pbinding++) {
+        if (pbinding->ptimer == nullptr) {
+            pbinding->ptimer = &timer;
+            pbinding->pfixed = &fixed;
+            break;
+        }
+    }
+}

platform/posix/usblink.cc

@@ -0,0 +1,19 @@
+#include <cstdint>
+#include <algorithm>
+#include <cstring>
+
+#include "usblink.h"
+
+USBLink::USBLink()
+    : tx_()
+{
+}
+
+void USBLink::write(const uint8_t* data, int length)
+{
+}
+
+bool USBLink::flush()
+{
+    return true;
+}

platform/stm32/start.cc

@@ -0,0 +1,82 @@
+#include <cstring>
+#include <cstdint>
+
+#include <vectors.h>
+
+#include "libmaple/nvic.h"
+#include "libmaple/usart.h"
+
+extern uint8_t __bss_start;
+extern uint8_t __bss_end;
+
+extern const uint8_t __data_load;
+extern uint8_t __data_start;
+extern uint8_t __data_end;
+
+typedef void (*init_function)(void);
+
+extern init_function __init_array_start;
+extern init_function __init_array_end;
+
+extern "C" int main(void);
+
+/** Main function.  Called by the startup code. */
+void _start()
+{
+  nvic_globalirq_disable();
+  nvic_init((uint32_t)&vectors, 0);
+
+  memcpy(&__data_start, &__data_load, &__data_end - &__data_start);
+  memset(&__bss_start, 0, &__bss_end - &__bss_start);
+
+  for (init_function* i = &__init_array_start; i < &__init_array_end; i++) {
+      (**i)();
+  }
+
+  nvic_globalirq_enable();
+  main();
+
+  for (;;) {
+  }
+}
+
+__attribute__((naked))
+static void irq_default()
+{
+  for (;;) {}
+}
+
+__attribute__((section(".vectors")))
+const struct Vectors vectors =
+{
+  .stack_top = &__stack_top,
+  .reset = _start,
+
+  irq_default, irq_default, irq_default, irq_default, irq_default,
+  {},
+  .svc = irq_default,
+  irq_default, 0, irq_default,
+
+  irq_default,
+
+  irq_default,
+  irq_default,
+  irq_default,
+  irq_default,
+  irq_default,
+  irq_default,
+  irq_default,
+  irq_default,
+  irq_default,
+  irq_default,
+  irq_default,
+  irq_default,
+  irq_default,
+  irq_default,
+  irq_default,
+  irq_default,
+  irq_default,
+  irq_default,
+  irq_default,
+  irq_default,
+};

roverif/init.cc → roverif/hal-coptercontrol.cc

@@ -1,16 +1,104 @@
-#include "libmaple/rcc.h"
-#include "libmaple/gpio.h"
-#include "libmaple/systick.h"
-#include "libmaple/flash.h"
-#include "libmaple/usart.h"
-#include "libmaple/delay.h"
+/*
+ * CopterControl specific implementation.
+ */
+
+#include "roverif.h"
+
+#include "libmaple/scb.h"
 #include "libmaple/timer.h"
+#include "libmaple/usart.h"
+#include "libmaple/gpio.h"
 #include "libmaple/iwdg.h"
 #include "libmaple/usb_cdcacm.h"
 #include "libmaple/usb/stm32f1/usb_reg_map.h"
+#include "libmaple/systick.h"
 
-#include "board.h"
-#include "roverif.h"
+#include <platform/stm32/vectors.h>
+
+volatile int stuck;
+
+
+void irq_timer4ch1()
+{
+    timer_gen_reg_map& regs = *TIMER4->regs.gen;
+    RoverIf::pwmin.irq(0, regs.CCR1, &regs.CCER, TIMER_CCER_CC1P);
+}
+
+void irq_timer3ch2()
+{
+    timer_gen_reg_map& regs = *TIMER3->regs.gen;
+    RoverIf::pwmin.irq(1, regs.CCR2, &regs.CCER, TIMER_CCER_CC2P);
+}
+
+void irq_timer3ch3()
+{
+    timer_gen_reg_map& regs = *TIMER3->regs.gen;
+    RoverIf::pwmin.irq(2, regs.CCR3, &regs.CCER, TIMER_CCER_CC3P);
+}
+
+void irq_timer3ch4()
+{
+    timer_gen_reg_map& regs = *TIMER3->regs.gen;
+    RoverIf::pwmin.irq(3, regs.CCR4, &regs.CCER, TIMER_CCER_CC4P);
+}
+
+void irq_timer2ch1()
+{
+    timer_gen_reg_map& regs = *TIMER2->regs.gen;
+    RoverIf::pwmin.irq(4, regs.CCR1, &regs.CCER, TIMER_CCER_CC1P);
+}
+
+void irq_timer2ch2()
+{
+    timer_gen_reg_map& regs = *TIMER2->regs.gen;
+    RoverIf::pwmin.irq(5, regs.CCR2, &regs.CCER, TIMER_CCER_CC2P);
+}
+
+__attribute__((noinline))
+static void _systick(uint32_t* sp)
+{
+    RoverIf::switcher.trigger(SysTickID);
+
+    if (++stuck == 1000) {
+        Debug::error("Stuck at %x", sp[6]);
+    }
+}
+
+void systick()
+{
+    uint32_t* sp;
+    asm volatile ("mov %0, sp" : "=r" (sp));
+    _systick(sp);
+}
+
+void RoverIf::set_status_led(bool level)
+{
+    if (level) {
+        GPIOA_BASE->ODR &= ~(1 << 6);
+    } else {
+        GPIOA_BASE->ODR |= 1 << 6;
+    }
+}
+
+void RoverIf::poll_hal()
+{
+    if (usb_cdcacm_data_available()) {
+        uint8_t rx[32];
+        int got = usb_cdcacm_rx(rx, sizeof(rx));
+        link.feed(rx, got);
+    }
+
+    iwdg_feed();
+}    
+
+void RoverIf::wait()
+{
+    asm ("wfi");
+}
+
+void Debug::putch(char ch)
+{
+}
 
 static void init_usb()
 {
@@ -146,30 +234,41 @@ static void init_timers()
     timer_attach_interrupt(TIMER2, TIMER_CC2_INTERRUPT, irq_timer2ch2);
 }
 
-void init()
+static void init_debug_usart()
 {
-    flash_enable_prefetch();
-    flash_set_latency(FLASH_WAIT_STATE_2);
+    usart_init(USART3);
+    usart_set_baud_rate(USART3, Board::APB1Clock, Board::DebugBaudRate);
+    usart_enable(USART3);
+
+    gpio_set_mode(GPIOA, 2, GPIO_AF_OUTPUT_PP);
+    gpio_set_mode(GPIOA, 6, GPIO_OUTPUT_PP);
+}
 
+static void init_clocks()
+{
     rcc_clk_init(RCC_CLKSRC_PLL, RCC_PLLSRC_HSE, RCC_PLLMUL_9);
     rcc_set_prescaler(RCC_PRESCALER_AHB, RCC_AHB_SYSCLK_DIV_1);
     rcc_set_prescaler(RCC_PRESCALER_APB1, RCC_APB1_HCLK_DIV_2);
     rcc_set_prescaler(RCC_PRESCALER_APB2, RCC_APB2_HCLK_DIV_1);
+}
 
-    systick_init(72000 - 1);
-
-    gpio_init_all();
+void RoverIf::init_hal()
+{
+    /* Reset after 500 ms */
+    iwdg_init(IWDG_PRE_32, Board::LSIClock / 32 * 2000/1000);
 
-    usart_init(USART2);
-    usart_set_baud_rate(USART2, 72000000/2, 115200);
-    usart_enable(USART2);
+    flash_enable_prefetch();
+    flash_set_latency(FLASH_WAIT_STATE_2);
+    init_clocks();
+    init_debug_usart();
 
-    gpio_set_mode(GPIOA, 2, GPIO_AF_OUTPUT_PP);
-    gpio_set_mode(GPIOA, 6, GPIO_OUTPUT_PP);
+    systick_init(Board::AHBClock / Board::Ticks - 1);
 
+    gpio_init_all();
     init_timers();
     init_usb();
+}
 
-    /* Reset after 500 ms */
-    iwdg_init(IWDG_PRE_32, 40000 / 32 * 2000/1000);
+void RoverIf::start_hal()
+{
 }

roverif/hal-posix.cc

@@ -0,0 +1,60 @@
+#include "roverif.h"
+#include "hal.h"
+#include "board.h"
+
+#include <cstdio>
+#include <time.h>
+
+static struct timespec _epoch;
+static int64_t _ticks;
+
+void HAL::init()
+{
+}
+
+void HAL::start()
+{
+    clock_gettime(CLOCK_MONOTONIC, &_epoch);
+}
+
+void HAL::poll()
+{
+}
+
+void HAL::wait()
+{
+    struct timespec now;
+    clock_gettime(CLOCK_MONOTONIC, &now);
+    int64_t elapsed = (now.tv_sec - _epoch.tv_sec) * Board::Ticks + (now.tv_nsec - _epoch.tv_nsec) / (1000000000 / Board::Ticks);
+
+    if (elapsed == _ticks) {
+        struct timespec sleep_for = { 0, 1000000 };
+        nanosleep(&sleep_for, NULL);
+    } else {
+        _ticks++;
+        RoverIf::switcher.trigger(SysTickID);
+        fflush(stdout);
+    }
+}
+
+void HAL::set_status_led(bool on)
+{
+    if (on) {
+        ::putchar('*');
+    } else {
+        ::putchar('.');
+    }
+}
+
+void Debug::putch(char ch)
+{
+    ::putchar(ch);
+}
+
+int main()
+{
+    RoverIf::init();
+    RoverIf::run();
+
+    return 0;
+}

roverif/hal.h

@@ -0,0 +1,10 @@
+class HAL
+{
+public:
+    static void init();
+    static void start();
+    static void poll();
+    static void wait();
+
+    static void set_status_led(bool on);
+};