diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 794fb679cf..a64d99cd49 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -138,7 +138,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con current_status->flag_system_armed = false; mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM"); usleep(500000); - reboot(); + up_systemreset(); /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ } else { invalid_state = true; diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c index e5ded7328f..2dc3e60c6b 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_init.c +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -294,9 +294,6 @@ __EXPORT int nsh_archinitialize(void) /* Get board information if available */ - /* Initialize the user GPIOs */ - px4fmu_gpio_init(); - #ifdef CONFIG_ADC int adc_state = adc_devinit(); diff --git a/apps/drivers/boards/px4fmu/px4fmu_internal.h b/apps/drivers/boards/px4fmu/px4fmu_internal.h index c58a8a5c46..6550fdf3da 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_internal.h +++ b/apps/drivers/boards/px4fmu/px4fmu_internal.h @@ -47,6 +47,12 @@ #include #include +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include + + /**************************************************************************************************** * Definitions ****************************************************************************************************/ @@ -150,17 +156,6 @@ extern void stm32_spiinitialize(void); -/**************************************************************************************************** - * Name: px4fmu_gpio_init - * - * Description: - * Called to configure the PX4FMU user GPIOs - * - ****************************************************************************************************/ - -extern void px4fmu_gpio_init(void); - - -// XXX additional SPI chipselect functions required? - #endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/apps/drivers/drv_gpio.h b/apps/drivers/drv_gpio.h index 361d731eb7..5b86dd920d 100644 --- a/apps/drivers/drv_gpio.h +++ b/apps/drivers/drv_gpio.h @@ -40,25 +40,75 @@ #include +#ifdef CONFIG_ARCH_BOARD_PX4FMU /* - * GPIO defines come from a board-specific header, as they are shared - * with board-specific logic. + * PX4FMU GPIO numbers. * - * The board-specific header must define: - * GPIO_DEVICE_PATH - * GPIO_RESET - * GPIO_SET_OUTPUT - * GPIO_SET_INPUT - * GPIO_SET_ALT_1 - * GPIO_SET_ALT_2 - * GPIO_SET_ALT_3 - * GPIO_SET_ALT_4 - * GPIO_SET - * GPIO_CLEAR - * GPIO_GET + * For shared pins, alternate function 1 selects the non-GPIO mode + * (USART2, CAN2, etc.) */ +# define GPIO_EXT_1 (1<<0) /**< high-power GPIO 1 */ +# define GPIO_EXT_2 (1<<1) /**< high-power GPIO 1 */ +# define GPIO_MULTI_1 (1<<2) /**< USART2 CTS */ +# define GPIO_MULTI_2 (1<<3) /**< USART2 RTS */ +# define GPIO_MULTI_3 (1<<4) /**< USART2 TX */ +# define GPIO_MULTI_4 (1<<5) /**< USART2 RX */ +# define GPIO_CAN_TX (1<<6) /**< CAN2 TX */ +# define GPIO_CAN_RX (1<<7) /**< CAN2 RX */ -/* Include board-specific GPIO definitions as well. */ -#include +/** + * Default GPIO device - other devices may also support this protocol if + * they also export GPIO-like things. This is always the GPIOs on the + * main board. + */ +# define GPIO_DEVICE_PATH "/dev/px4fmu" + +#endif + +#ifndef GPIO_DEVICE_PATH +# error No GPIO support for this board. +#endif + +/* + * IOCTL definitions. + * + * For all ioctls, the (arg) argument is a bitmask of GPIOs to be affected + * by the operation, with the LSB being the lowest-numbered GPIO. + * + * Note that there may be board-specific relationships between GPIOs; + * applications using GPIOs should be aware of this. + */ +#define _GPIOCBASE 0x6700 +#define GPIOC(_x) _IOC(_GPIOCBASE, _x) + +/** reset all board GPIOs to their default state */ +#define GPIO_RESET GPIOC(0) + +/** configure the board GPIOs in (arg) as outputs */ +#define GPIO_SET_OUTPUT GPIOC(1) + +/** configure the board GPIOs in (arg) as inputs */ +#define GPIO_SET_INPUT GPIOC(2) + +/** configure the board GPIOs in (arg) for the first alternate function (if supported) */ +#define GPIO_SET_ALT_1 GPIOC(3) + +/** configure the board GPIO (arg) for the second alternate function (if supported) */ +#define GPIO_SET_ALT_2 GPIOC(4) + +/** configure the board GPIO (arg) for the third alternate function (if supported) */ +#define GPIO_SET_ALT_3 GPIOC(5) + +/** configure the board GPIO (arg) for the fourth alternate function (if supported) */ +#define GPIO_SET_ALT_4 GPIOC(6) + +/** set the GPIOs in (arg) */ +#define GPIO_SET GPIOC(10) + +/** clear the GPIOs in (arg) */ +#define GPIO_CLEAR GPIOC(11) + +/** read all the GPIOs and return their values in *(uint32_t *)arg */ +#define GPIO_GET GPIOC(12) #endif /* _DRV_GPIO_H */ \ No newline at end of file diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 7943012cc6..f9079c3ff1 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -58,17 +58,21 @@ #include #include -#include +#include #include #include #include +#include /* * HMC5883 internal constants and data structures. */ +#define HMC5883L_BUS PX4_I2C_BUS_ONBOARD +#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 + /* Max measurement rate is 160Hz */ #define HMC5883_CONVERSION_INTERVAL (1000000 / 160) /* microseconds */ @@ -86,8 +90,6 @@ #define ADDR_ID_B 0x0b #define ADDR_ID_C 0x0c -#define HMC5883L_ADDRESS 0x1E - /* modes not changeable outside of driver */ #define HMC5883L_MODE_NORMAL (0 << 0) /* default */ #define HMC5883L_MODE_POSITIVE_BIAS (1 << 0) /* positive bias */ @@ -1100,8 +1102,7 @@ start() errx(1, "already started"); /* create the driver */ - /* XXX HORRIBLE hack - the bus number should not come from here */ - g_dev = new HMC5883(2); + g_dev = new HMC5883(HMC5883L_BUS); if (g_dev == nullptr) goto fail; diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 699cd36d25..b8845aaf18 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -57,6 +57,8 @@ #include #include +#include + #include #include @@ -240,7 +242,8 @@ private: #define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ #define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ -#define MS5611_ADDRESS_1 0x76 /* address select pins pulled high (PX4FMU series v1.6+) */ +#define MS5611_BUS PX4_I2C_BUS_ONBOARD +#define MS5611_ADDRESS_1 PX4_I2C_OBDEV_MS5611 /* address select pins pulled high (PX4FMU series v1.6+) */ #define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */ #define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ @@ -937,8 +940,7 @@ start() errx(1, "already started"); /* create the driver */ - /* XXX HORRIBLE hack - the bus number should not come from here */ - g_dev = new MS5611(2); + g_dev = new MS5611(MS5611_BUS); if (g_dev == nullptr) goto fail; diff --git a/apps/px4/fmu/Makefile b/apps/drivers/px4fmu/Makefile similarity index 96% rename from apps/px4/fmu/Makefile rename to apps/drivers/px4fmu/Makefile index 8317748729..7f7c2a7328 100644 --- a/apps/px4/fmu/Makefile +++ b/apps/drivers/px4fmu/Makefile @@ -39,4 +39,6 @@ APPNAME = fmu PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 2048 +INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common + include $(APPDIR)/mk/app.mk diff --git a/apps/px4/fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp similarity index 65% rename from apps/px4/fmu/fmu.cpp rename to apps/drivers/px4fmu/fmu.cpp index eccdeb78e8..fff713bb5e 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -57,16 +57,18 @@ #include #include #include +#include +#include #include #include #include #include -#include +#include -class FMUServo : public device::CDev +class PX4FMU : public device::CDev { public: enum Mode { @@ -74,18 +76,22 @@ public: MODE_4PWM, MODE_NONE }; - FMUServo(Mode mode, int update_rate); - ~FMUServo(); + PX4FMU(); + ~PX4FMU(); virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual int init(); + int set_mode(Mode mode); + int set_pwm_rate(unsigned rate); + private: static const unsigned _max_actuators = 4; Mode _mode; int _update_rate; + int _current_update_rate; int _task; int _t_actuators; int _t_armed; @@ -107,19 +113,50 @@ private: uint8_t control_group, uint8_t control_index, float &input); + + int pwm_ioctl(file *filp, int cmd, unsigned long arg); + + struct GPIOConfig { + uint32_t input; + uint32_t output; + uint32_t alt; + }; + + static const GPIOConfig _gpio_tab[]; + static const unsigned _ngpio; + + void gpio_reset(void); + void gpio_set_function(uint32_t gpios, int function); + void gpio_write(uint32_t gpios, int function); + uint32_t gpio_read(void); + int gpio_ioctl(file *filp, int cmd, unsigned long arg); + }; +const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1}, + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1}, + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1}, + {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, + {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, +}; + +const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); + namespace { -FMUServo *g_servo; +PX4FMU *g_fmu; } // namespace -FMUServo::FMUServo(Mode mode, int update_rate) : +PX4FMU::PX4FMU() : CDev("fmuservo", "/dev/px4fmu"), - _mode(mode), - _update_rate(update_rate), + _mode(MODE_NONE), + _update_rate(50), _task(-1), _t_actuators(-1), _t_armed(-1), @@ -130,9 +167,10 @@ FMUServo::FMUServo(Mode mode, int update_rate) : _armed(false), _mixers(nullptr) { + _debug_enabled = true; } -FMUServo::~FMUServo() +PX4FMU::~PX4FMU() { if (_task != -1) { /* tell the task we want it to go away */ @@ -156,11 +194,11 @@ FMUServo::~FMUServo() if (_primary_pwm_device) unregister_driver(PWM_OUTPUT_DEVICE_PATH); - g_servo = nullptr; + g_fmu = nullptr; } int -FMUServo::init() +PX4FMU::init() { int ret; @@ -179,12 +217,15 @@ FMUServo::init() _primary_pwm_device = true; } + /* reset GPIOs */ + gpio_reset(); + /* start the IO interface task */ _task = task_spawn("fmuservo", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1024, - (main_t)&FMUServo::task_main_trampoline, + (main_t)&PX4FMU::task_main_trampoline, nullptr); if (_task < 0) { @@ -196,44 +237,73 @@ FMUServo::init() } void -FMUServo::task_main_trampoline(int argc, char *argv[]) +PX4FMU::task_main_trampoline(int argc, char *argv[]) { - g_servo->task_main(); + g_fmu->task_main(); } -void -FMUServo::task_main() +int +PX4FMU::set_mode(Mode mode) { - /* configure for PWM output */ - switch (_mode) { + /* + * Configure for PWM output. + * + * Note that regardless of the configured mode, the task is always + * listening and mixing; the mode just selects which of the channels + * are presented on the output pins. + */ + switch (mode) { case MODE_2PWM: + debug("MODE_2PWM"); /* multi-port with flow control lines as PWM */ /* XXX magic numbers */ up_pwm_servo_init(0x3); + _update_rate = 50; /* default output rate */ break; case MODE_4PWM: + debug("MODE_4PWM"); /* multi-port as 4 PWM outs */ /* XXX magic numbers */ up_pwm_servo_init(0xf); - /* set the update rate for 4 PWM mode only for now */ - up_pwm_servo_set_rate(_update_rate); + _update_rate = 50; /* default output rate */ break; case MODE_NONE: - /* we should never get here... */ + debug("MODE_NONE"); + /* disable servo outputs and set a very low update rate */ + up_pwm_servo_deinit(); + _update_rate = 10; break; - } + default: + return -EINVAL; + } + _mode = mode; + return OK; +} + +int +PX4FMU::set_pwm_rate(unsigned rate) +{ + if ((rate > 500) || (rate < 10)) + return -EINVAL; + + _update_rate = rate; + return OK; +} + +void +PX4FMU::task_main() +{ /* * Subscribe to the appropriate PWM output topic based on whether we are the * primary PWM output or not. */ _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1)); - /* convert the update rate in hz to milliseconds, rounding down if necessary */ - int update_rate_in_ms = int(1000 / _update_rate); - orb_set_interval(_t_actuators, update_rate_in_ms); + /* force a reset of the update rate */ + _current_update_rate = 0; _t_armed = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(_t_armed, 200); /* 5Hz update rate */ @@ -258,7 +328,17 @@ FMUServo::task_main() /* loop until killed */ while (!_task_should_exit) { - /* sleep waiting for data, but no more than 100ms */ + /* handle update rate changes */ + if (_current_update_rate != _update_rate) { + int update_rate_in_ms = int(1000 / _update_rate); + if (update_rate_in_ms < 2) + update_rate_in_ms = 2; + orb_set_interval(_t_actuators, update_rate_in_ms); + up_pwm_servo_set_rate(_update_rate); + _current_update_rate = _update_rate; + } + + /* sleep waiting for data, but no more than a second */ int ret = ::poll(&fds[0], 2, 1000); /* this would be bad... */ @@ -323,7 +403,7 @@ FMUServo::task_main() } int -FMUServo::control_callback(uintptr_t handle, +PX4FMU::control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input) @@ -335,11 +415,43 @@ FMUServo::control_callback(uintptr_t handle, } int -FMUServo::ioctl(file *filp, int cmd, unsigned long arg) +PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret; + + debug("ioctl 0x%04x 0x%08x", cmd, arg); + + /* try it as a GPIO ioctl first */ + ret = gpio_ioctl(filp, cmd, arg); + if (ret != -ENOTTY) + return ret; + + /* if we are in valid PWM mode, try it as a PWM ioctl as well */ + switch(_mode) { + case MODE_2PWM: + case MODE_4PWM: + ret = pwm_ioctl(filp, cmd, arg); + break; + default: + debug("not in a PWM mode"); + break; + } + + /* if nobody wants it, let CDev have it */ + if (ret == -ENOTTY) + ret = CDev::ioctl(filp, cmd, arg); + + return ret; +} + +int +PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) { int ret = OK; int channel; + lock(); + switch (cmd) { case PWM_SERVO_ARM: up_pwm_servo_arm(true); @@ -437,11 +549,19 @@ FMUServo::ioctl(file *filp, int cmd, unsigned long arg) } _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + if (_mixers == nullptr) { + ret = -ENOMEM; + } else { - if (_mixers->load_from_file(path) != 0) { - delete _mixers; - _mixers = nullptr; - ret = -EINVAL; + debug("loading mixers from %s", path); + ret = _mixers->load_from_file(path); + + if (ret != 0) { + debug("mixer load failed with %d", ret); + delete _mixers; + _mixers = nullptr; + ret = -EINVAL; + } } break; @@ -452,6 +572,125 @@ FMUServo::ioctl(file *filp, int cmd, unsigned long arg) break; } + unlock(); + + return ret; +} + +void +PX4FMU::gpio_reset(void) +{ + /* + * Setup default GPIO config - all pins as GPIOs, GPIO driver chip + * to input mode. + */ + for (unsigned i = 0; i < _ngpio; i++) + stm32_configgpio(_gpio_tab[i].input); + + stm32_gpiowrite(GPIO_GPIO_DIR, 0); + stm32_configgpio(GPIO_GPIO_DIR); +} + +void +PX4FMU::gpio_set_function(uint32_t gpios, int function) +{ + /* + * GPIOs 0 and 1 must have the same direction as they are buffered + * by a shared 2-port driver. Any attempt to set either sets both. + */ + if (gpios & 3) { + gpios |= 3; + + /* flip the buffer to output mode if required */ + if (GPIO_SET_OUTPUT == function) + stm32_gpiowrite(GPIO_GPIO_DIR, 1); + } + + /* configure selected GPIOs as required */ + for (unsigned i = 0; i < _ngpio; i++) { + if (gpios & (1<ioctl(0, GPIO_RESET, 0); gpio_bits = 0; - servo_mode = FMUServo::MODE_NONE; + servo_mode = PX4FMU::MODE_NONE; switch (new_mode) { case PORT_FULL_GPIO: @@ -509,7 +734,7 @@ fmu_new_mode(PortMode new_mode, int update_rate) case PORT_FULL_PWM: /* select 4-pin PWM mode */ - servo_mode = FMUServo::MODE_4PWM; + servo_mode = PX4FMU::MODE_4PWM; break; case PORT_GPIO_AND_SERIAL: @@ -519,36 +744,47 @@ fmu_new_mode(PortMode new_mode, int update_rate) case PORT_PWM_AND_SERIAL: /* select 2-pin PWM mode */ - servo_mode = FMUServo::MODE_2PWM; + servo_mode = PX4FMU::MODE_2PWM; /* set RX/TX multi-GPIOs to serial mode */ gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; break; case PORT_PWM_AND_GPIO: /* select 2-pin PWM mode */ - servo_mode = FMUServo::MODE_2PWM; + servo_mode = PX4FMU::MODE_2PWM; break; } /* adjust GPIO config for serial mode(s) */ if (gpio_bits != 0) - ioctl(fd, GPIO_SET_ALT_1, gpio_bits); + g_fmu->ioctl(0, GPIO_SET_ALT_1, gpio_bits); - close(fd); + /* (re)set the PWM output mode */ + g_fmu->set_mode(servo_mode); + if ((servo_mode != PX4FMU::MODE_NONE) && (update_rate != 0)) + g_fmu->set_pwm_rate(update_rate); - /* create new PWM driver if required */ - if (servo_mode != FMUServo::MODE_NONE) { - g_servo = new FMUServo(servo_mode, update_rate); + return OK; +} - if (g_servo == nullptr) { +int +fmu_start(void) +{ + int ret = OK; + + if (g_fmu == nullptr) { + + g_fmu = new PX4FMU; + + if (g_fmu == nullptr) { ret = -ENOMEM; } else { - ret = g_servo->init(); + ret = g_fmu->init(); if (ret != OK) { - delete g_servo; - g_servo = nullptr; + delete g_fmu; + g_fmu = nullptr; } } } @@ -612,7 +848,7 @@ int fmu_main(int argc, char *argv[]) { PortMode new_mode = PORT_MODE_UNSET; - int pwm_update_rate_in_hz = 50; + int pwm_update_rate_in_hz = 0; if (!strcmp(argv[1], "test")) test(); @@ -620,6 +856,9 @@ fmu_main(int argc, char *argv[]) if (!strcmp(argv[1], "fake")) fake(argc - 1, argv + 1); + if (fmu_start() != OK) + errx(1, "failed to start the FMU driver"); + /* * Mode switches. * @@ -674,6 +913,6 @@ fmu_main(int argc, char *argv[]) /* test, etc. here */ fprintf(stderr, "FMU: unrecognised command, try:\n"); - fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-r pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); + fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); return -EINVAL; } diff --git a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp index 03cf3ee5d6..6799a0a21d 100644 --- a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -84,7 +84,7 @@ #include #ifndef CONFIG_HRT_TIMER -# error CONFIG_TONE_ALARM requires CONFIG_HRT_TIMER +# error This driver requires CONFIG_HRT_TIMER #endif /* Tone alarm configuration */ @@ -611,7 +611,6 @@ play_pattern(unsigned pattern) if (fd < 0) err(1, "/dev/tone_alarm"); - warnx("playing pattern %u", pattern); ret = ioctl(fd, TONE_SET_ALARM, pattern); if (ret != 0) diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index d8b9dc3db8..1eb621d09b 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -531,6 +531,7 @@ uorb_receive_thread(void *arg) struct pollfd fds[n_listeners]; for (unsigned i = 0; i < n_listeners; i++) { fds[i].fd = *listeners[i].subp; + fds[i].events = POLLIN; /* Invoke callback to set initial state */ //listeners[i].callback(&listener[i]); diff --git a/apps/systemcmds/reboot/reboot.c b/apps/systemcmds/reboot/reboot.c index e8886139d4..47d3cd9486 100644 --- a/apps/systemcmds/reboot/reboot.c +++ b/apps/systemcmds/reboot/reboot.c @@ -41,14 +41,13 @@ #include #include -#include "systemlib/systemlib.h" +#include __EXPORT int reboot_main(int argc, char *argv[]); int reboot_main(int argc, char *argv[]) { - reboot(); - return 0; + up_systemreset(); } diff --git a/apps/systemlib/Makefile b/apps/systemlib/Makefile index 4bc7033e72..5778c552e2 100644 --- a/apps/systemlib/Makefile +++ b/apps/systemlib/Makefile @@ -42,7 +42,8 @@ CSRCS = err.c \ bson/tinybson.c \ conversions.c \ cpuload.c \ - getopt_long.c + getopt_long.c \ + up_cxxinitialize.c # ppm_decode.c \ diff --git a/apps/systemlib/systemlib.c b/apps/systemlib/systemlib.c index 50ac62464a..750e783f55 100644 --- a/apps/systemlib/systemlib.c +++ b/apps/systemlib/systemlib.c @@ -73,41 +73,6 @@ const struct __multiport_info multiport_info = { static void kill_task(FAR _TCB *tcb, FAR void *arg); -/**************************************************************************** - * user_start - ****************************************************************************/ - -int reboot(void) -{ - sched_lock(); - // print text - printf("\r\nRebooting system - ending tasks and performing hard reset\r\n\r\n"); - fflush(stdout); - //usleep(5000); - - /* Sending kill signal to other tasks */ - //killall(); - - /* Waiting maximum time for all to exit */ - //usleep(5000); - //sched_lock(); - - /* Resetting CPU */ - // FIXME Need check for ARM architecture here -#ifndef NVIC_AIRCR -#define NVIC_AIRCR (*((uint32_t*)0xE000ED0C)) -#endif - - /* Set the SYSRESETREQ bit to force a reset */ - NVIC_AIRCR = 0x05fa0004; - - /* Spinning until the board is really reset */ - while (true); - - /* Should never reach here */ - return 0; -} - void killall() { // printf("Sending SIGUSR1 to all processes now\n"); diff --git a/apps/systemlib/systemlib.h b/apps/systemlib/systemlib.h index 997f40dedb..f31c5cd1f9 100644 --- a/apps/systemlib/systemlib.h +++ b/apps/systemlib/systemlib.h @@ -45,7 +45,7 @@ __BEGIN_DECLS /** Reboots the board */ -__EXPORT int reboot(void); +extern void up_systemreset(void) noreturn_function; /** Sends SIGUSR1 to all processes */ __EXPORT void killall(void); diff --git a/apps/systemlib/up_cxxinitialize.c b/apps/systemlib/up_cxxinitialize.c new file mode 100644 index 0000000000..c78f295708 --- /dev/null +++ b/apps/systemlib/up_cxxinitialize.c @@ -0,0 +1,150 @@ +/************************************************************************************ + * configs/stm32f4discovery/src/up_cxxinitialize.c + * arch/arm/src/board/up_cxxinitialize.c + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include + +#include + +//#include +//#include "chip.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ +/* Debug ****************************************************************************/ +/* Non-standard debug that may be enabled just for testing the static constructors */ + +#ifndef CONFIG_DEBUG +# undef CONFIG_DEBUG_CXX +#endif + +#ifdef CONFIG_DEBUG_CXX +# define cxxdbg dbg +# define cxxlldbg lldbg +# ifdef CONFIG_DEBUG_VERBOSE +# define cxxvdbg vdbg +# define cxxllvdbg llvdbg +# else +# define cxxvdbg(x...) +# define cxxllvdbg(x...) +# endif +#else +# define cxxdbg(x...) +# define cxxlldbg(x...) +# define cxxvdbg(x...) +# define cxxllvdbg(x...) +#endif + +/************************************************************************************ + * Private Types + ************************************************************************************/ +/* This type defines one entry in initialization array */ + +typedef void (*initializer_t)(void); + +/************************************************************************************ + * External references + ************************************************************************************/ +/* _sinit and _einit are symbols exported by the linker script that mark the + * beginning and the end of the C++ initialization section. + */ + +extern initializer_t _sinit; +extern initializer_t _einit; + +/* _stext and _etext are symbols exported by the linker script that mark the + * beginning and the end of text. + */ + +extern uint32_t _stext; +extern uint32_t _etext; + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/**************************************************************************** + * Name: up_cxxinitialize + * + * Description: + * If C++ and C++ static constructors are supported, then this function + * must be provided by board-specific logic in order to perform + * initialization of the static C++ class instances. + * + * This function should then be called in the application-specific + * user_start logic in order to perform the C++ initialization. NOTE + * that no component of the core NuttX RTOS logic is involved; This + * function defintion only provides the 'contract' between application + * specific C++ code and platform-specific toolchain support + * + ***************************************************************************/ + +__EXPORT void up_cxxinitialize(void) +{ + initializer_t *initp; + + cxxdbg("_sinit: %p _einit: %p _stext: %p _etext: %p\n", + &_sinit, &_einit, &_stext, &_etext); + + /* Visit each entry in the initialzation table */ + + for (initp = &_sinit; initp != &_einit; initp++) + { + initializer_t initializer = *initp; + cxxdbg("initp: %p initializer: %p\n", initp, initializer); + + /* Make sure that the address is non-NULL and lies in the text region + * defined by the linker script. Some toolchains may put NULL values + * or counts in the initialization table + */ + + if ((void*)initializer > (void*)&_stext && (void*)initializer < (void*)&_etext) + { + cxxdbg("Calling %p\n", initializer); + initializer(); + } + } +} diff --git a/nuttx/configs/px4fmu/common/ld.script b/nuttx/configs/px4fmu/common/ld.script index b1852b0bd9..de8179e8db 100644 --- a/nuttx/configs/px4fmu/common/ld.script +++ b/nuttx/configs/px4fmu/common/ld.script @@ -87,6 +87,15 @@ SECTIONS __errno = get_errno_ptr; } > flash + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + /* * Construction data for parameters. */ diff --git a/nuttx/configs/px4fmu/include/board.h b/nuttx/configs/px4fmu/include/board.h index a7dae6552a..3f0f26ba14 100755 --- a/nuttx/configs/px4fmu/include/board.h +++ b/nuttx/configs/px4fmu/include/board.h @@ -45,9 +45,6 @@ #ifndef __ASSEMBLY__ # include #endif -//#include "stm32_rcc.h" -//#include "stm32_sdio.h" -//#include "stm32_internal.h" /************************************************************************************ * Definitions @@ -298,7 +295,7 @@ * Note that these are unshifted addresses. */ #define PX4_I2C_OBDEV_HMC5883 0x1e -#define PX4_I2C_OBDEV_MS5611 NOTDEFINED +#define PX4_I2C_OBDEV_MS5611 0x76 #define PX4_I2C_OBDEV_EEPROM NOTDEFINED #define PX4_I2C_OBDEV_PX4IO_BL 0x18 @@ -327,11 +324,9 @@ /* * Tone alarm output */ -#ifdef CONFIG_TONE_ALARM -# define TONE_ALARM_TIMER 3 /* timer 3 */ -# define TONE_ALARM_CHANNEL 3 /* channel 3 */ -# define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8) -#endif +#define TONE_ALARM_TIMER 3 /* timer 3 */ +#define TONE_ALARM_CHANNEL 3 /* channel 3 */ +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8) /************************************************************************************ * Public Data @@ -362,37 +357,6 @@ extern "C" { EXTERN void stm32_boardinitialize(void); -/************************************************************************************ - * Button support. - * - * Description: - * up_buttoninit() must be called to initialize button resources. After - * that, up_buttons() may be called to collect the current state of all - * buttons or up_irqbutton() may be called to register button interrupt - * handlers. - * - * After up_buttoninit() has been called, up_buttons() may be called to - * collect the state of all buttons. up_buttons() returns an 8-bit bit set - * with each bit associated with a button. See the BUTTON_*_BIT - * definitions in board.h for the meaning of each bit. - * - * up_irqbutton() may be called to register an interrupt handler that will - * be called when a button is depressed or released. The ID value is a - * button enumeration value that uniquely identifies a button resource. See the - * BUTTON_* definitions in board.h for the meaning of enumeration - * value. The previous interrupt handler address is returned (so that it may - * restored, if so desired). - * - ************************************************************************************/ - -#ifdef CONFIG_ARCH_BUTTONS -EXTERN void up_buttoninit(void); -EXTERN uint8_t up_buttons(void); -#ifdef CONFIG_ARCH_IRQBUTTONS -EXTERN xcpt_t up_irqbutton(int id, xcpt_t irqhandler); -#endif -#endif - #undef EXTERN #if defined(__cplusplus) } diff --git a/nuttx/configs/px4fmu/include/drv_gpio.h b/nuttx/configs/px4fmu/include/drv_gpio.h deleted file mode 100644 index 22f80d0387..0000000000 --- a/nuttx/configs/px4fmu/include/drv_gpio.h +++ /dev/null @@ -1,107 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file GPIO driver interface. - * - * This header defines the basic interface to platform-specific GPIOs. - */ - -#ifndef _BOARD_DRV_GPIO_H -#define _BOARD_DRV_GPIO_H - -/* - * PX4FMU GPIO numbers. - * - * For shared pins, alternate function 1 selects the non-GPIO mode - * (USART2, CAN2, etc.) - */ -#define GPIO_EXT_1 (1<<0) /**< high-power GPIO 1 */ -#define GPIO_EXT_2 (1<<1) /**< high-power GPIO 1 */ -#define GPIO_MULTI_1 (1<<2) /**< USART2 CTS */ -#define GPIO_MULTI_2 (1<<3) /**< USART2 RTS */ -#define GPIO_MULTI_3 (1<<4) /**< USART2 TX */ -#define GPIO_MULTI_4 (1<<5) /**< USART2 RX */ -#define GPIO_CAN_TX (1<<6) /**< CAN2 TX */ -#define GPIO_CAN_RX (1<<7) /**< CAN2 RX */ - -/** - * Default GPIO device - other devices may also support this protocol if - * they also export GPIO-like things. This is always the GPIOs on the - * main board. - */ -#define GPIO_DEVICE_PATH "/dev/gpio" - -/* - * IOCTL definitions. - * - * For all ioctls, the (arg) argument is a bitmask of GPIOs to be affected - * by the operation, with the LSB being the lowest-numbered GPIO. - * - * Note that there may be board-specific relationships between GPIOs; - * applications using GPIOs should be aware of this. - */ -#define _GPIOCBASE 0x6700 -#define GPIOC(_x) _IOC(_GPIOCBASE, _x) - -/** reset all board GPIOs to their default state */ -#define GPIO_RESET GPIOC(0) - -/** configure the board GPIOs in (arg) as outputs */ -#define GPIO_SET_OUTPUT GPIOC(1) - -/** configure the board GPIOs in (arg) as inputs */ -#define GPIO_SET_INPUT GPIOC(2) - -/** configure the board GPIOs in (arg) for the first alternate function (if supported) */ -#define GPIO_SET_ALT_1 GPIOC(3) - -/** configure the board GPIO (arg) for the second alternate function (if supported) */ -#define GPIO_SET_ALT_2 GPIOC(4) - -/** configure the board GPIO (arg) for the third alternate function (if supported) */ -#define GPIO_SET_ALT_3 GPIOC(5) - -/** configure the board GPIO (arg) for the fourth alternate function (if supported) */ -#define GPIO_SET_ALT_4 GPIOC(6) - -/** set the GPIOs in (arg) */ -#define GPIO_SET GPIOC(10) - -/** clear the GPIOs in (arg) */ -#define GPIO_CLEAR GPIOC(11) - -/** read all the GPIOs and return their values in *(uint32_t *)arg */ -#define GPIO_GET GPIOC(12) - -#endif /* _DRV_GPIO_H */ diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 46df8a45df..67fe69a300 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -97,7 +97,7 @@ CONFIGURED_APPS += drivers/l3gd20 CONFIGURED_APPS += drivers/px4io CONFIGURED_APPS += drivers/stm32 CONFIGURED_APPS += drivers/stm32/tone_alarm -CONFIGURED_APPS += px4/fmu +CONFIGURED_APPS += drivers/px4fmu # Testing stuff CONFIGURED_APPS += px4/sensors_bringup diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index c2656217d4..91cd0366db 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -179,7 +179,7 @@ CONFIG_STM32_I2C1=y CONFIG_STM32_I2C2=y CONFIG_STM32_I2C3=y CONFIG_STM32_CAN1=n -CONFIG_STM32_CAN2=y +CONFIG_STM32_CAN2=n CONFIG_STM32_DAC=n CONFIG_STM32_PWR=y # APB2: @@ -542,7 +542,7 @@ CONFIG_DEBUG_I2C=n CONFIG_DEBUG_INPUT=n CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=n +CONFIG_HAVE_CXXINITIALIZE=y CONFIG_MM_REGIONS=2 CONFIG_ARCH_LOWPUTC=y CONFIG_MSEC_PER_TICK=1 diff --git a/nuttx/configs/px4fmu/src/Makefile b/nuttx/configs/px4fmu/src/Makefile index d880400137..13b26b84f5 100644 --- a/nuttx/configs/px4fmu/src/Makefile +++ b/nuttx/configs/px4fmu/src/Makefile @@ -41,7 +41,6 @@ ASRCS = AOBJS = $(ASRCS:.S=$(OBJEXT)) CSRCS = up_leds.c \ - drv_gpio.c \ drv_led.c drv_eeprom.c COBJS = $(CSRCS:.c=$(OBJEXT)) diff --git a/nuttx/configs/px4fmu/src/drv_gpio.c b/nuttx/configs/px4fmu/src/drv_gpio.c deleted file mode 100644 index be95420ddc..0000000000 --- a/nuttx/configs/px4fmu/src/drv_gpio.c +++ /dev/null @@ -1,195 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * GPIO driver for PX4FMU. - * - */ - -#include - -#include -#include -#include -#include - -#include -#include - -#include "up_arch.h" -#include "chip.h" -#include "stm32_internal.h" -#include "px4fmu-internal.h" - -#include - -static int px4fmu_gpio_ioctl(struct file *filep, int cmd, unsigned long arg); - -static const struct file_operations px4fmu_gpio_fops = { - .ioctl = px4fmu_gpio_ioctl, -}; - -static struct { - uint32_t input; - uint32_t output; - uint32_t alt; -} gpio_tab[] = { - {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, - {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, - {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, - {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1}, - {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1}, - {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1}, - {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, - {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, -}; - -#define NGPIO (sizeof(gpio_tab) / sizeof(gpio_tab[0])) - - -static void -px4fmu_gpio_reset(void) -{ - /* - * Setup default GPIO config - all pins as GPIOs, GPIO driver chip - * to input mode. - */ - for (unsigned i = 0; i < NGPIO; i++) - stm32_configgpio(gpio_tab[i].input); - - stm32_gpiowrite(GPIO_GPIO_DIR, 0); - stm32_configgpio(GPIO_GPIO_DIR); -} - -static void -px4fmu_gpio_set_function(uint32_t gpios, int function) -{ - /* - * GPIOs 0 and 1 must have the same direction as they are buffered - * by a shared 2-port driver. Any attempt to set either sets both. - */ - if (gpios & 3) { - gpios |= 3; - - /* flip the buffer to output mode if required */ - if (GPIO_SET_OUTPUT == function) - stm32_gpiowrite(GPIO_GPIO_DIR, 1); - } - - /* configure selected GPIOs as required */ - for (unsigned i = 0; i < NGPIO; i++) { - if (gpios & (1<