fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Move the tone_alarm driver out of the NuttX configs area and add it as an stm32-specific driver in the PX4 apps space.

Browse Source 
Add a new tone_alarm command that can be used to start/stop alarm tones from the shell.
This commit is contained in:
px4dev 2012-10-21 17:51:21 -07:00
parent 73521cbc66
commit 5925d146bc
9 changed files with 296 additions and 137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ROMFS/scripts/rcS
View File

@ -20,6 +20,11 @@
set MODE autostart
set USB autoconnect
#
# Start playing the startup tune
#
tone_alarm start
#
# Try to mount the microSD card.
#
@ -31,9 +36,6 @@ else
echo "[init] no microSD card found"
fi
usleep 500
#
# Look for an init script on the microSD card.
#

2
apps/commander/commander.c
View File

@ -57,8 +57,8 @@
#include <string.h>
#include <arch/board/drv_led.h>
#include <arch/board/up_hrt.h>
#include <arch/board/drv_tone_alarm.h>
#include <arch/board/up_hrt.h>
#include <drivers/drv_tone_alarm.h>
#include "state_machine_helper.h"
#include "systemlib/systemlib.h"
#include <math.h>

4
nuttx/configs/px4fmu/include/drv_tone_alarm.h → apps/drivers/drv_tone_alarm.h
View File

