diff options
Diffstat (limited to 'src/modules/px4iofirmware/px4io.c')
| -rw-r--r-- | src/modules/px4iofirmware/px4io.c | 135 |
1 files changed, 108 insertions, 27 deletions
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index bc8dfc116..d4c25911e 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -45,11 +45,13 @@ #include <string.h> #include <poll.h> #include <signal.h> +#include <crc32.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_hrt.h> #include <systemlib/perf_counter.h> +#include <systemlib/pwm_limit/pwm_limit.h> #include <stm32_uart.h> @@ -64,10 +66,7 @@ struct sys_state_s system_state; static struct hrt_call serial_dma_call; -#ifdef CONFIG_STM32_I2C1 -/* store i2c reset count XXX this should be a register, together with other error counters */ -volatile uint32_t i2c_loop_resets = 0; -#endif +pwm_limit_t pwm_limit; /* * a set of debug buffers to allow us to send debug information from ISRs @@ -119,6 +118,48 @@ show_debug_messages(void) } } +static void +heartbeat_blink(void) +{ + static bool heartbeat = false; + LED_BLUE(heartbeat = !heartbeat); +} + +static uint64_t reboot_time; + +/** + schedule a reboot in time_delta_usec microseconds + */ +void schedule_reboot(uint32_t time_delta_usec) +{ + reboot_time = hrt_absolute_time() + time_delta_usec; +} + +/** + check for a scheduled reboot + */ +static void check_reboot(void) +{ + if (reboot_time != 0 && hrt_absolute_time() > reboot_time) { + up_systemreset(); + } +} + +static void +calculate_fw_crc(void) +{ +#define APP_SIZE_MAX 0xf000 +#define APP_LOAD_ADDRESS 0x08001000 + // compute CRC of the current firmware + uint32_t sum = 0; + for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) { + uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS); + sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum); + } + r_page_setup[PX4IO_P_SETUP_CRC] = sum & 0xFFFF; + r_page_setup[PX4IO_P_SETUP_CRC+1] = sum >> 16; +} + int user_start(int argc, char *argv[]) { @@ -131,6 +172,9 @@ user_start(int argc, char *argv[]) /* configure the high-resolution time/callout interface */ hrt_init(); + /* calculate our fw CRC so FMU can decide if we need to update */ + calculate_fw_crc(); + /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. @@ -147,8 +191,15 @@ user_start(int argc, char *argv[]) LED_BLUE(false); LED_SAFETY(false); - /* turn on servo power */ + /* turn on servo power (if supported) */ +#ifdef POWER_SERVO POWER_SERVO(true); +#endif + + /* turn off S.Bus out (if supported) */ +#ifdef ENABLE_SBUS_OUT + ENABLE_SBUS_OUT(false); +#endif /* start the safety switch handler */ safety_init(); @@ -159,10 +210,11 @@ user_start(int argc, char *argv[]) /* initialise the control inputs */ controls_init(); -#ifdef CONFIG_STM32_I2C1 - /* start the i2c handler */ - i2c_init(); -#endif + /* set up the ADC */ + adc_init(); + + /* start the FMU interface */ + interface_init(); /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); @@ -176,30 +228,51 @@ user_start(int argc, char *argv[]) struct mallinfo minfo = mallinfo(); lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); -#if 0 - /* not enough memory, lock down */ - if (minfo.mxordblk < 500) { + /* initialize PWM limit lib */ + pwm_limit_init(&pwm_limit); + + /* + * P O L I C E L I G H T S + * + * Not enough memory, lock down. + * + * We might need to allocate mixers later, and this will + * ensure that a developer doing a change will notice + * that he just burned the remaining RAM with static + * allocations. We don't want him to be able to + * get past that point. This needs to be clearly + * documented in the dev guide. + * + */ + if (minfo.mxordblk < 600) { + lowsyslog("ERR: not enough MEM"); bool phase = false; - if (phase) { - LED_AMBER(true); - LED_BLUE(false); - } else { - LED_AMBER(false); - LED_BLUE(true); - } + while (true) { - phase = !phase; - usleep(300000); + if (phase) { + LED_AMBER(true); + LED_BLUE(false); + } else { + LED_AMBER(false); + LED_BLUE(true); + } + up_udelay(250000); + + phase = !phase; + } } -#endif + + /* Start the failsafe led init */ + failsafe_led_init(); /* * Run everything in a tight loop. */ uint64_t last_debug_time = 0; + uint64_t last_heartbeat_time = 0; for (;;) { /* track the rate at which the loop is running */ @@ -215,20 +288,28 @@ user_start(int argc, char *argv[]) controls_tick(); perf_end(controls_perf); - /* check for debug activity */ + if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) { + last_heartbeat_time = hrt_absolute_time(); + heartbeat_blink(); + } + + check_reboot(); + + /* check for debug activity (default: none) */ show_debug_messages(); - /* post debug state at ~1Hz */ + /* post debug state at ~1Hz - this is via an auxiliary serial port + * DEFAULTS TO OFF! + */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { struct mallinfo minfo = mallinfo(); - isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", + isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, - (unsigned)i2c_loop_resets, (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } |