fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 06:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:PX4/Firmware into calibration

Browse Source
This commit is contained in:
Lorenz Meier
2012-10-22 12:14:40 +02:00
parent a3f2114d54 5b9c469770
commit 3932bad137
10 changed files with 296 additions and 176 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+1 -1
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>
apps/drivers/drv_tone_alarm.h
+128
View File
@@ -0,0 +1,128 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/*
* Driver for the PX4 audio alarm port, /dev/tone_alarm.
*
* 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.
*
* 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.
*/
#ifndef DRV_TONE_ALARM_H_
#define DRV_TONE_ALARM_H_
#include <sys/ioctl.h>
#define _TONE_ALARM_BASE 0x7400
#define TONE_SET_ALARM _IOC(_TONE_ALARM_BASE, 1)
/* structure describing one note in a tone pattern */
struct tone_note {
uint8_t pitch;
uint8_t duration; /* duration in multiples of 10ms */
#define DURATION_END 0 /* ends the pattern */
#define DURATION_REPEAT 255 /* resets the note counter to zero */
};
enum tone_pitch {
TONE_NOTE_E4, /* E4 */
TONE_NOTE_F4, /* F4 */
TONE_NOTE_F4S, /* F#4/Gb4 */
TONE_NOTE_G4, /* G4 */
TONE_NOTE_G4S, /* G#4/Ab4 */
TONE_NOTE_A4, /* A4 */
TONE_NOTE_A4S, /* A#4/Bb4 */
TONE_NOTE_B4, /* B4 */
TONE_NOTE_C5, /* C5 */
TONE_NOTE_C5S, /* C#5/Db5 */
TONE_NOTE_D5, /* D5 */
TONE_NOTE_D5S, /* D#5/Eb5 */
TONE_NOTE_E5, /* E5 */
TONE_NOTE_F5, /* F5 */
TONE_NOTE_F5S, /* F#5/Gb5 */
TONE_NOTE_G5, /* G5 */
TONE_NOTE_G5S, /* G#5/Ab5 */
TONE_NOTE_A5, /* A5 */
TONE_NOTE_A5S, /* A#5/Bb5 */
TONE_NOTE_B5, /* B5 */
TONE_NOTE_C6, /* C6 */
TONE_NOTE_C6S, /* C#6/Db6 */
TONE_NOTE_D6, /* D6 */
TONE_NOTE_D6S, /* D#6/Eb6 */
TONE_NOTE_E6, /* E6 */
TONE_NOTE_F6, /* F6 */
TONE_NOTE_F6S, /* F#6/Gb6 */
TONE_NOTE_G6, /* G6 */
TONE_NOTE_G6S, /* G#6/Ab6 */
TONE_NOTE_A6, /* A6 */
TONE_NOTE_A6S, /* A#6/Bb6 */
TONE_NOTE_B6, /* B6 */
TONE_NOTE_C7, /* C7 */
TONE_NOTE_C7S, /* C#7/Db7 */
TONE_NOTE_D7, /* D7 */
TONE_NOTE_D7S, /* D#7/Eb7 */
TONE_NOTE_E7, /* E7 */
TONE_NOTE_F7, /* F7 */
TONE_NOTE_F7S, /* F#7/Gb7 */
TONE_NOTE_G7, /* G7 */
TONE_NOTE_G7S, /* G#7/Ab7 */
TONE_NOTE_A7, /* A7 */
TONE_NOTE_A7S, /* A#7/Bb7 */
TONE_NOTE_B7, /* B7 */
TONE_NOTE_C8, /* C8 */
TONE_NOTE_C8S, /* C#8/Db8 */
TONE_NOTE_D8, /* D8 */
TONE_NOTE_D8S, /* D#8/Eb8 */
TONE_NOTE_SILENCE,
TONE_NOTE_MAX
};
#endif /* DRV_TONE_ALARM_H_ */
apps/drivers/stm32/tone_alarm/Makefile
+43
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
apps/drivers/stm32/tone_alarm/tone_alarm.cpp
+632
View File
@@ -0,0 +1,632 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* Driver for the PX4 audio alarm port, /dev/tone_alarm.
*
* 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.
*
* 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 <drivers/device/device.h>
#include <drivers/drv_tone_alarm.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.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/up_hrt.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 <systemlib/err.h>
#ifndef CONFIG_HRT_TIMER
# error CONFIG_TONE_ALARM requires CONFIG_HRT_TIMER
#endif
/* Tone alarm configuration */
#if TONE_ALARM_TIMER == 2
# define TONE_ALARM_BASE STM32_TIM2_BASE
# define TONE_ALARM_CLOCK STM32_APB1_TIM2_CLKIN
# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM2EN
# ifdef CONFIG_STM32_TIM2
# error Must not set CONFIG_STM32_TIM2 when TONE_ALARM_TIMER is 2
# endif
#elif TONE_ALARM_TIMER == 3
# define TONE_ALARM_BASE STM32_TIM3_BASE
# define TONE_ALARM_CLOCK STM32_APB1_TIM3_CLKIN
# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM3EN
# ifdef CONFIG_STM32_TIM3
# error Must not set CONFIG_STM32_TIM3 when TONE_ALARM_TIMER is 3
# endif
#elif TONE_ALARM_TIMER == 4
# define TONE_ALARM_BASE STM32_TIM4_BASE
# define TONE_ALARM_CLOCK STM32_APB1_TIM4_CLKIN
# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM4EN
# ifdef CONFIG_STM32_TIM4
# error Must not set CONFIG_STM32_TIM4 when TONE_ALARM_TIMER is 4
# endif
#elif TONE_ALARM_TIMER == 5
# define TONE_ALARM_BASE STM32_TIM5_BASE
# define TONE_ALARM_CLOCK STM32_APB1_TIM5_CLKIN
# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM5EN
# ifdef CONFIG_STM32_TIM5
# error Must not set CONFIG_STM32_TIM5 when TONE_ALARM_TIMER is 5
# endif
#elif TONE_ALARM_TIMER == 9
# define TONE_ALARM_BASE STM32_TIM9_BASE
# define TONE_ALARM_CLOCK STM32_APB1_TIM9_CLKIN
# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM9EN
# ifdef CONFIG_STM32_TIM9
# error Must not set CONFIG_STM32_TIM9 when TONE_ALARM_TIMER is 9
# endif
#elif TONE_ALARM_TIMER == 10
# define TONE_ALARM_BASE STM32_TIM10_BASE
# define TONE_ALARM_CLOCK STM32_APB1_TIM10_CLKIN
# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM10EN
# ifdef CONFIG_STM32_TIM10
# error Must not set CONFIG_STM32_TIM10 when TONE_ALARM_TIMER is 10
# endif
#elif TONE_ALARM_TIMER == 11
# define TONE_ALARM_BASE STM32_TIM11_BASE
# define TONE_ALARM_CLOCK STM32_APB1_TIM11_CLKIN
# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM11EN
# ifdef CONFIG_STM32_TIM11
# 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 in order to use this driver.
#endif
#if TONE_ALARM_CHANNEL == 1
# define TONE_CCMR1 (3 << 4)
# define TONE_CCMR2 0
# define TONE_CCER (1 << 0)
# define TONE_rCCR rCCR1
#elif TONE_ALARM_CHANNEL == 2
# define TONE_CCMR1 (3 << 12)
# define TONE_CCMR2 0
# define TONE_CCER (1 << 4)
# define TONE_rCCR rCCR2
#elif TONE_ALARM_CHANNEL == 3
# define TONE_CCMR1 0
# define TONE_CCMR2 (3 << 4)
# define TONE_CCER (1 << 8)
# define TONE_rCCR rCCR3
#elif TONE_ALARM_CHANNEL == 4
# define TONE_CCMR1 0
# define TONE_CCMR2 (3 << 12)
# define TONE_CCER (1 << 12)
# define TONE_rCCR rCCR4
#else
# error Must set TONE_ALARM_CHANNEL to a value between 1 and 4 to use this driver.
#endif
/*
* Timer register accessors
*/
#define REG(_reg) (*(volatile uint32_t *)(TONE_ALARM_BASE + _reg))
#define rCR1 REG(STM32_GTIM_CR1_OFFSET)
#define rCR2 REG(STM32_GTIM_CR2_OFFSET)
#define rSMCR REG(STM32_GTIM_SMCR_OFFSET)
#define rDIER REG(STM32_GTIM_DIER_OFFSET)
#define rSR REG(STM32_GTIM_SR_OFFSET)
#define rEGR REG(STM32_GTIM_EGR_OFFSET)
#define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET)
#define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET)
#define rCCER REG(STM32_GTIM_CCER_OFFSET)
#define rCNT REG(STM32_GTIM_CNT_OFFSET)
#define rPSC REG(STM32_GTIM_PSC_OFFSET)
#define rARR REG(STM32_GTIM_ARR_OFFSET)
#define rCCR1 REG(STM32_GTIM_CCR1_OFFSET)
#define rCCR2 REG(STM32_GTIM_CCR2_OFFSET)
#define rCCR3 REG(STM32_GTIM_CCR3_OFFSET)
#define rCCR4 REG(STM32_GTIM_CCR4_OFFSET)
#define rDCR REG(STM32_GTIM_DCR_OFFSET)
#define rDMAR REG(STM32_GTIM_DMAR_OFFSET)
class ToneAlarm : public device::CDev
{
public:
ToneAlarm();
~ToneAlarm();
virtual int init();
virtual int ioctl(file *filp, int cmd, unsigned long arg);
virtual ssize_t 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},
{TONE_NOTE_C8, 12},
{TONE_NOTE_A7, 12},
{TONE_NOTE_D8, 12},
{TONE_NOTE_C8, 12},
{TONE_NOTE_D8, 4},
{TONE_NOTE_C8, 4},
{TONE_NOTE_D8, 4},
{TONE_NOTE_C8, 4},
{TONE_NOTE_D8, 4},
{TONE_NOTE_C8, 4},
},
{{TONE_NOTE_B6, 100}, {TONE_NOTE_B6, DURATION_REPEAT}},
{{TONE_NOTE_C7, 100}},
{{TONE_NOTE_D7, 100}},
{{TONE_NOTE_E7, 100}},
{ //This is tetris ;)
{TONE_NOTE_C6, 40},
{TONE_NOTE_G5, 20},
{TONE_NOTE_G5S, 20},
{TONE_NOTE_A5S, 40},
{TONE_NOTE_G5S, 20},
{TONE_NOTE_G5, 20},
{TONE_NOTE_F5, 40},
{TONE_NOTE_F5, 20},
{TONE_NOTE_G5S, 20},
{TONE_NOTE_C6, 40},
{TONE_NOTE_A5S, 20},
{TONE_NOTE_G5S, 20},
{TONE_NOTE_G5, 60},
{TONE_NOTE_G5S, 20},
{TONE_NOTE_A5S, 40},
{TONE_NOTE_C6, 40},
{TONE_NOTE_G5S, 40},
{TONE_NOTE_F5, 40},
{TONE_NOTE_F5, 60},
{TONE_NOTE_A5S, 40},
{TONE_NOTE_C6S, 20},
{TONE_NOTE_F6, 40},
{TONE_NOTE_D6S, 20},
{TONE_NOTE_C6S, 20},
{TONE_NOTE_C6, 60},
{TONE_NOTE_G5S, 20},
{TONE_NOTE_C6, 40},
{TONE_NOTE_A5S, 20},
{TONE_NOTE_G5S, 20},
{TONE_NOTE_G5, 40},
{TONE_NOTE_G5, 20},
{TONE_NOTE_G5S, 20},
{TONE_NOTE_A5S, 40},
{TONE_NOTE_C6, 40},
{TONE_NOTE_G5S, 40},
{TONE_NOTE_F5, 40},
{TONE_NOTE_F5, 60},
}
};
const uint16_t ToneAlarm::_notes[_note_max] = {
63707, /* E4 */
60132, /* F4 */
56758, /* F#4/Gb4 */
53571, /* G4 */
50565, /* G#4/Ab4 */
47727, /* A4 */
45048, /* A#4/Bb4 */
42520, /* B4 */
40133, /* C5 */
37880, /* C#5/Db5 */
35755, /* D5 */
33748, /* D#5/Eb5 */
31853, /* E5 */
30066, /* F5 */
28378, /* F#5/Gb5 */
26786, /* G5 */
25282, /* G#5/Ab5 */
23863, /* A5 */
22524, /* A#5/Bb5 */
21260, /* B5 */
20066, /* C6 */
18940, /* C#6/Db6 */
17877, /* D6 */
16874, /* D#6/Eb6 */
15927, /* E6 */
15033, /* F6 */
14189, /* F#6/Gb6 */
13393, /* G6 */
12641, /* G#6/Ab6 */
11931, /* A6 */
11262, /* A#6/Bb6 */
10630, /* B6 */
10033, /* C7 */
9470, /* C#7/Db7 */
8938, /* D7 */
8437, /* D#7/Eb7 */
7963, /* E7 */
7516, /* F7 */
7094, /* F#7/Gb7 */
6696, /* G7 */
6320, /* G#7/Ab7 */
5965, /* A7 */
5631, /* A#7/Bb7 */
5315, /* B7 */
5016, /* C8 */
4735, /* C#8/Db8 */
4469, /* D8 */
4218 /* D#8/Eb8 */
};
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]);
ToneAlarm::ToneAlarm() :
CDev("tone_alarm", "/dev/tone_alarm"),
_current_pattern(_pattern_none),
_next_note(0)
{
// 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);
/* initialise the timer */
rCR1 = 0;
rCR2 = 0;
rSMCR = 0;
rDIER = 0;
rCCER &= TONE_CCER; /* unlock CCMR* registers */
rCCMR1 = TONE_CCMR1;
rCCMR2 = TONE_CCMR2;
rCCER = TONE_CCER;
rDCR = 0;
/* toggle the CC output each time the count passes 1 */
TONE_rCCR = 1;
/*
* Configure the timebase to free-run at half max frequency.
* XXX this should be more flexible in order to get a better
* frequency range, but for the F4 with the APB1 timers based
* at 42MHz, this gets us down to ~320Hz or so.
*/
rPSC = 1;
/* make sure the timer is running */
rCR1 = GTIM_CR1_CEN;
debug("ready");
return OK;
}
int
ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
{
int result = 0;
unsigned new_pattern = arg;
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:
debug("TONE_SET_ALARM %u", arg);
if (new_pattern == 0) {
/* cancel any current alarm */
_current_pattern = _pattern_none;
next();
} else if (new_pattern > _max_pattern) {
/* not a legal alarm value */
result = -ERANGE;
} else if ((_current_pattern == _pattern_none) || (new_pattern > _current_pattern)) {
/* higher priority than the current alarm */
_current_pattern = new_pattern;
_next_note = 0;
/* and start playing it */
next();
} else {
/* current pattern is higher priority than the new pattern, ignore */
}
break;
default:
result = -ENOTTY;
break;
}
// irqrestore(flags);
/* give it to the superclass if we didn't like it */
if (result == -ENOTTY)
result = CDev::ioctl(filp, cmd, arg);
return result;
}
int
ToneAlarm::write(file *filp, const char *buffer, size_t len)
{
irqstate_t flags;
/* sanity-check the size of the write */
if (len > (_max_pattern_len * sizeof(tone_note)))
return -EFBIG;
if ((len % sizeof(tone_note)) || (len == 0))
return -EIO;
if (!check((tone_note *)buffer))
return -EIO;
flags = irqsave();
/* if we aren't playing an alarm tone */
if (_current_pattern <= _pattern_user) {
/* reset the tone state to play the new user pattern */
_current_pattern = _pattern_user;
_next_note = 0;
/* copy in the new pattern */
memset(_user_pattern, 0, sizeof(_user_pattern));
memcpy(_user_pattern, buffer, len);
/* and start it */
debug("starting user pattern");
next();
}
irqrestore(flags);
return len;
}
void
ToneAlarm::next_trampoline(void *arg)
{
ToneAlarm *ta = (ToneAlarm *)arg;
ta->next();
}
void
ToneAlarm::next(void)
{
const tone_note *np;
/* stop the current note */
rCCER &= ~TONE_CCER;
/* if we are no longer playing a pattern, we have nothing else to do here */
if (_current_pattern == _pattern_none) {
stop();
return;
}
ASSERT(_next_note < _note_max);
/* find the note to play */
if (_current_pattern == _pattern_user) {
np = &_user_pattern[_next_note];
} else {
np = &_patterns[_current_pattern - 1][_next_note];
}
/* work out which note is next */
_next_note++;
if (_next_note >= _note_max) {
/* hit the end of the pattern, stop */
_current_pattern = _pattern_none;
} else if (np[1].duration == DURATION_END) {
/* hit the end of the pattern, stop */
_current_pattern = _pattern_none;
} else if (np[1].duration == DURATION_REPEAT) {
/* next note is a repeat, rewind in preparation */
_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];
/* force an update, reloads the counter and all registers */
rEGR = GTIM_EGR_UG;
/* enable the output */
rCCER |= TONE_CCER;
}
/* arrange a callback when the note/rest is done */
hrt_call_after(&_note_end, (hrt_abstime)10000 * np->duration, (hrt_callout)next_trampoline, this);
}
bool
ToneAlarm::check(tone_note *pattern)
{
for (unsigned i = 0; i < _note_max; i++) {
/* first note must not be repeat or end */
if ((i == 0) &&
((pattern[i].duration == DURATION_END) || (pattern[i].duration == DURATION_REPEAT)))
return false;
if (pattern[i].duration == DURATION_END)
break;
/* pitch must be legal */
if (pattern[i].pitch >= _note_max)
return false;
}
return true;
}
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");
}
apps/px4/tests/test_hrt.c
+1 -1
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>