@ -63,8 +63,6 @@
#define _TONE_ALARM_BASE 0x7400
#define TONE_SET_ALARM _IOC(_TONE_ALARM_BASE, 1)
extern int tone_alarm_init(void);
/* structure describing one note in a tone pattern */
struct tone_note {
uint8_t pitch;
@ -127,4 +125,4 @@ enum tone_pitch {
TONE_NOTE_MAX
};
#endif /* DRV_TONE_ALARM_H_ */
#endif /* DRV_TONE_ALARM_H_ */

43
apps/drivers/stm32/tone_alarm/Makefile Normal file
View File

@ -0,0 +1,43 @@
############################################################################
#
# 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.
#
############################################################################
#
# Tone alarm driver
#
APPNAME = tone_alarm
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
include $(APPDIR)/mk/app.mk

365
nuttx/configs/px4fmu/src/drv_tone_alarm.c → apps/drivers/stm32/tone_alarm/tone_alarm.cpp
View File

@ -31,46 +31,61 @@
*
****************************************************************************/
/*
* Speaker driver supporting alarm sequences.
/**
* Driver for the PX4 audio alarm port, /dev/tone_alarm.
*
* Works with any of the 'generic' STM32 timers that has an output
* pin, does not require an interrupt.
* The tone_alarm driver supports a set of predefined "alarm"
* patterns and one user-supplied pattern. Patterns are ordered by
* priority, with a higher-priority pattern interrupting any
* lower-priority pattern that might be playing.
*
* Depends on the HRT timer.
* The TONE_SET_ALARM ioctl can be used to select a predefined
* alarm pattern, from 1 - <TBD>. Selecting pattern zero silences
* the alarm.
*
* To supply a custom pattern, write an array of 1 - <TBD> tone_note
* structures to /dev/tone_alarm. The custom pattern has a priority
* of zero.
*
* Patterns will normally play once and then silence (if a pattern
* was overridden it will not resume). A pattern may be made to
* repeat by inserting a note with the duration set to
* DURATION_REPEAT. This pattern will loop until either a
* higher-priority pattern is started or pattern zero is requested
* via the ioctl.
*/
#include <nuttx/config.h>
#include <nuttx/arch.h>
#include <nuttx/irq.h>
#include <drivers/device/device.h>
#include <drivers/drv_tone_alarm.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <assert.h>
#include <debug.h>
#include <time.h>
#include <queue.h>
#include <errno.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <stdio.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <arch/board/drv_tone_alarm.h>
#include <arch/board/up_hrt.h>
#include "chip.h"
#include "up_internal.h"
#include "up_arch.h"
#include <arch/stm32/chip.h>
#include <up_internal.h>
#include <up_arch.h>
#include "stm32_internal.h"
#include "stm32_gpio.h"
#include "stm32_tim.h"
#include <stm32_internal.h>
#include <stm32_gpio.h>
#include <stm32_tim.h>
#ifdef CONFIG_TONE_ALARM
# ifndef CONFIG_HRT_TIMER
# error CONFIG_TONE_ALARM requires CONFIG_HRT_TIMER
# endif
#include <systemlib/err.h>
#ifndef CONFIG_HRT_TIMER
# error CONFIG_TONE_ALARM requires CONFIG_HRT_TIMER
#endif
/* Tone alarm configuration */
#if TONE_ALARM_TIMER == 2
@ -123,7 +138,7 @@
# error Must not set CONFIG_STM32_TIM11 when TONE_ALARM_TIMER is 11
# endif
#else
# error Must set TONE_ALARM_TIMER to a generic timer if CONFIG_TONE_ALARM is set
# error Must set TONE_ALARM_TIMER to a generic timer in order to use this driver.
#endif
#if TONE_ALARM_CHANNEL == 1
@ -147,9 +162,10 @@
# define TONE_CCER (1 << 12)
# define TONE_rCCR rCCR4
#else
# error Must set TONE_ALARM_CHANNEL to a value between 1 and 4
# error Must set TONE_ALARM_CHANNEL to a value between 1 and 4 to use this driver.
#endif
/*
* Timer register accessors
*/
@ -174,11 +190,42 @@
#define rDCR REG(STM32_GTIM_DCR_OFFSET)
#define rDMAR REG(STM32_GTIM_DMAR_OFFSET)
#define TONE_MAX_PATTERN 6
#define TONE_MAX_PATTERN_LEN 40
class ToneAlarm : public device::CDev
{
public:
ToneAlarm();
~ToneAlarm();
/* predefined patterns for alarms 1-TONE_MAX_PATTERN */
const struct tone_note patterns[TONE_MAX_PATTERN][TONE_MAX_PATTERN_LEN] = {
virtual int init();
virtual int ioctl(file *filp, int cmd, unsigned long arg);
virtual int write(file *filp, const char *buffer, size_t len);
private:
static const unsigned _max_pattern = 6;
static const unsigned _pattern_none = _max_pattern + 1;
static const unsigned _pattern_user = 0;
static const unsigned _max_pattern_len = 40;
static const unsigned _note_max = TONE_NOTE_MAX;
static const tone_note _patterns[_max_pattern][_max_pattern_len];
static const uint16_t _notes[_note_max];
unsigned _current_pattern;
unsigned _next_note;
hrt_call _note_end;
tone_note _user_pattern[_max_pattern_len];
static void next_trampoline(void *arg);
void next();
bool check(tone_note *pattern);
void stop();
};
/* predefined patterns for alarms 1-_max_pattern */
const tone_note ToneAlarm::_patterns[_max_pattern][_max_pattern_len] = {
{
{TONE_NOTE_A7, 12},
{TONE_NOTE_D8, 12},
@ -193,7 +240,7 @@ const struct tone_note patterns[TONE_MAX_PATTERN][TONE_MAX_PATTERN_LEN] = {
{TONE_NOTE_D8, 4},
{TONE_NOTE_C8, 4},
},
{{TONE_NOTE_B6, 100}},
{{TONE_NOTE_B6, 100}, {TONE_NOTE_B6, DURATION_REPEAT}},
{{TONE_NOTE_C7, 100}},
{{TONE_NOTE_D7, 100}},
{{TONE_NOTE_E7, 100}},
@ -217,7 +264,6 @@ const struct tone_note patterns[TONE_MAX_PATTERN][TONE_MAX_PATTERN_LEN] = {
{TONE_NOTE_G5S, 40},
{TONE_NOTE_F5, 40},
{TONE_NOTE_F5, 60},
{TONE_NOTE_A5S, 40},
{TONE_NOTE_C6S, 20},
{TONE_NOTE_F6, 40},
@ -239,7 +285,7 @@ const struct tone_note patterns[TONE_MAX_PATTERN][TONE_MAX_PATTERN_LEN] = {
}
};
static uint16_t notes[TONE_NOTE_MAX] = {
const uint16_t ToneAlarm::_notes[_note_max] = {
63707, /* E4 */
60132, /* F4 */
56758, /* F#4/Gb4 */
@ -290,46 +336,46 @@ static uint16_t notes[TONE_NOTE_MAX] = {
4218 /* D#8/Eb8 */
};
/* current state of the tone driver */
struct state {
int pattern; /* current pattern */
#define PATTERN_NONE -1
#define PATTERN_USER 0
int note; /* next note to play */
struct hrt_call note_end;
struct tone_note user_pattern[TONE_MAX_PATTERN_LEN]; /* user-supplied pattern (plays at pattern 0) */
};
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]);
static struct state tone_state;
static int tone_write(struct file *filp, const char *buffer, size_t len);
static int tone_ioctl(struct file *filep, int cmd, unsigned long arg);
static const struct file_operations tone_fops = {
.write = tone_write,
.ioctl = tone_ioctl
};
static void tone_next(void);
static bool tone_ok(struct tone_note *pattern);
int
tone_alarm_init(void)
ToneAlarm::ToneAlarm() :
CDev("tone_alarm", "/dev/tone_alarm"),
_current_pattern(_pattern_none),
_next_note(0)
{
/* configure the GPIO */
// enable debug() calls
//_debug_enabled = true;
}
ToneAlarm::~ToneAlarm()
{
}
int
ToneAlarm::init()
{
int ret;
ret = CDev::init();
if (ret != OK)
return ret;
/* configure the GPIO to the idle state */
stm32_configgpio(GPIO_TONE_ALARM);
/* clock/power on our timer */
modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE);
/* default the state */
tone_state.pattern = -1;
/* initialise the timer */
rCR1 = 0;
rCR2 = 0;
rSMCR = 0;
rDIER = 0;
rCCER = 0; /* unlock CCMR* registers */
rCCER &= TONE_CCER; /* unlock CCMR* registers */
rCCMR1 = TONE_CCMR1;
rCCMR2 = TONE_CCMR2;
rCCER = TONE_CCER;
@ -346,153 +392,163 @@ tone_alarm_init(void)
*/
rPSC = 1;
tone_state.pattern = PATTERN_NONE;
tone_state.note = 0;
/* make sure the timer is running */
rCR1 = GTIM_CR1_CEN;
/* play the startup tune */
tone_ioctl(NULL, TONE_SET_ALARM, 1);
/* register the device */
return register_driver("/dev/tone_alarm", &tone_fops, 0666, NULL);
debug("ready");
return OK;
}
static int
tone_ioctl(struct file *filep, int cmd, unsigned long arg)
int
ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
{
int result = 0;
int new = (int)arg;
unsigned new_pattern = arg;
irqstate_t flags = irqsave();
debug("ioctl %i %u", cmd, new_pattern);
// irqstate_t flags = irqsave();
/* decide whether to increase the alarm level to cmd or leave it alone */
switch (cmd) {
case TONE_SET_ALARM:
if (new == 0) {
debug("TONE_SET_ALARM %u", arg);
if (new_pattern == 0) {
/* cancel any current alarm */
tone_state.pattern = PATTERN_NONE;
tone_next();
_current_pattern = _pattern_none;
next();
} else if (new > TONE_MAX_PATTERN) {
} else if (new_pattern > _max_pattern) {
/* not a legal alarm value */
result = -ERANGE;
} else if (new > tone_state.pattern) {
} else if ((_current_pattern == _pattern_none) || (new_pattern > _current_pattern)) {
/* higher priority than the current alarm */
tone_state.pattern = new;
tone_state.note = 0;
_current_pattern = new_pattern;
_next_note = 0;
/* and start playing it */
tone_next();
next();
} else {
/* current pattern is higher priority than the new pattern, ignore */
}
break;
default:
result = -EINVAL;
result = -ENOTTY;
break;
}
irqrestore(flags);
// irqrestore(flags);
/* give it to the superclass if we didn't like it */
if (result == -ENOTTY)
result = CDev::ioctl(filp, cmd, arg);
return result;
}
static int
tone_write(struct file *filp, const char *buffer, size_t len)
int
ToneAlarm::write(file *filp, const char *buffer, size_t len)
{
irqstate_t flags;
/* sanity-check the size of the write */
if (len > (TONE_MAX_PATTERN_LEN * sizeof(struct tone_note)))
if (len > (_max_pattern_len * sizeof(tone_note)))
return -EFBIG;
if ((len % sizeof(struct tone_note)) || (len == 0))
if ((len % sizeof(tone_note)) || (len == 0))
return -EIO;
if (!tone_ok((struct tone_note *)buffer))
if (!check((tone_note *)buffer))
return -EIO;
flags = irqsave();
/* if we aren't playing an alarm tone */
if (tone_state.pattern <= PATTERN_USER) {
if (_current_pattern <= _pattern_user) {
/* reset the tone state to play the new user pattern */
tone_state.pattern = PATTERN_USER;
tone_state.note = 0;
_current_pattern = _pattern_user;
_next_note = 0;
/* copy in the new pattern */
memset(tone_state.user_pattern, 0, sizeof(tone_state.user_pattern));
memcpy(tone_state.user_pattern, buffer, len);
memset(_user_pattern, 0, sizeof(_user_pattern));
memcpy(_user_pattern, buffer, len);
/* and start it */
tone_next();
debug("starting user pattern");
next();
}
irqrestore(flags);
return len;
}
static void
tone_next(void)
void
ToneAlarm::next_trampoline(void *arg)
{
const struct tone_note *np;
ToneAlarm *ta = (ToneAlarm *)arg;
ta->next();
}
void
ToneAlarm::next(void)
{
const tone_note *np;
/* stop the current note */
rCR1 = 0;
rCCER &= ~TONE_CCER;
/* if we are no longer playing a pattern, we have nothing else to do here */
if (tone_state.pattern == PATTERN_NONE) {
if (_current_pattern == _pattern_none) {
stop();
return;
}
/* if the current pattern has ended, clear the pattern and stop */
if (tone_state.note == TONE_NOTE_MAX) {
tone_state.pattern = PATTERN_NONE;
return;
}
ASSERT(_next_note < _note_max);
/* find the note to play */
if (tone_state.pattern == PATTERN_USER) {
np = &tone_state.user_pattern[tone_state.note];
if (_current_pattern == _pattern_user) {
np = &_user_pattern[_next_note];
} else {
np = &patterns[tone_state.pattern - 1][tone_state.note];
np = &_patterns[_current_pattern - 1][_next_note];
}
/* work out which note is next */
tone_state.note++;
if (tone_state.note >= TONE_NOTE_MAX) {
_next_note++;
if (_next_note >= _note_max) {
/* hit the end of the pattern, stop */
tone_state.pattern = PATTERN_NONE;
_current_pattern = _pattern_none;
} else if (np[1].duration == DURATION_END) {
/* hit the end of the pattern, stop */
tone_state.pattern = PATTERN_NONE;
_current_pattern = _pattern_none;
} else if (np[1].duration == DURATION_REPEAT) {
/* next note is a repeat, rewind in preparation */
tone_state.note = 0;
_next_note = 0;
}
/* set the timer to play the note, if required */
if (np->pitch <= TONE_NOTE_SILENCE) {
/* set reload based on the pitch */
rARR = notes[np->pitch];
rARR = _notes[np->pitch];
/* force an update, reloads the counter and all registers */
rEGR = GTIM_EGR_UG;
/* start the timer */
rCR1 = GTIM_CR1_CEN;
/* enable the output */
rCCER |= TONE_CCER;
}
/* arrange a callback when the note/rest is done */
hrt_call_after(&tone_state.note_end, (hrt_abstime)10000 * np->duration, (hrt_callout)tone_next, NULL);
hrt_call_after(&_note_end, (hrt_abstime)10000 * np->duration, (hrt_callout)next_trampoline, this);
}
static bool
tone_ok(struct tone_note *pattern)
bool
ToneAlarm::check(tone_note *pattern)
{
int i;
for (i = 0; i < TONE_NOTE_MAX; i++) {
for (unsigned i = 0; i < _note_max; i++) {
/* first note must not be repeat or end */
if ((i == 0) &&
@ -502,10 +558,75 @@ tone_ok(struct tone_note *pattern)
break;
/* pitch must be legal */
if (pattern[i].pitch >= TONE_NOTE_MAX)
if (pattern[i].pitch >= _note_max)
return false;
}
return true;
}
#endif /* CONFIG_TONE_ALARM */
void
ToneAlarm::stop()
{
/* stop the current note */
rCCER &= ~TONE_CCER;
/*
* Make sure the GPIO is not driving the speaker.
*
* XXX this presumes PX4FMU and the onboard speaker driver FET.
*/
stm32_gpiowrite(GPIO_TONE_ALARM, 0);
}
/**
* Local functions in support of the shell command.
*/
namespace
{
ToneAlarm *g_dev;
int
play_pattern(unsigned pattern)
{
int fd, ret;
fd = open("/dev/tone_alarm", 0);
if (fd < 0)
err(1, "/dev/tone_alarm");
warnx("playing pattern %u", pattern);
ret = ioctl(fd, TONE_SET_ALARM, pattern);
if (ret != 0)
err(1, "TONE_SET_ALARM");
exit(0);
}
} // namespace
int
tone_alarm_main(int argc, char *argv[])
{
unsigned pattern;
/* start the driver lazily */
if (g_dev == nullptr) {
g_dev = new ToneAlarm;
if (g_dev == nullptr)
errx(1, "couldn't allocate the ToneAlarm driver");
if (g_dev->init() != OK) {
delete g_dev;
errx(1, "ToneAlarm init failed");
}
}
if ((argc > 1) && !strcmp(argv[1], "start"))
play_pattern(1);
if ((argc > 1) && !strcmp(argv[1], "stop"))
play_pattern(0);
if ((pattern = strtol(argv[1], nullptr, 10)) != 0)
play_pattern(pattern);
errx(1, "unrecognised command, try 'start', 'stop' or an alarm number");
}

2
apps/px4/tests/test_hrt.c
View File

@ -51,7 +51,7 @@
#include <arch/board/board.h>
#include <arch/board/up_hrt.h>
#include <arch/board/drv_tone_alarm.h>
#include <drivers/drv_tone_alarm.h>
#include <nuttx/spi.h>

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

@ -93,6 +93,7 @@ CONFIGURED_APPS += drivers/hmc5883
CONFIGURED_APPS += drivers/mpu6000
CONFIGURED_APPS += drivers/bma180
CONFIGURED_APPS += drivers/l3gd20
CONFIGURED_APPS += drivers/stm32/tone_alarm
CONFIGURED_APPS += px4/px4io/driver
CONFIGURED_APPS += px4/fmu

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

@ -43,7 +43,7 @@ AOBJS = $(ASRCS:.S=$(OBJEXT))
CSRCS = up_boot.c up_leds.c up_spi.c up_hrt.c \
drv_gpio.c \
drv_led.c drv_eeprom.c \
drv_tone_alarm.c up_pwm_servo.c up_usbdev.c \
up_pwm_servo.c up_usbdev.c \
up_cpuload.c
ifeq ($(CONFIG_NSH_ARCHINIT),y)

6
nuttx/configs/px4fmu/src/up_nsh.c
View File

@ -58,7 +58,6 @@
#include <arch/board/up_hrt.h>
#include <arch/board/up_cpuload.h>
#include <arch/board/drv_tone_alarm.h>
#include <arch/board/up_adc.h>
#include <arch/board/board.h>
#include <arch/board/drv_led.h>
@ -277,10 +276,5 @@ int nsh_archinitialize(void)
}
#endif
/* configure the tone generator */
#ifdef CONFIG_TONE_ALARM
tone_alarm_init();
#endif
return OK;
}