dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge remote-tracking branch 'origin/master' into fw_control

This commit is contained in:
Thomas Gubler 2012-10-28 15:27:13 +01:00
parent e5f56a1a8f 1a70b2f4ed
commit 8d764170aa
22 changed files with 563 additions and 492 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
apps/commander/state_machine_helper.c
View File

@ -138,7 +138,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
current_status->flag_system_armed = false;
mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM");
usleep(500000);
reboot();
up_systemreset();
/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
} else {
invalid_state = true;

3
apps/drivers/boards/px4fmu/px4fmu_init.c
View File

@ -294,9 +294,6 @@ __EXPORT int nsh_archinitialize(void)
/* Get board information if available */
/* Initialize the user GPIOs */
px4fmu_gpio_init();
#ifdef CONFIG_ADC
int adc_state = adc_devinit();

21
apps/drivers/boards/px4fmu/px4fmu_internal.h
View File

@ -47,6 +47,12 @@
#include <nuttx/compiler.h>
#include <stdint.h>
__BEGIN_DECLS
/* these headers are not C++ safe */
#include <stm32_internal.h>
/****************************************************************************************************
* Definitions
****************************************************************************************************/
@ -150,17 +156,6 @@
extern void stm32_spiinitialize(void);
/****************************************************************************************************
* Name: px4fmu_gpio_init
*
* Description:
* Called to configure the PX4FMU user GPIOs
*
****************************************************************************************************/
extern void px4fmu_gpio_init(void);
// XXX additional SPI chipselect functions required?
#endif /* __ASSEMBLY__ */
__END_DECLS

82
apps/drivers/drv_gpio.h
View File

@ -40,25 +40,75 @@
#include <sys/ioctl.h>
#ifdef CONFIG_ARCH_BOARD_PX4FMU
/*
* GPIO defines come from a board-specific header, as they are shared
* with board-specific logic.
* PX4FMU GPIO numbers.
*
* The board-specific header must define:
* GPIO_DEVICE_PATH
* GPIO_RESET
* GPIO_SET_OUTPUT
* GPIO_SET_INPUT
* GPIO_SET_ALT_1
* GPIO_SET_ALT_2
* GPIO_SET_ALT_3
* GPIO_SET_ALT_4
* GPIO_SET
* GPIO_CLEAR
* GPIO_GET
* For shared pins, alternate function 1 selects the non-GPIO mode
* (USART2, CAN2, etc.)
*/
# define GPIO_EXT_1 (1<<0) /**< high-power GPIO 1 */
# define GPIO_EXT_2 (1<<1) /**< high-power GPIO 1 */
# define GPIO_MULTI_1 (1<<2) /**< USART2 CTS */
# define GPIO_MULTI_2 (1<<3) /**< USART2 RTS */
# define GPIO_MULTI_3 (1<<4) /**< USART2 TX */
# define GPIO_MULTI_4 (1<<5) /**< USART2 RX */
# define GPIO_CAN_TX (1<<6) /**< CAN2 TX */
# define GPIO_CAN_RX (1<<7) /**< CAN2 RX */
/* Include board-specific GPIO definitions as well. */
#include <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 */

11
apps/drivers/hmc5883/hmc5883.cpp
View File

@ -58,17 +58,21 @@
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h>
/*
* HMC5883 internal constants and data structures.
*/
#define HMC5883L_BUS PX4_I2C_BUS_ONBOARD
#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883
/* Max measurement rate is 160Hz */
#define HMC5883_CONVERSION_INTERVAL (1000000 / 160) /* microseconds */
@ -86,8 +90,6 @@
#define ADDR_ID_B 0x0b
#define ADDR_ID_C 0x0c
#define HMC5883L_ADDRESS 0x1E
/* modes not changeable outside of driver */
#define HMC5883L_MODE_NORMAL (0 << 0) /* default */
#define HMC5883L_MODE_POSITIVE_BIAS (1 << 0) /* positive bias */
@ -1100,8 +1102,7 @@ start()
errx(1, "already started");
/* create the driver */
/* XXX HORRIBLE hack - the bus number should not come from here */
g_dev = new HMC5883(2);
g_dev = new HMC5883(HMC5883L_BUS);
if (g_dev == nullptr)
goto fail;

8
apps/drivers/ms5611/ms5611.cpp
View File

@ -57,6 +57,8 @@
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <arch/board/board.h>
#include <drivers/drv_hrt.h>
#include <systemlib/perf_counter.h>
@ -240,7 +242,8 @@ private:
#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */
#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */
#define MS5611_ADDRESS_1 0x76 /* address select pins pulled high (PX4FMU series v1.6+) */
#define MS5611_BUS PX4_I2C_BUS_ONBOARD
#define MS5611_ADDRESS_1 PX4_I2C_OBDEV_MS5611 /* address select pins pulled high (PX4FMU series v1.6+) */
#define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */
#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */
@ -937,8 +940,7 @@ start()
errx(1, "already started");
/* create the driver */
/* XXX HORRIBLE hack - the bus number should not come from here */
g_dev = new MS5611(2);
g_dev = new MS5611(MS5611_BUS);
if (g_dev == nullptr)
goto fail;

2
apps/px4/fmu/Makefile → apps/drivers/px4fmu/Makefile
View File

@ -39,4 +39,6 @@ APPNAME = fmu
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
include $(APPDIR)/mk/app.mk

357
apps/px4/fmu/fmu.cpp → apps/drivers/px4fmu/fmu.cpp
View File

@ -57,16 +57,18 @@
#include <drivers/device/device.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
#include <drivers/boards/px4fmu/px4fmu_internal.h>
#include <systemlib/systemlib.h>
#include <systemlib/mixer/mixer.h>
#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.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:
enum Mode {
@ -74,18 +76,22 @@ public:
MODE_4PWM,
MODE_NONE
};
FMUServo(Mode mode, int update_rate);
~FMUServo();
PX4FMU();
~PX4FMU();
virtual int ioctl(file *filp, int cmd, unsigned long arg);
virtual int init();
int set_mode(Mode mode);
int set_pwm_rate(unsigned rate);
private:
static const unsigned _max_actuators = 4;
Mode _mode;
int _update_rate;
int _current_update_rate;
int _task;
int _t_actuators;
int _t_armed;
@ -107,19 +113,50 @@ private:
uint8_t control_group,
uint8_t control_index,
float &input);
int pwm_ioctl(file *filp, int cmd, unsigned long arg);
struct GPIOConfig {
uint32_t input;
uint32_t output;
uint32_t alt;
};
static const GPIOConfig _gpio_tab[];
static const unsigned _ngpio;
void gpio_reset(void);
void gpio_set_function(uint32_t gpios, int function);
void gpio_write(uint32_t gpios, int function);
uint32_t gpio_read(void);
int gpio_ioctl(file *filp, int cmd, unsigned long arg);
};
const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
{GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1},
{GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1},
{GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1},
{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1},
{GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2},
{GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2},
};
const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]);
namespace
{
FMUServo *g_servo;
PX4FMU *g_fmu;
} // namespace
FMUServo::FMUServo(Mode mode, int update_rate) :
PX4FMU::PX4FMU() :
CDev("fmuservo", "/dev/px4fmu"),
_mode(mode),
_update_rate(update_rate),
_mode(MODE_NONE),
_update_rate(50),
_task(-1),
_t_actuators(-1),
_t_armed(-1),
@ -130,9 +167,10 @@ FMUServo::FMUServo(Mode mode, int update_rate) :
_armed(false),
_mixers(nullptr)
{
_debug_enabled = true;
}
FMUServo::~FMUServo()
PX4FMU::~PX4FMU()
{
if (_task != -1) {
/* tell the task we want it to go away */
@ -156,11 +194,11 @@ FMUServo::~FMUServo()
if (_primary_pwm_device)
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
g_servo = nullptr;
g_fmu = nullptr;
}
int
FMUServo::init()
PX4FMU::init()
{
int ret;
@ -179,12 +217,15 @@ FMUServo::init()
_primary_pwm_device = true;
}
/* reset GPIOs */
gpio_reset();
/* start the IO interface task */
_task = task_spawn("fmuservo",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
1024,
(main_t)&FMUServo::task_main_trampoline,
(main_t)&PX4FMU::task_main_trampoline,
nullptr);
if (_task < 0) {
@ -196,44 +237,73 @@ FMUServo::init()
}
void
FMUServo::task_main_trampoline(int argc, char *argv[])
PX4FMU::task_main_trampoline(int argc, char *argv[])
{
g_servo->task_main();
g_fmu->task_main();
}
void
FMUServo::task_main()
int
PX4FMU::set_mode(Mode mode)
{
/* configure for PWM output */
switch (_mode) {
/*
* Configure for PWM output.
*
* Note that regardless of the configured mode, the task is always
* listening and mixing; the mode just selects which of the channels
* are presented on the output pins.
*/
switch (mode) {
case MODE_2PWM:
debug("MODE_2PWM");
/* multi-port with flow control lines as PWM */
/* XXX magic numbers */
up_pwm_servo_init(0x3);
_update_rate = 50; /* default output rate */
break;
case MODE_4PWM:
debug("MODE_4PWM");
/* multi-port as 4 PWM outs */
/* XXX magic numbers */
up_pwm_servo_init(0xf);
/* set the update rate for 4 PWM mode only for now */
up_pwm_servo_set_rate(_update_rate);
_update_rate = 50; /* default output rate */
break;
case MODE_NONE:
/* we should never get here... */
debug("MODE_NONE");
/* disable servo outputs and set a very low update rate */
up_pwm_servo_deinit();
_update_rate = 10;
break;
default:
return -EINVAL;
}
_mode = mode;
return OK;
}
int
PX4FMU::set_pwm_rate(unsigned rate)
{
if ((rate > 500) || (rate < 10))
return -EINVAL;
_update_rate = rate;
return OK;
}
void
PX4FMU::task_main()
{
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not.
*/
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1));
/* convert the update rate in hz to milliseconds, rounding down if necessary */
int update_rate_in_ms = int(1000 / _update_rate);
orb_set_interval(_t_actuators, update_rate_in_ms);
/* force a reset of the update rate */
_current_update_rate = 0;
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
@ -258,7 +328,17 @@ FMUServo::task_main()
/* loop until killed */
while (!_task_should_exit) {
/* sleep waiting for data, but no more than 100ms */
/* handle update rate changes */
if (_current_update_rate != _update_rate) {
int update_rate_in_ms = int(1000 / _update_rate);
if (update_rate_in_ms < 2)
update_rate_in_ms = 2;
orb_set_interval(_t_actuators, update_rate_in_ms);
up_pwm_servo_set_rate(_update_rate);
_current_update_rate = _update_rate;
}
/* sleep waiting for data, but no more than a second */
int ret = ::poll(&fds[0], 2, 1000);
/* this would be bad... */
@ -323,7 +403,7 @@ FMUServo::task_main()
}
int
FMUServo::control_callback(uintptr_t handle,
PX4FMU::control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
@ -335,11 +415,43 @@ FMUServo::control_callback(uintptr_t handle,
}
int
FMUServo::ioctl(file *filp, int cmd, unsigned long arg)
PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
{
int ret;
debug("ioctl 0x%04x 0x%08x", cmd, arg);
/* try it as a GPIO ioctl first */
ret = gpio_ioctl(filp, cmd, arg);
if (ret != -ENOTTY)
return ret;
/* if we are in valid PWM mode, try it as a PWM ioctl as well */
switch(_mode) {
case MODE_2PWM:
case MODE_4PWM:
ret = pwm_ioctl(filp, cmd, arg);
break;
default:
debug("not in a PWM mode");
break;
}
/* if nobody wants it, let CDev have it */
if (ret == -ENOTTY)
ret = CDev::ioctl(filp, cmd, arg);
return ret;
}
int
PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
{
int ret = OK;
int channel;
lock();
switch (cmd) {
case PWM_SERVO_ARM:
up_pwm_servo_arm(true);
@ -437,12 +549,20 @@ FMUServo::ioctl(file *filp, int cmd, unsigned long arg)
}
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers == nullptr) {
ret = -ENOMEM;
} else {
if (_mixers->load_from_file(path) != 0) {
debug("loading mixers from %s", path);
ret = _mixers->load_from_file(path);
if (ret != 0) {
debug("mixer load failed with %d", ret);
delete _mixers;
_mixers = nullptr;
ret = -EINVAL;
}
}
break;
}
@ -452,6 +572,125 @@ FMUServo::ioctl(file *filp, int cmd, unsigned long arg)
break;
}
unlock();
return ret;
}
void
PX4FMU::gpio_reset(void)
{
/*
* Setup default GPIO config - all pins as GPIOs, GPIO driver chip
* to input mode.
*/
for (unsigned i = 0; i < _ngpio; i++)
stm32_configgpio(_gpio_tab[i].input);
stm32_gpiowrite(GPIO_GPIO_DIR, 0);
stm32_configgpio(GPIO_GPIO_DIR);
}
void
PX4FMU::gpio_set_function(uint32_t gpios, int function)
{
/*
* GPIOs 0 and 1 must have the same direction as they are buffered
* by a shared 2-port driver. Any attempt to set either sets both.
*/
if (gpios & 3) {
gpios |= 3;
/* flip the buffer to output mode if required */
if (GPIO_SET_OUTPUT == function)
stm32_gpiowrite(GPIO_GPIO_DIR, 1);
}
/* configure selected GPIOs as required */
for (unsigned i = 0; i < _ngpio; i++) {
if (gpios & (1<<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;
}
@ -473,28 +712,14 @@ PortMode g_port_mode;
int
fmu_new_mode(PortMode new_mode, int update_rate)
{
int fd;
int ret = OK;
uint32_t gpio_bits;
FMUServo::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;
}
PX4FMU::Mode servo_mode;
/* reset to all-inputs */
ioctl(fd, GPIO_RESET, 0);
g_fmu->ioctl(0, GPIO_RESET, 0);
gpio_bits = 0;
servo_mode = FMUServo::MODE_NONE;
servo_mode = PX4FMU::MODE_NONE;
switch (new_mode) {
case PORT_FULL_GPIO:
@ -509,7 +734,7 @@ fmu_new_mode(PortMode new_mode, int update_rate)
case PORT_FULL_PWM:
/* select 4-pin PWM mode */
servo_mode = FMUServo::MODE_4PWM;
servo_mode = PX4FMU::MODE_4PWM;
break;
case PORT_GPIO_AND_SERIAL:
@ -519,36 +744,47 @@ fmu_new_mode(PortMode new_mode, int update_rate)
case PORT_PWM_AND_SERIAL:
/* select 2-pin PWM mode */
servo_mode = FMUServo::MODE_2PWM;
servo_mode = PX4FMU::MODE_2PWM;
/* set RX/TX multi-GPIOs to serial mode */
gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
break;
case PORT_PWM_AND_GPIO:
/* select 2-pin PWM mode */
servo_mode = FMUServo::MODE_2PWM;
servo_mode = PX4FMU::MODE_2PWM;
break;
}
/* adjust GPIO config for serial mode(s) */
if (gpio_bits != 0)
ioctl(fd, GPIO_SET_ALT_1, gpio_bits);
g_fmu->ioctl(0, GPIO_SET_ALT_1, gpio_bits);
close(fd);
/* (re)set the PWM output mode */
g_fmu->set_mode(servo_mode);
if ((servo_mode != PX4FMU::MODE_NONE) && (update_rate != 0))
g_fmu->set_pwm_rate(update_rate);
/* create new PWM driver if required */
if (servo_mode != FMUServo::MODE_NONE) {
g_servo = new FMUServo(servo_mode, update_rate);
return OK;
}
if (g_servo == nullptr) {
int
fmu_start(void)
{
int ret = OK;
if (g_fmu == nullptr) {
g_fmu = new PX4FMU;
if (g_fmu == nullptr) {
ret = -ENOMEM;
} else {
ret = g_servo->init();
ret = g_fmu->init();
if (ret != OK) {
delete g_servo;
g_servo = nullptr;
delete g_fmu;
g_fmu = nullptr;
}
}
}
@ -612,7 +848,7 @@ int
fmu_main(int argc, char *argv[])
{
PortMode new_mode = PORT_MODE_UNSET;
int pwm_update_rate_in_hz = 50;
int pwm_update_rate_in_hz = 0;
if (!strcmp(argv[1], "test"))
test();
@ -620,6 +856,9 @@ fmu_main(int argc, char *argv[])
if (!strcmp(argv[1], "fake"))
fake(argc - 1, argv + 1);
if (fmu_start() != OK)
errx(1, "failed to start the FMU driver");
/*
* Mode switches.
*
@ -674,6 +913,6 @@ fmu_main(int argc, char *argv[])
/* test, etc. here */
fprintf(stderr, "FMU: unrecognised command, try:\n");
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-r pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
return -EINVAL;
}

3
apps/drivers/stm32/tone_alarm/tone_alarm.cpp
View File

@ -84,7 +84,7 @@
#include <systemlib/err.h>
#ifndef CONFIG_HRT_TIMER
# error CONFIG_TONE_ALARM requires CONFIG_HRT_TIMER
# error This driver requires CONFIG_HRT_TIMER
#endif
/* Tone alarm configuration */
@ -611,7 +611,6 @@ play_pattern(unsigned pattern)
if (fd < 0)
err(1, "/dev/tone_alarm");
warnx("playing pattern %u", pattern);
ret = ioctl(fd, TONE_SET_ALARM, pattern);
if (ret != 0)

1
apps/mavlink/orb_listener.c
View File

@ -531,6 +531,7 @@ uorb_receive_thread(void *arg)
struct pollfd fds[n_listeners];
for (unsigned i = 0; i < n_listeners; i++) {
fds[i].fd = *listeners[i].subp;
fds[i].events = POLLIN;
/* Invoke callback to set initial state */
//listeners[i].callback(&listener[i]);

5
apps/systemcmds/reboot/reboot.c
View File

@ -41,14 +41,13 @@
#include <unistd.h>
#include <stdio.h>
#include "systemlib/systemlib.h"
#include <systemlib/systemlib.h>
__EXPORT int reboot_main(int argc, char *argv[]);
int reboot_main(int argc, char *argv[])
{
reboot();
return 0;
up_systemreset();
}

3
apps/systemlib/Makefile
View File

@ -42,7 +42,8 @@ CSRCS = err.c \
bson/tinybson.c \
conversions.c \
cpuload.c \
getopt_long.c
getopt_long.c \
up_cxxinitialize.c
# ppm_decode.c \

35
apps/systemlib/systemlib.c
View File

@ -73,41 +73,6 @@ const struct __multiport_info multiport_info = {
static void kill_task(FAR _TCB *tcb, FAR void *arg);
/****************************************************************************
* user_start
****************************************************************************/
int reboot(void)
{
sched_lock();
// print text
printf("\r\nRebooting system - ending tasks and performing hard reset\r\n\r\n");
fflush(stdout);
//usleep(5000);
/* Sending kill signal to other tasks */
//killall();
/* Waiting maximum time for all to exit */
//usleep(5000);
//sched_lock();
/* Resetting CPU */
// FIXME Need check for ARM architecture here
#ifndef NVIC_AIRCR
#define NVIC_AIRCR (*((uint32_t*)0xE000ED0C))
#endif
/* Set the SYSRESETREQ bit to force a reset */
NVIC_AIRCR = 0x05fa0004;
/* Spinning until the board is really reset */
while (true);
/* Should never reach here */
return 0;
}
void killall()
{
// printf("Sending SIGUSR1 to all processes now\n");

2
apps/systemlib/systemlib.h
View File

@ -45,7 +45,7 @@
__BEGIN_DECLS
/** Reboots the board */
__EXPORT int reboot(void);
extern void up_systemreset(void) noreturn_function;
/** Sends SIGUSR1 to all processes */
__EXPORT void killall(void);

150
apps/systemlib/up_cxxinitialize.c Normal file
View File

@ -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();
}
}
}

9
nuttx/configs/px4fmu/common/ld.script
View File

@ -87,6 +87,15 @@ SECTIONS
__errno = get_errno_ptr;
} > flash
/*
* Init functions (static constructors and the like)
*/
.init_section : {
_sinit = ABSOLUTE(.);
KEEP(*(.init_array .init_array.*))
_einit = ABSOLUTE(.);
} > flash
/*
* Construction data for parameters.
*/

38
nuttx/configs/px4fmu/include/board.h
View File

@ -45,9 +45,6 @@
#ifndef __ASSEMBLY__
# include <stdint.h>
#endif
//#include "stm32_rcc.h"
//#include "stm32_sdio.h"
//#include "stm32_internal.h"
/************************************************************************************
* Definitions
@ -298,7 +295,7 @@
* Note that these are unshifted addresses.
*/
#define PX4_I2C_OBDEV_HMC5883 0x1e
#define PX4_I2C_OBDEV_MS5611 NOTDEFINED
#define PX4_I2C_OBDEV_MS5611 0x76
#define PX4_I2C_OBDEV_EEPROM NOTDEFINED
#define PX4_I2C_OBDEV_PX4IO_BL 0x18
@ -327,11 +324,9 @@
/*
* Tone alarm output
*/
#ifdef CONFIG_TONE_ALARM
#define TONE_ALARM_TIMER 3 /* timer 3 */
#define TONE_ALARM_CHANNEL 3 /* channel 3 */
#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8)
#endif
/************************************************************************************
* Public Data
@ -362,37 +357,6 @@ extern "C" {
EXTERN void stm32_boardinitialize(void);
/************************************************************************************
* Button support.
*
* Description:
* up_buttoninit() must be called to initialize button resources. After
* that, up_buttons() may be called to collect the current state of all
* buttons or up_irqbutton() may be called to register button interrupt
* handlers.
*
* After up_buttoninit() has been called, up_buttons() may be called to
* collect the state of all buttons. up_buttons() returns an 8-bit bit set
* with each bit associated with a button. See the BUTTON_*_BIT
* definitions in board.h for the meaning of each bit.
*
* up_irqbutton() may be called to register an interrupt handler that will
* be called when a button is depressed or released. The ID value is a
* button enumeration value that uniquely identifies a button resource. See the
* BUTTON_* definitions in board.h for the meaning of enumeration
* value. The previous interrupt handler address is returned (so that it may
* restored, if so desired).
*
************************************************************************************/
#ifdef CONFIG_ARCH_BUTTONS
EXTERN void up_buttoninit(void);
EXTERN uint8_t up_buttons(void);
#ifdef CONFIG_ARCH_IRQBUTTONS
EXTERN xcpt_t up_irqbutton(int id, xcpt_t irqhandler);
#endif
#endif
#undef EXTERN
#if defined(__cplusplus)
}

107
nuttx/configs/px4fmu/include/drv_gpio.h
View File

@ -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 */

2
nuttx/configs/px4fmu/nsh/appconfig
View File

@ -97,7 +97,7 @@ CONFIGURED_APPS += drivers/l3gd20
CONFIGURED_APPS += drivers/px4io
CONFIGURED_APPS += drivers/stm32
CONFIGURED_APPS += drivers/stm32/tone_alarm
CONFIGURED_APPS += px4/fmu
CONFIGURED_APPS += drivers/px4fmu
# Testing stuff
CONFIGURED_APPS += px4/sensors_bringup

4
nuttx/configs/px4fmu/nsh/defconfig
View File

@ -179,7 +179,7 @@ CONFIG_STM32_I2C1=y
CONFIG_STM32_I2C2=y
CONFIG_STM32_I2C3=y
CONFIG_STM32_CAN1=n
CONFIG_STM32_CAN2=y
CONFIG_STM32_CAN2=n
CONFIG_STM32_DAC=n
CONFIG_STM32_PWR=y
# APB2:
@ -542,7 +542,7 @@ CONFIG_DEBUG_I2C=n
CONFIG_DEBUG_INPUT=n
CONFIG_HAVE_CXX=y
CONFIG_HAVE_CXXINITIALIZE=n
CONFIG_HAVE_CXXINITIALIZE=y
CONFIG_MM_REGIONS=2
CONFIG_ARCH_LOWPUTC=y
CONFIG_MSEC_PER_TICK=1

1
nuttx/configs/px4fmu/src/Makefile
View File

@ -41,7 +41,6 @@ ASRCS =
AOBJS = $(ASRCS:.S=$(OBJEXT))
CSRCS = up_leds.c \
drv_gpio.c \
drv_led.c drv_eeprom.c
COBJS = $(CSRCS:.c=$(OBJEXT))

195
nuttx/configs/px4fmu/src/drv_gpio.c
View File

@ -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;
}