forked from Archive/PX4-Autopilot
Merge remote-tracking branch 'origin/master' into fw_control
This commit is contained in:
commit
8d764170aa
|
@ -138,7 +138,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
|
||||||
current_status->flag_system_armed = false;
|
current_status->flag_system_armed = false;
|
||||||
mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM");
|
mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM");
|
||||||
usleep(500000);
|
usleep(500000);
|
||||||
reboot();
|
up_systemreset();
|
||||||
/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
|
/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
|
||||||
} else {
|
} else {
|
||||||
invalid_state = true;
|
invalid_state = true;
|
||||||
|
|
|
@ -294,9 +294,6 @@ __EXPORT int nsh_archinitialize(void)
|
||||||
|
|
||||||
/* Get board information if available */
|
/* Get board information if available */
|
||||||
|
|
||||||
/* Initialize the user GPIOs */
|
|
||||||
px4fmu_gpio_init();
|
|
||||||
|
|
||||||
#ifdef CONFIG_ADC
|
#ifdef CONFIG_ADC
|
||||||
int adc_state = adc_devinit();
|
int adc_state = adc_devinit();
|
||||||
|
|
||||||
|
|
|
@ -47,6 +47,12 @@
|
||||||
#include <nuttx/compiler.h>
|
#include <nuttx/compiler.h>
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
|
||||||
|
__BEGIN_DECLS
|
||||||
|
|
||||||
|
/* these headers are not C++ safe */
|
||||||
|
#include <stm32_internal.h>
|
||||||
|
|
||||||
|
|
||||||
/****************************************************************************************************
|
/****************************************************************************************************
|
||||||
* Definitions
|
* Definitions
|
||||||
****************************************************************************************************/
|
****************************************************************************************************/
|
||||||
|
@ -150,17 +156,6 @@
|
||||||
|
|
||||||
extern void stm32_spiinitialize(void);
|
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__ */
|
#endif /* __ASSEMBLY__ */
|
||||||
|
|
||||||
|
__END_DECLS
|
||||||
|
|
|
@ -40,25 +40,75 @@
|
||||||
|
|
||||||
#include <sys/ioctl.h>
|
#include <sys/ioctl.h>
|
||||||
|
|
||||||
|
#ifdef CONFIG_ARCH_BOARD_PX4FMU
|
||||||
/*
|
/*
|
||||||
* GPIO defines come from a board-specific header, as they are shared
|
* PX4FMU GPIO numbers.
|
||||||
* with board-specific logic.
|
|
||||||
*
|
*
|
||||||
* The board-specific header must define:
|
* For shared pins, alternate function 1 selects the non-GPIO mode
|
||||||
* GPIO_DEVICE_PATH
|
* (USART2, CAN2, etc.)
|
||||||
* 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
|
|
||||||
*/
|
*/
|
||||||
|
# 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 <arch/board/drv_gpio.h>
|
* 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 */
|
#endif /* _DRV_GPIO_H */
|
|
@ -58,17 +58,21 @@
|
||||||
#include <nuttx/wqueue.h>
|
#include <nuttx/wqueue.h>
|
||||||
#include <nuttx/clock.h>
|
#include <nuttx/clock.h>
|
||||||
|
|
||||||
#include <drivers/drv_hrt.h>
|
#include <arch/board/board.h>
|
||||||
|
|
||||||
#include <systemlib/perf_counter.h>
|
#include <systemlib/perf_counter.h>
|
||||||
#include <systemlib/err.h>
|
#include <systemlib/err.h>
|
||||||
|
|
||||||
#include <drivers/drv_mag.h>
|
#include <drivers/drv_mag.h>
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* HMC5883 internal constants and data structures.
|
* 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 */
|
/* Max measurement rate is 160Hz */
|
||||||
#define HMC5883_CONVERSION_INTERVAL (1000000 / 160) /* microseconds */
|
#define HMC5883_CONVERSION_INTERVAL (1000000 / 160) /* microseconds */
|
||||||
|
|
||||||
|
@ -86,8 +90,6 @@
|
||||||
#define ADDR_ID_B 0x0b
|
#define ADDR_ID_B 0x0b
|
||||||
#define ADDR_ID_C 0x0c
|
#define ADDR_ID_C 0x0c
|
||||||
|
|
||||||
#define HMC5883L_ADDRESS 0x1E
|
|
||||||
|
|
||||||
/* modes not changeable outside of driver */
|
/* modes not changeable outside of driver */
|
||||||
#define HMC5883L_MODE_NORMAL (0 << 0) /* default */
|
#define HMC5883L_MODE_NORMAL (0 << 0) /* default */
|
||||||
#define HMC5883L_MODE_POSITIVE_BIAS (1 << 0) /* positive bias */
|
#define HMC5883L_MODE_POSITIVE_BIAS (1 << 0) /* positive bias */
|
||||||
|
@ -1100,8 +1102,7 @@ start()
|
||||||
errx(1, "already started");
|
errx(1, "already started");
|
||||||
|
|
||||||
/* create the driver */
|
/* create the driver */
|
||||||
/* XXX HORRIBLE hack - the bus number should not come from here */
|
g_dev = new HMC5883(HMC5883L_BUS);
|
||||||
g_dev = new HMC5883(2);
|
|
||||||
|
|
||||||
if (g_dev == nullptr)
|
if (g_dev == nullptr)
|
||||||
goto fail;
|
goto fail;
|
||||||
|
|
|
@ -57,6 +57,8 @@
|
||||||
#include <nuttx/wqueue.h>
|
#include <nuttx/wqueue.h>
|
||||||
#include <nuttx/clock.h>
|
#include <nuttx/clock.h>
|
||||||
|
|
||||||
|
#include <arch/board/board.h>
|
||||||
|
|
||||||
#include <drivers/drv_hrt.h>
|
#include <drivers/drv_hrt.h>
|
||||||
|
|
||||||
#include <systemlib/perf_counter.h>
|
#include <systemlib/perf_counter.h>
|
||||||
|
@ -240,7 +242,8 @@ private:
|
||||||
#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */
|
#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */
|
||||||
#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */
|
#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 MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */
|
||||||
|
|
||||||
#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */
|
#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */
|
||||||
|
@ -937,8 +940,7 @@ start()
|
||||||
errx(1, "already started");
|
errx(1, "already started");
|
||||||
|
|
||||||
/* create the driver */
|
/* create the driver */
|
||||||
/* XXX HORRIBLE hack - the bus number should not come from here */
|
g_dev = new MS5611(MS5611_BUS);
|
||||||
g_dev = new MS5611(2);
|
|
||||||
|
|
||||||
if (g_dev == nullptr)
|
if (g_dev == nullptr)
|
||||||
goto fail;
|
goto fail;
|
||||||
|
|
|
@ -39,4 +39,6 @@ APPNAME = fmu
|
||||||
PRIORITY = SCHED_PRIORITY_DEFAULT
|
PRIORITY = SCHED_PRIORITY_DEFAULT
|
||||||
STACKSIZE = 2048
|
STACKSIZE = 2048
|
||||||
|
|
||||||
|
INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
|
||||||
|
|
||||||
include $(APPDIR)/mk/app.mk
|
include $(APPDIR)/mk/app.mk
|
|
@ -57,16 +57,18 @@
|
||||||
#include <drivers/device/device.h>
|
#include <drivers/device/device.h>
|
||||||
#include <drivers/drv_pwm_output.h>
|
#include <drivers/drv_pwm_output.h>
|
||||||
#include <drivers/drv_gpio.h>
|
#include <drivers/drv_gpio.h>
|
||||||
|
#include <drivers/boards/px4fmu/px4fmu_internal.h>
|
||||||
|
|
||||||
|
#include <systemlib/systemlib.h>
|
||||||
#include <systemlib/mixer/mixer.h>
|
#include <systemlib/mixer/mixer.h>
|
||||||
#include <drivers/drv_mixer.h>
|
#include <drivers/drv_mixer.h>
|
||||||
|
|
||||||
#include <uORB/topics/actuator_controls.h>
|
#include <uORB/topics/actuator_controls.h>
|
||||||
#include <uORB/topics/actuator_outputs.h>
|
#include <uORB/topics/actuator_outputs.h>
|
||||||
|
|
||||||
#include <systemlib/systemlib.h>
|
#include <systemlib/err.h>
|
||||||
|
|
||||||
class FMUServo : public device::CDev
|
class PX4FMU : public device::CDev
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
enum Mode {
|
enum Mode {
|
||||||
|
@ -74,18 +76,22 @@ public:
|
||||||
MODE_4PWM,
|
MODE_4PWM,
|
||||||
MODE_NONE
|
MODE_NONE
|
||||||
};
|
};
|
||||||
FMUServo(Mode mode, int update_rate);
|
PX4FMU();
|
||||||
~FMUServo();
|
~PX4FMU();
|
||||||
|
|
||||||
virtual int ioctl(file *filp, int cmd, unsigned long arg);
|
virtual int ioctl(file *filp, int cmd, unsigned long arg);
|
||||||
|
|
||||||
virtual int init();
|
virtual int init();
|
||||||
|
|
||||||
|
int set_mode(Mode mode);
|
||||||
|
int set_pwm_rate(unsigned rate);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
static const unsigned _max_actuators = 4;
|
static const unsigned _max_actuators = 4;
|
||||||
|
|
||||||
Mode _mode;
|
Mode _mode;
|
||||||
int _update_rate;
|
int _update_rate;
|
||||||
|
int _current_update_rate;
|
||||||
int _task;
|
int _task;
|
||||||
int _t_actuators;
|
int _t_actuators;
|
||||||
int _t_armed;
|
int _t_armed;
|
||||||
|
@ -107,19 +113,50 @@ private:
|
||||||
uint8_t control_group,
|
uint8_t control_group,
|
||||||
uint8_t control_index,
|
uint8_t control_index,
|
||||||
float &input);
|
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
|
namespace
|
||||||
{
|
{
|
||||||
|
|
||||||
FMUServo *g_servo;
|
PX4FMU *g_fmu;
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|
||||||
FMUServo::FMUServo(Mode mode, int update_rate) :
|
PX4FMU::PX4FMU() :
|
||||||
CDev("fmuservo", "/dev/px4fmu"),
|
CDev("fmuservo", "/dev/px4fmu"),
|
||||||
_mode(mode),
|
_mode(MODE_NONE),
|
||||||
_update_rate(update_rate),
|
_update_rate(50),
|
||||||
_task(-1),
|
_task(-1),
|
||||||
_t_actuators(-1),
|
_t_actuators(-1),
|
||||||
_t_armed(-1),
|
_t_armed(-1),
|
||||||
|
@ -130,9 +167,10 @@ FMUServo::FMUServo(Mode mode, int update_rate) :
|
||||||
_armed(false),
|
_armed(false),
|
||||||
_mixers(nullptr)
|
_mixers(nullptr)
|
||||||
{
|
{
|
||||||
|
_debug_enabled = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
FMUServo::~FMUServo()
|
PX4FMU::~PX4FMU()
|
||||||
{
|
{
|
||||||
if (_task != -1) {
|
if (_task != -1) {
|
||||||
/* tell the task we want it to go away */
|
/* tell the task we want it to go away */
|
||||||
|
@ -156,11 +194,11 @@ FMUServo::~FMUServo()
|
||||||
if (_primary_pwm_device)
|
if (_primary_pwm_device)
|
||||||
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
|
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
|
||||||
|
|
||||||
g_servo = nullptr;
|
g_fmu = nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
int
|
int
|
||||||
FMUServo::init()
|
PX4FMU::init()
|
||||||
{
|
{
|
||||||
int ret;
|
int ret;
|
||||||
|
|
||||||
|
@ -179,12 +217,15 @@ FMUServo::init()
|
||||||
_primary_pwm_device = true;
|
_primary_pwm_device = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* reset GPIOs */
|
||||||
|
gpio_reset();
|
||||||
|
|
||||||
/* start the IO interface task */
|
/* start the IO interface task */
|
||||||
_task = task_spawn("fmuservo",
|
_task = task_spawn("fmuservo",
|
||||||
SCHED_DEFAULT,
|
SCHED_DEFAULT,
|
||||||
SCHED_PRIORITY_DEFAULT,
|
SCHED_PRIORITY_DEFAULT,
|
||||||
1024,
|
1024,
|
||||||
(main_t)&FMUServo::task_main_trampoline,
|
(main_t)&PX4FMU::task_main_trampoline,
|
||||||
nullptr);
|
nullptr);
|
||||||
|
|
||||||
if (_task < 0) {
|
if (_task < 0) {
|
||||||
|
@ -196,44 +237,73 @@ FMUServo::init()
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
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
|
int
|
||||||
FMUServo::task_main()
|
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:
|
case MODE_2PWM:
|
||||||
|
debug("MODE_2PWM");
|
||||||
/* multi-port with flow control lines as PWM */
|
/* multi-port with flow control lines as PWM */
|
||||||
/* XXX magic numbers */
|
/* XXX magic numbers */
|
||||||
up_pwm_servo_init(0x3);
|
up_pwm_servo_init(0x3);
|
||||||
|
_update_rate = 50; /* default output rate */
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MODE_4PWM:
|
case MODE_4PWM:
|
||||||
|
debug("MODE_4PWM");
|
||||||
/* multi-port as 4 PWM outs */
|
/* multi-port as 4 PWM outs */
|
||||||
/* XXX magic numbers */
|
/* XXX magic numbers */
|
||||||
up_pwm_servo_init(0xf);
|
up_pwm_servo_init(0xf);
|
||||||
/* set the update rate for 4 PWM mode only for now */
|
_update_rate = 50; /* default output rate */
|
||||||
up_pwm_servo_set_rate(_update_rate);
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MODE_NONE:
|
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;
|
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
|
* Subscribe to the appropriate PWM output topic based on whether we are the
|
||||||
* primary PWM output or not.
|
* primary PWM output or not.
|
||||||
*/
|
*/
|
||||||
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
|
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
|
||||||
ORB_ID(actuator_controls_1));
|
ORB_ID(actuator_controls_1));
|
||||||
/* convert the update rate in hz to milliseconds, rounding down if necessary */
|
/* force a reset of the update rate */
|
||||||
int update_rate_in_ms = int(1000 / _update_rate);
|
_current_update_rate = 0;
|
||||||
orb_set_interval(_t_actuators, update_rate_in_ms);
|
|
||||||
|
|
||||||
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
|
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
|
||||||
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
|
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
|
||||||
|
@ -258,7 +328,17 @@ FMUServo::task_main()
|
||||||
/* loop until killed */
|
/* loop until killed */
|
||||||
while (!_task_should_exit) {
|
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);
|
int ret = ::poll(&fds[0], 2, 1000);
|
||||||
|
|
||||||
/* this would be bad... */
|
/* this would be bad... */
|
||||||
|
@ -323,7 +403,7 @@ FMUServo::task_main()
|
||||||
}
|
}
|
||||||
|
|
||||||
int
|
int
|
||||||
FMUServo::control_callback(uintptr_t handle,
|
PX4FMU::control_callback(uintptr_t handle,
|
||||||
uint8_t control_group,
|
uint8_t control_group,
|
||||||
uint8_t control_index,
|
uint8_t control_index,
|
||||||
float &input)
|
float &input)
|
||||||
|
@ -335,11 +415,43 @@ FMUServo::control_callback(uintptr_t handle,
|
||||||
}
|
}
|
||||||
|
|
||||||
int
|
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 ret = OK;
|
||||||
int channel;
|
int channel;
|
||||||
|
|
||||||
|
lock();
|
||||||
|
|
||||||
switch (cmd) {
|
switch (cmd) {
|
||||||
case PWM_SERVO_ARM:
|
case PWM_SERVO_ARM:
|
||||||
up_pwm_servo_arm(true);
|
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);
|
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
|
||||||
|
if (_mixers == nullptr) {
|
||||||
|
ret = -ENOMEM;
|
||||||
|
} else {
|
||||||
|
|
||||||
if (_mixers->load_from_file(path) != 0) {
|
debug("loading mixers from %s", path);
|
||||||
delete _mixers;
|
ret = _mixers->load_from_file(path);
|
||||||
_mixers = nullptr;
|
|
||||||
ret = -EINVAL;
|
if (ret != 0) {
|
||||||
|
debug("mixer load failed with %d", ret);
|
||||||
|
delete _mixers;
|
||||||
|
_mixers = nullptr;
|
||||||
|
ret = -EINVAL;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
@ -452,6 +572,125 @@ FMUServo::ioctl(file *filp, int cmd, unsigned long arg)
|
||||||
break;
|
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<<i)) {
|
||||||
|
switch (function) {
|
||||||
|
case GPIO_SET_INPUT:
|
||||||
|
stm32_configgpio(_gpio_tab[i].input);
|
||||||
|
break;
|
||||||
|
case GPIO_SET_OUTPUT:
|
||||||
|
stm32_configgpio(_gpio_tab[i].output);
|
||||||
|
break;
|
||||||
|
case GPIO_SET_ALT_1:
|
||||||
|
if (_gpio_tab[i].alt != 0)
|
||||||
|
stm32_configgpio(_gpio_tab[i].alt);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* flip buffer to input mode if required */
|
||||||
|
if ((GPIO_SET_INPUT == function) && (gpios & 3))
|
||||||
|
stm32_gpiowrite(GPIO_GPIO_DIR, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
PX4FMU::gpio_write(uint32_t gpios, int function)
|
||||||
|
{
|
||||||
|
int value = (function == GPIO_SET) ? 1 : 0;
|
||||||
|
|
||||||
|
for (unsigned i = 0; i < _ngpio; i++)
|
||||||
|
if (gpios & (1<<i))
|
||||||
|
stm32_gpiowrite(_gpio_tab[i].output, value);
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t
|
||||||
|
PX4FMU::gpio_read(void)
|
||||||
|
{
|
||||||
|
uint32_t bits = 0;
|
||||||
|
|
||||||
|
for (unsigned i = 0; i < _ngpio; i++)
|
||||||
|
if (stm32_gpioread(_gpio_tab[i].input))
|
||||||
|
bits |= (1 << i);
|
||||||
|
|
||||||
|
return bits;
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||||
|
{
|
||||||
|
int ret = OK;
|
||||||
|
|
||||||
|
lock();
|
||||||
|
|
||||||
|
switch (cmd) {
|
||||||
|
|
||||||
|
case GPIO_RESET:
|
||||||
|
gpio_reset();
|
||||||
|
break;
|
||||||
|
|
||||||
|
case GPIO_SET_OUTPUT:
|
||||||
|
case GPIO_SET_INPUT:
|
||||||
|
case GPIO_SET_ALT_1:
|
||||||
|
gpio_set_function(arg, cmd);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case GPIO_SET_ALT_2:
|
||||||
|
case GPIO_SET_ALT_3:
|
||||||
|
case GPIO_SET_ALT_4:
|
||||||
|
ret = -EINVAL;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case GPIO_SET:
|
||||||
|
case GPIO_CLEAR:
|
||||||
|
gpio_write(arg, cmd);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case GPIO_GET:
|
||||||
|
*(uint32_t *)arg = gpio_read();
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
ret = -ENOTTY;
|
||||||
|
}
|
||||||
|
|
||||||
|
unlock();
|
||||||
|
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -473,28 +712,14 @@ PortMode g_port_mode;
|
||||||
int
|
int
|
||||||
fmu_new_mode(PortMode new_mode, int update_rate)
|
fmu_new_mode(PortMode new_mode, int update_rate)
|
||||||
{
|
{
|
||||||
int fd;
|
|
||||||
int ret = OK;
|
|
||||||
uint32_t gpio_bits;
|
uint32_t gpio_bits;
|
||||||
FMUServo::Mode servo_mode;
|
PX4FMU::Mode servo_mode;
|
||||||
|
|
||||||
/* get hold of the GPIO configuration descriptor */
|
|
||||||
fd = open(GPIO_DEVICE_PATH, 0);
|
|
||||||
|
|
||||||
if (fd < 0)
|
|
||||||
return -errno;
|
|
||||||
|
|
||||||
/* start by tearing down any existing state and revert to all-GPIO-inputs */
|
|
||||||
if (g_servo != nullptr) {
|
|
||||||
delete g_servo;
|
|
||||||
g_servo = nullptr;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* reset to all-inputs */
|
/* reset to all-inputs */
|
||||||
ioctl(fd, GPIO_RESET, 0);
|
g_fmu->ioctl(0, GPIO_RESET, 0);
|
||||||
|
|
||||||
gpio_bits = 0;
|
gpio_bits = 0;
|
||||||
servo_mode = FMUServo::MODE_NONE;
|
servo_mode = PX4FMU::MODE_NONE;
|
||||||
|
|
||||||
switch (new_mode) {
|
switch (new_mode) {
|
||||||
case PORT_FULL_GPIO:
|
case PORT_FULL_GPIO:
|
||||||
|
@ -509,7 +734,7 @@ fmu_new_mode(PortMode new_mode, int update_rate)
|
||||||
|
|
||||||
case PORT_FULL_PWM:
|
case PORT_FULL_PWM:
|
||||||
/* select 4-pin PWM mode */
|
/* select 4-pin PWM mode */
|
||||||
servo_mode = FMUServo::MODE_4PWM;
|
servo_mode = PX4FMU::MODE_4PWM;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case PORT_GPIO_AND_SERIAL:
|
case PORT_GPIO_AND_SERIAL:
|
||||||
|
@ -519,36 +744,47 @@ fmu_new_mode(PortMode new_mode, int update_rate)
|
||||||
|
|
||||||
case PORT_PWM_AND_SERIAL:
|
case PORT_PWM_AND_SERIAL:
|
||||||
/* select 2-pin PWM mode */
|
/* select 2-pin PWM mode */
|
||||||
servo_mode = FMUServo::MODE_2PWM;
|
servo_mode = PX4FMU::MODE_2PWM;
|
||||||
/* set RX/TX multi-GPIOs to serial mode */
|
/* set RX/TX multi-GPIOs to serial mode */
|
||||||
gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
|
gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case PORT_PWM_AND_GPIO:
|
case PORT_PWM_AND_GPIO:
|
||||||
/* select 2-pin PWM mode */
|
/* select 2-pin PWM mode */
|
||||||
servo_mode = FMUServo::MODE_2PWM;
|
servo_mode = PX4FMU::MODE_2PWM;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* adjust GPIO config for serial mode(s) */
|
/* adjust GPIO config for serial mode(s) */
|
||||||
if (gpio_bits != 0)
|
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 */
|
return OK;
|
||||||
if (servo_mode != FMUServo::MODE_NONE) {
|
}
|
||||||
g_servo = new FMUServo(servo_mode, update_rate);
|
|
||||||
|
|
||||||
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;
|
ret = -ENOMEM;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
ret = g_servo->init();
|
ret = g_fmu->init();
|
||||||
|
|
||||||
if (ret != OK) {
|
if (ret != OK) {
|
||||||
delete g_servo;
|
delete g_fmu;
|
||||||
g_servo = nullptr;
|
g_fmu = nullptr;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -612,7 +848,7 @@ int
|
||||||
fmu_main(int argc, char *argv[])
|
fmu_main(int argc, char *argv[])
|
||||||
{
|
{
|
||||||
PortMode new_mode = PORT_MODE_UNSET;
|
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"))
|
if (!strcmp(argv[1], "test"))
|
||||||
test();
|
test();
|
||||||
|
@ -620,6 +856,9 @@ fmu_main(int argc, char *argv[])
|
||||||
if (!strcmp(argv[1], "fake"))
|
if (!strcmp(argv[1], "fake"))
|
||||||
fake(argc - 1, argv + 1);
|
fake(argc - 1, argv + 1);
|
||||||
|
|
||||||
|
if (fmu_start() != OK)
|
||||||
|
errx(1, "failed to start the FMU driver");
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Mode switches.
|
* Mode switches.
|
||||||
*
|
*
|
||||||
|
@ -674,6 +913,6 @@ fmu_main(int argc, char *argv[])
|
||||||
/* test, etc. here */
|
/* test, etc. here */
|
||||||
|
|
||||||
fprintf(stderr, "FMU: unrecognised command, try:\n");
|
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;
|
return -EINVAL;
|
||||||
}
|
}
|
|
@ -84,7 +84,7 @@
|
||||||
#include <systemlib/err.h>
|
#include <systemlib/err.h>
|
||||||
|
|
||||||
#ifndef CONFIG_HRT_TIMER
|
#ifndef CONFIG_HRT_TIMER
|
||||||
# error CONFIG_TONE_ALARM requires CONFIG_HRT_TIMER
|
# error This driver requires CONFIG_HRT_TIMER
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
/* Tone alarm configuration */
|
/* Tone alarm configuration */
|
||||||
|
@ -611,7 +611,6 @@ play_pattern(unsigned pattern)
|
||||||
if (fd < 0)
|
if (fd < 0)
|
||||||
err(1, "/dev/tone_alarm");
|
err(1, "/dev/tone_alarm");
|
||||||
|
|
||||||
warnx("playing pattern %u", pattern);
|
|
||||||
ret = ioctl(fd, TONE_SET_ALARM, pattern);
|
ret = ioctl(fd, TONE_SET_ALARM, pattern);
|
||||||
|
|
||||||
if (ret != 0)
|
if (ret != 0)
|
||||||
|
|
|
@ -531,6 +531,7 @@ uorb_receive_thread(void *arg)
|
||||||
struct pollfd fds[n_listeners];
|
struct pollfd fds[n_listeners];
|
||||||
for (unsigned i = 0; i < n_listeners; i++) {
|
for (unsigned i = 0; i < n_listeners; i++) {
|
||||||
fds[i].fd = *listeners[i].subp;
|
fds[i].fd = *listeners[i].subp;
|
||||||
|
fds[i].events = POLLIN;
|
||||||
|
|
||||||
/* Invoke callback to set initial state */
|
/* Invoke callback to set initial state */
|
||||||
//listeners[i].callback(&listener[i]);
|
//listeners[i].callback(&listener[i]);
|
||||||
|
|
|
@ -41,14 +41,13 @@
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
|
||||||
#include "systemlib/systemlib.h"
|
#include <systemlib/systemlib.h>
|
||||||
|
|
||||||
__EXPORT int reboot_main(int argc, char *argv[]);
|
__EXPORT int reboot_main(int argc, char *argv[]);
|
||||||
|
|
||||||
int reboot_main(int argc, char *argv[])
|
int reboot_main(int argc, char *argv[])
|
||||||
{
|
{
|
||||||
reboot();
|
up_systemreset();
|
||||||
return 0;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -42,7 +42,8 @@ CSRCS = err.c \
|
||||||
bson/tinybson.c \
|
bson/tinybson.c \
|
||||||
conversions.c \
|
conversions.c \
|
||||||
cpuload.c \
|
cpuload.c \
|
||||||
getopt_long.c
|
getopt_long.c \
|
||||||
|
up_cxxinitialize.c
|
||||||
|
|
||||||
# ppm_decode.c \
|
# ppm_decode.c \
|
||||||
|
|
||||||
|
|
|
@ -73,41 +73,6 @@ const struct __multiport_info multiport_info = {
|
||||||
|
|
||||||
static void kill_task(FAR _TCB *tcb, FAR void *arg);
|
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()
|
void killall()
|
||||||
{
|
{
|
||||||
// printf("Sending SIGUSR1 to all processes now\n");
|
// printf("Sending SIGUSR1 to all processes now\n");
|
||||||
|
|
|
@ -45,7 +45,7 @@
|
||||||
__BEGIN_DECLS
|
__BEGIN_DECLS
|
||||||
|
|
||||||
/** Reboots the board */
|
/** Reboots the board */
|
||||||
__EXPORT int reboot(void);
|
extern void up_systemreset(void) noreturn_function;
|
||||||
|
|
||||||
/** Sends SIGUSR1 to all processes */
|
/** Sends SIGUSR1 to all processes */
|
||||||
__EXPORT void killall(void);
|
__EXPORT void killall(void);
|
||||||
|
|
|
@ -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 <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 <debug.h>
|
||||||
|
|
||||||
|
#include <nuttx/arch.h>
|
||||||
|
|
||||||
|
//#include <arch/stm32/chip.h>
|
||||||
|
//#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();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -87,6 +87,15 @@ SECTIONS
|
||||||
__errno = get_errno_ptr;
|
__errno = get_errno_ptr;
|
||||||
} > flash
|
} > flash
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Init functions (static constructors and the like)
|
||||||
|
*/
|
||||||
|
.init_section : {
|
||||||
|
_sinit = ABSOLUTE(.);
|
||||||
|
KEEP(*(.init_array .init_array.*))
|
||||||
|
_einit = ABSOLUTE(.);
|
||||||
|
} > flash
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Construction data for parameters.
|
* Construction data for parameters.
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -45,9 +45,6 @@
|
||||||
#ifndef __ASSEMBLY__
|
#ifndef __ASSEMBLY__
|
||||||
# include <stdint.h>
|
# include <stdint.h>
|
||||||
#endif
|
#endif
|
||||||
//#include "stm32_rcc.h"
|
|
||||||
//#include "stm32_sdio.h"
|
|
||||||
//#include "stm32_internal.h"
|
|
||||||
|
|
||||||
/************************************************************************************
|
/************************************************************************************
|
||||||
* Definitions
|
* Definitions
|
||||||
|
@ -298,7 +295,7 @@
|
||||||
* Note that these are unshifted addresses.
|
* Note that these are unshifted addresses.
|
||||||
*/
|
*/
|
||||||
#define PX4_I2C_OBDEV_HMC5883 0x1e
|
#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_EEPROM NOTDEFINED
|
||||||
|
|
||||||
#define PX4_I2C_OBDEV_PX4IO_BL 0x18
|
#define PX4_I2C_OBDEV_PX4IO_BL 0x18
|
||||||
|
@ -327,11 +324,9 @@
|
||||||
/*
|
/*
|
||||||
* Tone alarm output
|
* Tone alarm output
|
||||||
*/
|
*/
|
||||||
#ifdef CONFIG_TONE_ALARM
|
#define TONE_ALARM_TIMER 3 /* timer 3 */
|
||||||
# define TONE_ALARM_TIMER 3 /* timer 3 */
|
#define TONE_ALARM_CHANNEL 3 /* channel 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)
|
||||||
# define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/************************************************************************************
|
/************************************************************************************
|
||||||
* Public Data
|
* Public Data
|
||||||
|
@ -362,37 +357,6 @@ extern "C" {
|
||||||
|
|
||||||
EXTERN void stm32_boardinitialize(void);
|
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
|
#undef EXTERN
|
||||||
#if defined(__cplusplus)
|
#if defined(__cplusplus)
|
||||||
}
|
}
|
||||||
|
|
|
@ -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 */
|
|
|
@ -97,7 +97,7 @@ CONFIGURED_APPS += drivers/l3gd20
|
||||||
CONFIGURED_APPS += drivers/px4io
|
CONFIGURED_APPS += drivers/px4io
|
||||||
CONFIGURED_APPS += drivers/stm32
|
CONFIGURED_APPS += drivers/stm32
|
||||||
CONFIGURED_APPS += drivers/stm32/tone_alarm
|
CONFIGURED_APPS += drivers/stm32/tone_alarm
|
||||||
CONFIGURED_APPS += px4/fmu
|
CONFIGURED_APPS += drivers/px4fmu
|
||||||
|
|
||||||
# Testing stuff
|
# Testing stuff
|
||||||
CONFIGURED_APPS += px4/sensors_bringup
|
CONFIGURED_APPS += px4/sensors_bringup
|
||||||
|
|
|
@ -179,7 +179,7 @@ CONFIG_STM32_I2C1=y
|
||||||
CONFIG_STM32_I2C2=y
|
CONFIG_STM32_I2C2=y
|
||||||
CONFIG_STM32_I2C3=y
|
CONFIG_STM32_I2C3=y
|
||||||
CONFIG_STM32_CAN1=n
|
CONFIG_STM32_CAN1=n
|
||||||
CONFIG_STM32_CAN2=y
|
CONFIG_STM32_CAN2=n
|
||||||
CONFIG_STM32_DAC=n
|
CONFIG_STM32_DAC=n
|
||||||
CONFIG_STM32_PWR=y
|
CONFIG_STM32_PWR=y
|
||||||
# APB2:
|
# APB2:
|
||||||
|
@ -542,7 +542,7 @@ CONFIG_DEBUG_I2C=n
|
||||||
CONFIG_DEBUG_INPUT=n
|
CONFIG_DEBUG_INPUT=n
|
||||||
|
|
||||||
CONFIG_HAVE_CXX=y
|
CONFIG_HAVE_CXX=y
|
||||||
CONFIG_HAVE_CXXINITIALIZE=n
|
CONFIG_HAVE_CXXINITIALIZE=y
|
||||||
CONFIG_MM_REGIONS=2
|
CONFIG_MM_REGIONS=2
|
||||||
CONFIG_ARCH_LOWPUTC=y
|
CONFIG_ARCH_LOWPUTC=y
|
||||||
CONFIG_MSEC_PER_TICK=1
|
CONFIG_MSEC_PER_TICK=1
|
||||||
|
|
|
@ -41,7 +41,6 @@ ASRCS =
|
||||||
AOBJS = $(ASRCS:.S=$(OBJEXT))
|
AOBJS = $(ASRCS:.S=$(OBJEXT))
|
||||||
|
|
||||||
CSRCS = up_leds.c \
|
CSRCS = up_leds.c \
|
||||||
drv_gpio.c \
|
|
||||||
drv_led.c drv_eeprom.c
|
drv_led.c drv_eeprom.c
|
||||||
|
|
||||||
COBJS = $(CSRCS:.c=$(OBJEXT))
|
COBJS = $(CSRCS:.c=$(OBJEXT))
|
||||||
|
|
|
@ -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 <nuttx/config.h>
|
|
||||||
|
|
||||||
#include <stdint.h>
|
|
||||||
#include <stdbool.h>
|
|
||||||
#include <debug.h>
|
|
||||||
#include <errno.h>
|
|
||||||
|
|
||||||
#include <nuttx/spi.h>
|
|
||||||
#include <arch/board/board.h>
|
|
||||||
|
|
||||||
#include "up_arch.h"
|
|
||||||
#include "chip.h"
|
|
||||||
#include "stm32_internal.h"
|
|
||||||
#include "px4fmu-internal.h"
|
|
||||||
|
|
||||||
#include <arch/board/drv_gpio.h>
|
|
||||||
|
|
||||||
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<<i)) {
|
|
||||||
switch (function) {
|
|
||||||
case GPIO_SET_INPUT:
|
|
||||||
stm32_configgpio(gpio_tab[i].input);
|
|
||||||
break;
|
|
||||||
case GPIO_SET_OUTPUT:
|
|
||||||
stm32_configgpio(gpio_tab[i].output);
|
|
||||||
break;
|
|
||||||
case GPIO_SET_ALT_1:
|
|
||||||
if (gpio_tab[i].alt != 0)
|
|
||||||
stm32_configgpio(gpio_tab[i].alt);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/* flip buffer to input mode if required */
|
|
||||||
if ((GPIO_SET_INPUT == function) && (gpios & 3))
|
|
||||||
stm32_gpiowrite(GPIO_GPIO_DIR, 0);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void
|
|
||||||
px4fmu_gpio_write(uint32_t gpios, int function)
|
|
||||||
{
|
|
||||||
int value = (function == GPIO_SET) ? 1 : 0;
|
|
||||||
|
|
||||||
for (unsigned i = 0; i < NGPIO; i++)
|
|
||||||
if (gpios & (1<<i))
|
|
||||||
stm32_gpiowrite(gpio_tab[i].output, value);
|
|
||||||
}
|
|
||||||
|
|
||||||
static uint32_t
|
|
||||||
px4fmu_gpio_read(void)
|
|
||||||
{
|
|
||||||
uint32_t bits = 0;
|
|
||||||
|
|
||||||
for (unsigned i = 0; i < NGPIO; i++)
|
|
||||||
if (stm32_gpioread(gpio_tab[i].input))
|
|
||||||
bits |= (1 << i);
|
|
||||||
|
|
||||||
return bits;
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
px4fmu_gpio_init(void)
|
|
||||||
{
|
|
||||||
/* reset all GPIOs to default state */
|
|
||||||
px4fmu_gpio_reset();
|
|
||||||
|
|
||||||
/* register the driver */
|
|
||||||
register_driver(GPIO_DEVICE_PATH, &px4fmu_gpio_fops, 0666, NULL);
|
|
||||||
}
|
|
||||||
|
|
||||||
static int
|
|
||||||
px4fmu_gpio_ioctl(struct file *filep, int cmd, unsigned long arg)
|
|
||||||
{
|
|
||||||
int result = OK;
|
|
||||||
|
|
||||||
switch (cmd) {
|
|
||||||
|
|
||||||
case GPIO_RESET:
|
|
||||||
px4fmu_gpio_reset();
|
|
||||||
break;
|
|
||||||
|
|
||||||
case GPIO_SET_OUTPUT:
|
|
||||||
case GPIO_SET_INPUT:
|
|
||||||
case GPIO_SET_ALT_1:
|
|
||||||
px4fmu_gpio_set_function(arg, cmd);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case GPIO_SET:
|
|
||||||
case GPIO_CLEAR:
|
|
||||||
px4fmu_gpio_write(arg, cmd);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case GPIO_GET:
|
|
||||||
*(uint32_t *)arg = px4fmu_gpio_read();
|
|
||||||
break;
|
|
||||||
|
|
||||||
default:
|
|
||||||
result = -ENOTTY;
|
|
||||||
}
|
|
||||||
return result;
|
|
||||||
}
|
|
||||||
|
|
Loading…
Reference in New Issue