diff options
author | px4dev <px4@purgatory.org> | 2012-10-23 21:43:41 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-10-23 23:51:13 -0700 |
commit | 3d79b9a0b057ed9bf41329ce052d0b4152cd0a1a (patch) | |
tree | a477c4ef7c88e7a6a2402207070af3160080fcf6 | |
parent | c3fe915b44a1b32f05b182b3079c722b82b20fb9 (diff) | |
download | px4-firmware-3d79b9a0b057ed9bf41329ce052d0b4152cd0a1a.tar.gz px4-firmware-3d79b9a0b057ed9bf41329ce052d0b4152cd0a1a.tar.bz2 px4-firmware-3d79b9a0b057ed9bf41329ce052d0b4152cd0a1a.zip |
Tease the PWM driver out and fix some build issues after cleaning up behind the cpuload pieces.
-rw-r--r-- | apps/commander/commander.c | 2 | ||||
-rw-r--r-- | apps/drivers/boards/px4fmu/px4fmu_init.c | 3 | ||||
-rw-r--r-- | apps/drivers/drv_pwm_output.h | 66 | ||||
-rw-r--r-- | apps/drivers/stm32/Makefile | 42 | ||||
-rw-r--r-- | apps/drivers/stm32/drv_hrt.c (renamed from apps/drivers/boards/px4fmu/drv_hrt.c) | 0 | ||||
-rw-r--r-- | apps/drivers/stm32/up_pwm_servo.c (renamed from nuttx/configs/px4fmu/src/up_pwm_servo.c) | 4 | ||||
-rw-r--r-- | apps/systemcmds/top/top.c | 2 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/include/up_cpuload.h | 62 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/nsh/appconfig | 1 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/Makefile | 2 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/up_cpuload.c | 183 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/up_nsh.c | 280 |
12 files changed, 114 insertions, 533 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index b53b675df..a7550b54b 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -95,7 +95,7 @@ PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ //PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ -#include <arch/board/up_cpuload.h> +#include <systemlib/cpuload.h> extern struct system_load_s system_load; /* Decouple update interval and hysteris counters, all depends on intervals */ diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c index c35290589..bb30087e0 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_init.c +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -63,7 +63,6 @@ #include "px4fmu_internal.h" #include "stm32_uart.h" -#include <arch/board/up_cpuload.h> #include <arch/board/up_adc.h> #include <arch/board/board.h> #include <arch/board/drv_led.h> @@ -71,6 +70,8 @@ #include <drivers/drv_hrt.h> +#include <systemlib/cpuload.h> + /**************************************************************************** * Pre-Processor Definitions ****************************************************************************/ diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index 340175a4b..fc458e9b0 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -41,14 +41,15 @@ * channel. */ -#ifndef _DRV_PWM_OUTPUT_H -#define _DRV_PWM_OUTPUT_H +#pragma once #include <stdint.h> #include <sys/ioctl.h> #include "drv_orb_dev.h" +__BEGIN_DECLS + /** * Path for the default PWM output device. * @@ -109,4 +110,63 @@ ORB_DECLARE(output_pwm); #define PWM_SERVO_GET(_servo) _IOC(_PWM_SERVO_BASE, 0x40 + _servo) -#endif /* _DRV_PWM_OUTPUT_H */ +/* + * Low-level PWM output interface. + * + * This is the low-level API to the platform-specific PWM driver. + */ + +/** + * Intialise the PWM servo outputs using the specified configuration. + * + * @param channel_mask Bitmask of channels (LSB = channel 0) to enable. + * This allows some of the channels to remain configured + * as GPIOs or as another function. + * @return OK on success. + */ +__EXPORT extern int up_pwm_servo_init(uint32_t channel_mask); + +/** + * De-initialise the PWM servo outputs. + */ +__EXPORT extern void up_pwm_servo_deinit(void); + +/** + * Arm or disarm servo outputs. + * + * When disarmed, servos output no pulse. + * + * @bug This function should, but does not, guarantee that any pulse + * currently in progress is cleanly completed. + * + * @param armed If true, outputs are armed; if false they + * are disarmed. + */ +__EXPORT extern void up_pwm_servo_arm(bool armed); + +/** + * Set the servo update rate + * + * @param rate The update rate in Hz to set. + * @return OK on success, -ERANGE if an unsupported update rate is set. + */ +__EXPORT extern int up_pwm_servo_set_rate(unsigned rate); + +/** + * Set the current output value for a channel. + * + * @param channel The channel to set. + * @param value The output pulse width in microseconds. + */ +__EXPORT extern int up_pwm_servo_set(unsigned channel, servo_position_t value); + +/** + * Get the current output value for a channel. + * + * @param channel The channel to read. + * @return The output pulse width in microseconds, or zero if + * outputs are not armed or not configured. + */ +__EXPORT extern servo_position_t up_pwm_servo_get(unsigned channel); + +__END_DECLS diff --git a/apps/drivers/stm32/Makefile b/apps/drivers/stm32/Makefile new file mode 100644 index 000000000..4ad57f413 --- /dev/null +++ b/apps/drivers/stm32/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# STM32 driver support code +# +# Modules in this directory are compiled for all STM32 targets. +# + +INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/boards/px4fmu/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c index 11b98fd1b..11b98fd1b 100644 --- a/apps/drivers/boards/px4fmu/drv_hrt.c +++ b/apps/drivers/stm32/drv_hrt.c diff --git a/nuttx/configs/px4fmu/src/up_pwm_servo.c b/apps/drivers/stm32/up_pwm_servo.c index d278da55c..708aa8884 100644 --- a/nuttx/configs/px4fmu/src/up_pwm_servo.c +++ b/apps/drivers/stm32/up_pwm_servo.c @@ -32,6 +32,8 @@ ****************************************************************************/ /* + * @file drv_pwm_servo.c + * * Servo driver supporting PWM servos connected to STM32 timer blocks. * * Works with any of the 'generic' or 'advanced' STM32 timers that @@ -54,7 +56,7 @@ #include <stdio.h> #include <arch/board/board.h> -#include <arch/board/up_pwm_servo.h> +#include <drivers/drv_pwm_output.h> #include "chip.h" #include "up_internal.h" diff --git a/apps/systemcmds/top/top.c b/apps/systemcmds/top/top.c index b373d9ae7..7a2a993c2 100644 --- a/apps/systemcmds/top/top.c +++ b/apps/systemcmds/top/top.c @@ -46,7 +46,7 @@ #include <string.h> #include <poll.h> -#include <arch/board/up_cpuload.h> +#include <systemlib/cpuload.h> #include <drivers/drv_hrt.h> /** diff --git a/nuttx/configs/px4fmu/include/up_cpuload.h b/nuttx/configs/px4fmu/include/up_cpuload.h deleted file mode 100644 index b61c5c550..000000000 --- a/nuttx/configs/px4fmu/include/up_cpuload.h +++ /dev/null @@ -1,62 +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. - * - ****************************************************************************/ - -#ifndef UP_CPULOAD_H_ -#define UP_CPULOAD_H_ - -#ifdef CONFIG_SCHED_INSTRUMENTATION - -#include <nuttx/sched.h> - -struct system_load_taskinfo_s { - uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load - uint64_t curr_start_time; ///< Start time of the current scheduling slot - uint64_t start_time; ///< FIRST start time of task - FAR struct _TCB *tcb; ///< - bool valid; ///< Task is currently active / valid -}; - -struct system_load_s { - uint64_t start_time; ///< Global start time of measurements - struct system_load_taskinfo_s tasks[CONFIG_MAX_TASKS]; - uint8_t initialized; - int total_count; - int running_count; - int sleeping_count; -}; - -void cpuload_initialize_once(void); - -#endif - -#endif diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index b587b3386..cfd86aac1 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -95,6 +95,7 @@ CONFIGURED_APPS += drivers/mpu6000 CONFIGURED_APPS += drivers/bma180 CONFIGURED_APPS += drivers/l3gd20 CONFIGURED_APPS += drivers/px4io +CONFIGURED_APPS += drivers/stm32 CONFIGURED_APPS += drivers/stm32/tone_alarm CONFIGURED_APPS += px4/fmu diff --git a/nuttx/configs/px4fmu/src/Makefile b/nuttx/configs/px4fmu/src/Makefile index 0768a9d60..f6140fb93 100644 --- a/nuttx/configs/px4fmu/src/Makefile +++ b/nuttx/configs/px4fmu/src/Makefile @@ -43,7 +43,7 @@ AOBJS = $(ASRCS:.S=$(OBJEXT)) CSRCS = up_boot.c up_leds.c up_spi.c \ drv_gpio.c \ drv_led.c drv_eeprom.c \ - up_pwm_servo.c up_usbdev.c + up_usbdev.c ifeq ($(CONFIG_ADC),y) CSRCS += up_adc.c diff --git a/nuttx/configs/px4fmu/src/up_cpuload.c b/nuttx/configs/px4fmu/src/up_cpuload.c deleted file mode 100644 index bf867ae8b..000000000 --- a/nuttx/configs/px4fmu/src/up_cpuload.c +++ /dev/null @@ -1,183 +0,0 @@ -/**************************************************************************** - * configs/px4fmu/src/up_leds.c - * arch/arm/src/board/up_leds.c - * - * Copyright (C) 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt <gnutt@nuttx.org> - * - * 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 <nuttx/config.h> - -#include <stdint.h> -#include <stdbool.h> -#include <debug.h> - -#include <sys/time.h> -#include <sched.h> - -#include <arch/board/board.h> -#include <arch/board/up_hrt.h> -#include <arch/board/up_cpuload.h> - -#include "chip.h" -#include "up_arch.h" -#include "up_internal.h" -#include "stm32_internal.h" -#include "px4fmu-internal.h" - -/**************************************************************************** - * Definitions - ****************************************************************************/ - - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: - ****************************************************************************/ - -#ifdef CONFIG_SCHED_INSTRUMENTATION - -struct system_load_s system_load; - -extern FAR _TCB *sched_gettcb(pid_t pid); - -void cpuload_initialize_once(void); - -void cpuload_initialize_once() -{ -// if (!system_load.initialized) -// { - system_load.start_time = hrt_absolute_time(); - int i; - for (i = 0; i < CONFIG_MAX_TASKS; i++) - { - system_load.tasks[i].valid = false; - } - system_load.total_count = 0; - - uint64_t now = hrt_absolute_time(); - - /* initialize idle thread statically */ - system_load.tasks[0].start_time = now; - system_load.tasks[0].total_runtime = 0; - system_load.tasks[0].curr_start_time = 0; - system_load.tasks[0].tcb = sched_gettcb(0); - system_load.tasks[0].valid = true; - system_load.total_count++; - - /* initialize init thread statically */ - system_load.tasks[1].start_time = now; - system_load.tasks[1].total_runtime = 0; - system_load.tasks[1].curr_start_time = 0; - system_load.tasks[1].tcb = sched_gettcb(1); - system_load.tasks[1].valid = true; - /* count init thread */ - system_load.total_count++; - // } -} - -void sched_note_start(FAR _TCB *tcb ) -{ - /* search first free slot */ - int i; - for (i = 1; i < CONFIG_MAX_TASKS; i++) - { - if (!system_load.tasks[i].valid) - { - /* slot is available */ - system_load.tasks[i].start_time = hrt_absolute_time(); - system_load.tasks[i].total_runtime = 0; - system_load.tasks[i].curr_start_time = 0; - system_load.tasks[i].tcb = tcb; - system_load.tasks[i].valid = true; - system_load.total_count++; - break; - } - } -} - -void sched_note_stop(FAR _TCB *tcb ) -{ - int i; - for (i = 1; i < CONFIG_MAX_TASKS; i++) - { - if (system_load.tasks[i].tcb->pid == tcb->pid) - { - /* mark slot as fee */ - system_load.tasks[i].valid = false; - system_load.tasks[i].total_runtime = 0; - system_load.tasks[i].curr_start_time = 0; - system_load.tasks[i].tcb = NULL; - system_load.total_count--; - break; - } - } -} - -void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb) -{ - uint64_t new_time = hrt_absolute_time(); - - /* Kind of inefficient: find both tasks and update times */ - uint8_t both_found = 0; - for (int i = 0; i < CONFIG_MAX_TASKS; i++) - { - /* Task ending its current scheduling run */ - if (system_load.tasks[i].tcb->pid == pFromTcb->pid) - { - //if (system_load.tasks[i].curr_start_time != 0) - { - system_load.tasks[i].total_runtime += new_time - system_load.tasks[i].curr_start_time; - } - both_found++; - } - else if (system_load.tasks[i].tcb->pid == pToTcb->pid) - { - system_load.tasks[i].curr_start_time = new_time; - both_found++; - } - - /* Do only iterate as long as needed */ - if (both_found == 2) - { - break; - } - } -} - -#endif /* CONFIG_SCHED_INSTRUMENTATION */ diff --git a/nuttx/configs/px4fmu/src/up_nsh.c b/nuttx/configs/px4fmu/src/up_nsh.c deleted file mode 100644 index 763207d3a..000000000 --- a/nuttx/configs/px4fmu/src/up_nsh.c +++ /dev/null @@ -1,280 +0,0 @@ -/**************************************************************************** - * config/px4fmu/src/up_nsh.c - * arch/arm/src/board/up_nsh.c - * - * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt <spudmonkey@racsa.co.cr> - * - * 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 <nuttx/config.h> - -#include <stdbool.h> -#include <stdio.h> -#include <debug.h> -#include <errno.h> - -#include <nuttx/arch.h> -#include <nuttx/spi.h> -#include <nuttx/i2c.h> -#include <nuttx/mmcsd.h> -#include <nuttx/analog/adc.h> -#include <nuttx/arch.h> - -#include "stm32_internal.h" -#include "px4fmu-internal.h" -#include "stm32_uart.h" - -#include <arch/board/up_hrt.h> -#include <arch/board/up_cpuload.h> -#include <arch/board/up_adc.h> -#include <arch/board/board.h> -#include <arch/board/drv_led.h> -#include <arch/board/drv_eeprom.h> - -/**************************************************************************** - * Pre-Processor Definitions - ****************************************************************************/ - -/* Configuration ************************************************************/ - -/* Debug ********************************************************************/ - -#ifdef CONFIG_CPP_HAVE_VARARGS -# ifdef CONFIG_DEBUG -# define message(...) lib_lowprintf(__VA_ARGS__) -# else -# define message(...) printf(__VA_ARGS__) -# endif -#else -# ifdef CONFIG_DEBUG -# define message lib_lowprintf -# else -# define message printf -# endif -#endif - -/**************************************************************************** - * Protected Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: nsh_archinitialize - * - * Description: - * Perform architecture specific initialization - * - ****************************************************************************/ - -static struct spi_dev_s *spi1; -static struct spi_dev_s *spi3; -static struct i2c_dev_s *i2c1; -static struct i2c_dev_s *i2c2; -static struct i2c_dev_s *i2c3; - -#include <math.h> - -#ifdef __cplusplus -int matherr(struct __exception *e) { - return 1; -} -#else -int matherr(struct exception *e) { - return 1; -} -#endif - -int nsh_archinitialize(void) -{ - int result; - - /* INIT 1 Lowest level NuttX initialization has been done at this point, LEDs and UARTs are configured */ - - /* INIT 2 Configuring PX4 low-level peripherals, these will be always needed */ - - /* configure the high-resolution time/callout interface */ -#ifdef CONFIG_HRT_TIMER - hrt_init(); -#endif - - /* configure CPU load estimation */ - #ifdef CONFIG_SCHED_INSTRUMENTATION - cpuload_initialize_once(); - #endif - - /* set up the serial DMA polling */ -#ifdef SERIAL_HAVE_DMA - { - static struct hrt_call serial_dma_call; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); - } -#endif - - message("\r\n"); - - up_ledoff(LED_BLUE); - up_ledoff(LED_AMBER); - - up_ledon(LED_BLUE); - - /* Configure user-space led driver */ - px4fmu_led_init(); - - /* Configure SPI-based devices */ - - spi1 = up_spiinitialize(1); - if (!spi1) - { - message("[boot] FAILED to initialize SPI port 1\r\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - // Default SPI1 to 1MHz and de-assert the known chip selects. - SPI_SETFREQUENCY(spi1, 10000000); - SPI_SETBITS(spi1, 8); - SPI_SETMODE(spi1, SPIDEV_MODE3); - SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); - SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false); - SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); - up_udelay(20); - - message("[boot] Successfully initialized SPI port 1\r\n"); - - /* initialize I2C2 bus */ - - i2c2 = up_i2cinitialize(2); - if (!i2c2) { - message("[boot] FAILED to initialize I2C bus 2\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - /* set I2C2 speed */ - I2C_SETFREQUENCY(i2c2, 400000); - - - i2c3 = up_i2cinitialize(3); - if (!i2c3) { - message("[boot] FAILED to initialize I2C bus 3\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - /* set I2C3 speed */ - I2C_SETFREQUENCY(i2c3, 400000); - - /* try to attach, don't fail if device is not responding */ - (void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS, - FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES, - FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES, - FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1); - -#if defined(CONFIG_STM32_SPI3) - /* Get the SPI port */ - - message("[boot] Initializing SPI port 3\n"); - spi3 = up_spiinitialize(3); - if (!spi3) - { - message("[boot] FAILED to initialize SPI port 3\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - message("[boot] Successfully initialized SPI port 3\n"); - - /* Now bind the SPI interface to the MMCSD driver */ - result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3); - if (result != OK) - { - message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); -#endif /* SPI3 */ - - /* initialize I2C1 bus */ - - i2c1 = up_i2cinitialize(1); - if (!i2c1) { - message("[boot] FAILED to initialize I2C bus 1\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - /* set I2C1 speed */ - I2C_SETFREQUENCY(i2c1, 400000); - - /* INIT 3: MULTIPORT-DEPENDENT INITIALIZATION */ - - /* Get board information if available */ - - /* Initialize the user GPIOs */ - px4fmu_gpio_init(); - -#ifdef CONFIG_ADC - int adc_state = adc_devinit(); - if (adc_state != OK) - { - /* Try again */ - adc_state = adc_devinit(); - if (adc_state != OK) - { - /* Give up */ - message("[boot] FAILED adc_devinit: %d\n", adc_state); - return -ENODEV; - } - } -#endif - - return OK; -} |