3 changed files with 0 additions and 312 deletions
@ -1,5 +0,0 @@
@@ -1,5 +0,0 @@
|
||||
#
|
||||
# Board-specific startup code for the PX4IOv2
|
||||
#
|
||||
|
||||
SRCS = px4iov2_init.c
|
@ -1,172 +0,0 @@
@@ -1,172 +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 px4iov2_init.c |
||||
* |
||||
* PX4FMU-specific early startup code. This file implements the |
||||
* nsh_archinitialize() function that is called early by nsh during startup. |
||||
* |
||||
* Code here is run before the rcS script is invoked; it should start required |
||||
* subsystems and perform board-specific initialisation. |
||||
*/ |
||||
|
||||
/****************************************************************************
|
||||
* Included Files |
||||
****************************************************************************/ |
||||
|
||||
#include <nuttx/config.h> |
||||
|
||||
#include <stdbool.h> |
||||
#include <stdio.h> |
||||
#include <debug.h> |
||||
#include <errno.h> |
||||
|
||||
#include <nuttx/arch.h> |
||||
|
||||
#include "stm32_internal.h" |
||||
#include "px4iov2_internal.h" |
||||
|
||||
#include <arch/board/board.h> |
||||
|
||||
/****************************************************************************
|
||||
* Pre-Processor Definitions |
||||
****************************************************************************/ |
||||
|
||||
/* Configuration ************************************************************/ |
||||
|
||||
/* Debug ********************************************************************/ |
||||
|
||||
#ifdef CONFIG_CPP_HAVE_VARARGS |
||||
# ifdef CONFIG_DEBUG |
||||
# define message(...) lowsyslog(__VA_ARGS__) |
||||
# else |
||||
# define message(...) printf(__VA_ARGS__) |
||||
# endif |
||||
#else |
||||
# ifdef CONFIG_DEBUG |
||||
# define message lowsyslog |
||||
# else |
||||
# define message printf |
||||
# endif |
||||
#endif |
||||
|
||||
/****************************************************************************
|
||||
* Protected Functions |
||||
****************************************************************************/ |
||||
|
||||
/****************************************************************************
|
||||
* Public Functions |
||||
****************************************************************************/ |
||||
|
||||
/************************************************************************************
|
||||
* Name: stm32_boardinitialize |
||||
* |
||||
* Description: |
||||
* All STM32 architectures must provide the following entry point. This entry point |
||||
* is called early in the intitialization -- after all memory has been configured |
||||
* and mapped but before any devices have been initialized. |
||||
* |
||||
************************************************************************************/ |
||||
|
||||
__EXPORT void stm32_boardinitialize(void) |
||||
{ |
||||
|
||||
/* configure GPIOs */ |
||||
|
||||
/* turn off - all leds are active low */ |
||||
stm32_gpiowrite(BOARD_GPIO_LED1, true); |
||||
stm32_gpiowrite(BOARD_GPIO_LED2, true); |
||||
stm32_gpiowrite(BOARD_GPIO_LED3, true); |
||||
stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_LED1)); |
||||
stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_LED2)); |
||||
stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_LED3)); |
||||
|
||||
|
||||
stm32_configgpio(BOARD_GPIO_INPUT_FLOAT(BOARD_GPIO_BTN_SAFETY)); |
||||
|
||||
/* spektrum power enable is active high - disable it by default */ |
||||
stm32_gpiowrite(BOARD_GPIO_SPEKTRUM_PWR_EN, false); |
||||
stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_SPEKTRUM_PWR_EN)); |
||||
|
||||
/* servo power enable is active low, and has a pull down resistor
|
||||
* to keep it low during boot (since it may power the whole board.) |
||||
*/ |
||||
stm32_gpiowrite(BOARD_GPIO_SERVO_PWR_EN, false); |
||||
stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_SERVO_PWR_EN)); |
||||
|
||||
stm32_configgpio(BOARD_GPIO_INPUT_PUP(BOARD_GPIO_SERVO_FAULT_DETECT)); |
||||
|
||||
stm32_configgpio(BOARD_GPIO_INPUT_FLOAT(BOARD_GPIO_TIM_RSSI)); /* xxx alternate function */ |
||||
stm32_configgpio(BOARD_GPIO_INPUT_ANALOG(BOARD_GPIO_ADC_RSSI)); |
||||
stm32_configgpio(BOARD_GPIO_INPUT_ANALOG(BOARD_GPIO_ADC_VSERVO)); |
||||
|
||||
stm32_configgpio(BOARD_GPIO_INPUT_FLOAT(BOARD_GPIO_SBUS_INPUT)); /* xxx alternate function */ |
||||
|
||||
stm32_gpiowrite(BOARD_GPIO_SBUS_OUTPUT, false); |
||||
stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_SBUS_OUTPUT)); |
||||
/* sbus output enable is active low - disable it by default */ |
||||
stm32_gpiowrite(BOARD_GPIO_SBUS_OENABLE, true); |
||||
stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_SBUS_OENABLE)); |
||||
|
||||
|
||||
stm32_configgpio(BOARD_GPIO_INPUT_FLOAT(BOARD_GPIO_PPM)); /* xxx alternate function */ |
||||
|
||||
stm32_gpiowrite(BOARD_GPIO_PWM1, false); |
||||
stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM1)); |
||||
|
||||
stm32_gpiowrite(BOARD_GPIO_PWM2, false); |
||||
stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM2)); |
||||
|
||||
stm32_gpiowrite(BOARD_GPIO_PWM3, false); |
||||
stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM3)); |
||||
|
||||
stm32_gpiowrite(BOARD_GPIO_PWM4, false); |
||||
stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM4)); |
||||
|
||||
stm32_gpiowrite(BOARD_GPIO_PWM5, false); |
||||
stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM5)); |
||||
|
||||
stm32_gpiowrite(BOARD_GPIO_PWM6, false); |
||||
stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM6)); |
||||
|
||||
stm32_gpiowrite(BOARD_GPIO_PWM7, false); |
||||
stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM7)); |
||||
|
||||
stm32_gpiowrite(BOARD_GPIO_PWM8, false); |
||||
stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM8)); |
||||
|
||||
// message("[boot] Successfully initialized px4iov2 gpios\n");
|
||||
|
||||
return OK; |
||||
} |
@ -1,135 +0,0 @@
@@ -1,135 +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 px4iov2_internal.h |
||||
* |
||||
* PX4IOV2 internal definitions |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
/****************************************************************************************************
|
||||
* Included Files |
||||
****************************************************************************************************/ |
||||
|
||||
#include <nuttx/config.h> |
||||
#include <nuttx/compiler.h> |
||||
#include <stdint.h> |
||||
|
||||
__BEGIN_DECLS |
||||
|
||||
/* these headers are not C++ safe */ |
||||
#include <stm32_internal.h> |
||||
|
||||
|
||||
/****************************************************************************************************
|
||||
* Definitions |
||||
****************************************************************************************************/ |
||||
/* Configuration ************************************************************************************/ |
||||
|
||||
/******************************************************************************
|
||||
* GPIOS |
||||
******************************************************************************/ |
||||
|
||||
#define BOARD_GPIO_OUTPUT(pin) (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ |
||||
GPIO_OUTPUT_CLEAR|(pin)) |
||||
#define BOARD_GPIO_INPUT_FLOAT(pin) (GPIO_INPUT|GPIO_CNF_INFLOAT|\ |
||||
GPIO_MODE_INPUT|(pin)) |
||||
#define BOARD_GPIO_INPUT_PUP(pin) (GPIO_INPUT|GPIO_CNF_INPULLUP|\ |
||||
GPIO_MODE_INPUT|(pin)) |
||||
#define BOARD_GPIO_INPUT_ANALOG(pin) (GPIO_INPUT|GPIO_CNF_ANALOGIN|\ |
||||
GPIO_MODE_INPUT|(pin)) |
||||
|
||||
/* LEDS **********************************************************************/ |
||||
|
||||
#define BOARD_GPIO_LED1 (GPIO_PORTB|GPIO_PIN14) |
||||
#define BOARD_GPIO_LED2 (GPIO_PORTB|GPIO_PIN15) |
||||
#define BOARD_GPIO_LED3 (GPIO_PORTB|GPIO_PIN13) |
||||
|
||||
#define BOARD_GPIO_LED_BLUE BOARD_GPIO_LED1 |
||||
#define BOARD_GPIO_LED_AMBER BOARD_GPIO_LED2 |
||||
#define BOARD_GPIO_LED_SAFETY BOARD_GPIO_LED3 |
||||
|
||||
/* Safety switch button *******************************************************/ |
||||
|
||||
#define BOARD_GPIO_BTN_SAFETY (GPIO_PORTB|GPIO_PIN5) |
||||
|
||||
/* Power switch controls ******************************************************/ |
||||
|
||||
#define BOARD_GPIO_SPEKTRUM_PWR_EN (GPIO_PORTC|GPIO_PIN13) |
||||
|
||||
#define BOARD_GPIO_SERVO_PWR_EN (GPIO_PORTC|GPIO_PIN15) |
||||
|
||||
#define BOARD_GPIO_SERVO_FAULT_DETECT (GPIO_PORTB|GPIO_PIN13) |
||||
|
||||
|
||||
/* Analog inputs **************************************************************/ |
||||
|
||||
#define BOARD_GPIO_ADC_VSERVO (GPIO_PORTA|GPIO_PIN4) |
||||
/* the same rssi signal goes to both an adc and a timer input */ |
||||
#define BOARD_GPIO_ADC_RSSI (GPIO_PORTA|GPIO_PIN5) |
||||
#define BOARD_GPIO_TIM_RSSI (GPIO_PORTA|GPIO_PIN12) |
||||
|
||||
/* PWM pins **************************************************************/ |
||||
|
||||
#define BOARD_GPIO_PPM (GPIO_PORTA|GPIO_PIN8) |
||||
|
||||
#define BOARD_GPIO_PWM1 (GPIO_PORTA|GPIO_PIN0) |
||||
#define BOARD_GPIO_PWM2 (GPIO_PORTA|GPIO_PIN1) |
||||
#define BOARD_GPIO_PWM3 (GPIO_PORTB|GPIO_PIN8) |
||||
#define BOARD_GPIO_PWM4 (GPIO_PORTB|GPIO_PIN9) |
||||
#define BOARD_GPIO_PWM5 (GPIO_PORTA|GPIO_PIN6) |
||||
#define BOARD_GPIO_PWM6 (GPIO_PORTA|GPIO_PIN7) |
||||
#define BOARD_GPIO_PWM7 (GPIO_PORTB|GPIO_PIN0) |
||||
#define BOARD_GPIO_PWM8 (GPIO_PORTB|GPIO_PIN1) |
||||
|
||||
/* SBUS pins *************************************************************/ |
||||
|
||||
#define BOARD_GPIO_SBUS_INPUT (GPIO_PORTB|GPIO_PIN11) |
||||
#define BOARD_GPIO_SBUS_OUTPUT (GPIO_PORTB|GPIO_PIN10) |
||||
#define BOARD_GPIO_SBUS_OENABLE (GPIO_PORTB|GPIO_PIN4) |
||||
|
||||
/****************************************************************************************************
|
||||
* Public Types |
||||
****************************************************************************************************/ |
||||
|
||||
/****************************************************************************************************
|
||||
* Public data |
||||
****************************************************************************************************/ |
||||
|
||||
#ifndef __ASSEMBLY__ |
||||
|
||||
#endif /* __ASSEMBLY__ */ |
||||
|
||||
__END_DECLS |
Loading…
Reference in new issue