forked from Archive/PX4-Autopilot
ToneAlarm class refactoring to implement an interface for hardware specific methods and a single ToneAlarm class.
This commit is contained in:
parent
0f386ee52a
commit
dc5f18bdcd
|
@ -45,6 +45,7 @@ px4_add_board(
|
|||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
uavcan
|
||||
|
||||
MODULES
|
||||
|
|
|
@ -50,6 +50,7 @@ px4_add_board(
|
|||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
uavcan
|
||||
|
||||
MODULES
|
||||
|
|
|
@ -50,6 +50,7 @@ px4_add_board(
|
|||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
#tone_alarm
|
||||
uavcan
|
||||
|
||||
MODULES
|
||||
|
|
|
@ -48,6 +48,7 @@ px4_add_board(
|
|||
#tap_esc
|
||||
#telemetry # all available telemetry drivers
|
||||
#test_ppm
|
||||
tone_alarm
|
||||
|
||||
MODULES
|
||||
#attitude_estimator_q
|
||||
|
|
|
@ -47,6 +47,7 @@ px4_add_board(
|
|||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
#test_ppm # NOT Portable YET
|
||||
tone_alarm
|
||||
|
||||
MODULES
|
||||
attitude_estimator_q
|
||||
|
|
|
@ -59,6 +59,7 @@ px4_add_board(
|
|||
#tap_esc
|
||||
#telemetry # all available telemetry drivers
|
||||
#test_ppm
|
||||
tone_alarm
|
||||
#uavcan
|
||||
|
||||
MODULES
|
||||
|
|
|
@ -55,6 +55,7 @@ px4_add_board(
|
|||
#tap_esc
|
||||
#telemetry # all available telemetry drivers
|
||||
#test_ppm
|
||||
tone_alarm
|
||||
#uavcan
|
||||
|
||||
MODULES
|
||||
|
|
|
@ -56,6 +56,7 @@ px4_add_board(
|
|||
#tap_esc
|
||||
#telemetry # all available telemetry drivers
|
||||
#test_ppm
|
||||
tone_alarm
|
||||
#uavcan
|
||||
|
||||
MODULES
|
||||
|
|
|
@ -58,6 +58,7 @@ px4_add_board(
|
|||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
uavcan
|
||||
|
||||
MODULES
|
||||
|
|
|
@ -58,6 +58,7 @@ px4_add_board(
|
|||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
uavcan
|
||||
|
||||
MODULES
|
||||
|
|
|
@ -58,6 +58,7 @@ px4_add_board(
|
|||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
#uavcan
|
||||
|
||||
MODULES
|
||||
|
|
|
@ -43,6 +43,7 @@ px4_add_board(
|
|||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
uavcan
|
||||
|
||||
MODULES
|
||||
|
|
|
@ -44,6 +44,7 @@ px4_add_board(
|
|||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
uavcan
|
||||
|
||||
MODULES
|
||||
|
|
|
@ -43,6 +43,7 @@ px4_add_board(
|
|||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
#uavcan
|
||||
|
||||
MODULES
|
||||
|
|
|
@ -57,6 +57,7 @@ px4_add_board(
|
|||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
uavcan
|
||||
|
||||
MODULES
|
||||
|
|
|
@ -57,6 +57,7 @@ px4_add_board(
|
|||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
uavcan
|
||||
|
||||
MODULES
|
||||
|
|
|
@ -58,6 +58,7 @@ px4_add_board(
|
|||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
uavcan
|
||||
|
||||
MODULES
|
||||
|
|
|
@ -58,6 +58,7 @@ px4_add_board(
|
|||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
uavcan
|
||||
|
||||
MODULES
|
||||
|
|
|
@ -58,6 +58,7 @@ px4_add_board(
|
|||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
#uavcan
|
||||
|
||||
MODULES
|
||||
|
|
|
@ -17,7 +17,8 @@ px4_add_board(
|
|||
#magnetometer # all available magnetometer drivers
|
||||
pwm_out_sim
|
||||
#telemetry # all available telemetry drivers
|
||||
tone_alarm_sim
|
||||
sim/tone_alarm
|
||||
tone_alarm
|
||||
#uavcan
|
||||
|
||||
MODULES
|
||||
|
|
|
@ -17,7 +17,8 @@ px4_add_board(
|
|||
#magnetometer # all available magnetometer drivers
|
||||
pwm_out_sim
|
||||
#telemetry # all available telemetry drivers
|
||||
tone_alarm_sim
|
||||
sim/tone_alarm
|
||||
tone_alarm
|
||||
#uavcan
|
||||
|
||||
MODULES
|
||||
|
|
|
@ -17,7 +17,8 @@ px4_add_board(
|
|||
#magnetometer # all available magnetometer drivers
|
||||
pwm_out_sim
|
||||
#telemetry # all available telemetry drivers
|
||||
tone_alarm_sim
|
||||
sim/tone_alarm
|
||||
tone_alarm
|
||||
#uavcan
|
||||
|
||||
MODULES
|
||||
|
|
|
@ -0,0 +1,193 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013-2019 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 ToneAlarmInterface.cpp
|
||||
*/
|
||||
|
||||
#include <chip/sam_pinmap.h>
|
||||
|
||||
#include <drivers/device/device.h>
|
||||
#include <lib/drivers/tone_alarm/ToneAlarmInterface.h>
|
||||
#include <px4_defines.h>
|
||||
|
||||
/* Check that tone alarm and HRT timers are different */
|
||||
#if defined(TONE_ALARM_CHANNEL) && defined(HRT_TIMER_CHANNEL)
|
||||
# if TONE_ALARM_CHANNEL == HRT_TIMER_CHANNEL
|
||||
# error TONE_ALARM_CHANNEL and HRT_TIMER_CHANNEL must use different timers.
|
||||
# endif
|
||||
#endif
|
||||
|
||||
#if defined(TONE_ALARM_TIMER)
|
||||
# error "TONE_ALARM_TIMER should not be defined, instead define TONE_ALARM_CHANNEL from 0-11"
|
||||
#endif
|
||||
|
||||
#ifndef TONE_ALARM_CLOCK
|
||||
#define TONE_ALARM_CLOCK (BOARD_MCK_FREQUENCY/128)
|
||||
#endif
|
||||
|
||||
/* Period of the free-running counter, in microseconds. */
|
||||
#ifndef TONE_ALARM_COUNTER_PERIOD
|
||||
#define TONE_ALARM_COUNTER_PERIOD 65536
|
||||
#endif
|
||||
|
||||
/* Tone alarm configuration */
|
||||
#if TONE_ALARM_CHANNEL == 0 || TONE_ALARM_CHANNEL == 1 || TONE_ALARM_CHANNEL == 2
|
||||
# define TONE_ALARM_TIMER 0
|
||||
#endif
|
||||
#if TONE_ALARM_CHANNEL == 3 || TONE_ALARM_CHANNEL == 4 || TONE_ALARM_CHANNEL == 5
|
||||
# define TONE_ALARM_TIMER 1
|
||||
#endif
|
||||
#if TONE_ALARM_CHANNEL == 6 || TONE_ALARM_CHANNEL == 7 || TONE_ALARM_CHANNEL == 8
|
||||
# define TONE_ALARM_TIMER 2
|
||||
#endif
|
||||
#if TONE_ALARM_CHANNEL == 9 || TONE_ALARM_CHANNEL == 10 || TONE_ALARM_CHANNEL == 11
|
||||
# define TONE_ALARM_TIMER 3
|
||||
#endif
|
||||
|
||||
/* HRT configuration */
|
||||
#if TONE_ALARM_TIMER == 0
|
||||
# define HRT_TIMER_BASE SAM_TC012_BASE
|
||||
# if !defined(CONFIG_SAMV7_TC0)
|
||||
# error "HRT_TIMER_CHANNEL 0-2 Require CONFIG_SAMV7_TC0=y"
|
||||
# endif
|
||||
#elif TONE_ALARM_TIMER == 1
|
||||
# define HRT_TIMER_BASE SAM_TC345_BASE
|
||||
# if !defined(CONFIG_SAMV7_TC1)
|
||||
# error "HRT_TIMER_CHANNEL 3-5 Require CONFIG_SAMV7_TC1=y"
|
||||
# endif
|
||||
#elif TONE_ALARM_TIMER == 2
|
||||
# define HRT_TIMER_BASE SAM_TC678_BASE
|
||||
# if !defined(CONFIG_SAMV7_TC2)
|
||||
# error "HRT_TIMER_CHANNEL 6-8 Require CONFIG_SAMV7_TC2=y"
|
||||
# endif
|
||||
#elif TONE_ALARM_TIMER == 3
|
||||
# define HRT_TIMER_BASE SAM_TC901_BASE
|
||||
# if !defined(CONFIG_SAMV7_TC3)
|
||||
# error "HRT_TIMER_CHANNEL 9-11 Require CONFIG_SAMV7_TC3=y"
|
||||
# endif
|
||||
#else
|
||||
# error "HRT_TIMER_CHANNEL should be defined valid value are from 0-11"
|
||||
# endif
|
||||
|
||||
|
||||
/* Timer register accessors. */
|
||||
#define REG(_reg) (*(volatile uint32_t *)(SAM_TC0_BASE + SAM_TC_CHAN_OFFSET(HRT_TIMER_CHANNEL) + _reg))
|
||||
|
||||
#define rCCR REG(SAM_TC_CCR_OFFSET)
|
||||
#define rCMR REG(SAM_TC_CMR_OFFSET)
|
||||
#define rCV REG(SAM_TC_CV_OFFSET)
|
||||
#define rEMR REG(SAM_TC_EMR_OFFSET)
|
||||
#define rIER REG(SAM_TC_IER_OFFSET)
|
||||
#define rIDR REG(SAM_TC_IDR_OFFSET)
|
||||
#define rIMR REG(SAM_TC_IMR_OFFSET)
|
||||
#define rRA REG(SAM_TC_RA_OFFSET)
|
||||
#define rRAB REG(SAM_TC_RAB_OFFSET)
|
||||
#define rRB REG(SAM_TC_RB_OFFSET)
|
||||
#define rRC REG(SAM_TC_RC_OFFSET)
|
||||
#define rSMMR REG(SAM_TC_SMMR_OFFSET)
|
||||
#define rSR REG(SAM_TC_SR_OFFSET)
|
||||
|
||||
void ToneAlarmInterface::init()
|
||||
{
|
||||
#ifdef GPIO_TONE_ALARM_NEG
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM_NEG);
|
||||
#else
|
||||
// Configure the GPIO to the idle state.
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM_IDLE);
|
||||
#endif
|
||||
|
||||
// TODO - This is dead code.
|
||||
#if TONE_ALARM_NOT_DONE
|
||||
// Initialize the timer.
|
||||
rCCER &= TONE_CCER; // Unlock CCMR* registers.
|
||||
rCCER = TONE_CCER;
|
||||
rCCMR1 = TONE_CCMR1;
|
||||
rCCMR2 = TONE_CCMR2;
|
||||
rDCR = 0;
|
||||
rCR1 = 0;
|
||||
rCR2 = 0;
|
||||
rDIER = 0;
|
||||
rSMCR = 0;
|
||||
|
||||
#ifdef rBDTR // If using an advanced timer, you need to activate the output.
|
||||
rBDTR = ATIM_BDTR_MOE; // Enable the main output of the advanced timer.
|
||||
#endif
|
||||
|
||||
TONE_rCCR = 1; // Toggle the CC output each time the count passes 1.
|
||||
rPSC = 0; // Default the timer to a prescale value of 1; playing notes will change this.
|
||||
rCR1 = GTIM_CR1_CEN; // Ensure the timer is running.
|
||||
#endif
|
||||
}
|
||||
|
||||
void ToneAlarmInterface::start_note(unsigned frequency)
|
||||
{
|
||||
// Calculate the signal switching period.
|
||||
// (Signal switching period is one half of the frequency period).
|
||||
float signal_period = (1.0f / frequency) * 0.5f;
|
||||
|
||||
// Calculate the hardware clock divisor rounded to the nearest integer.
|
||||
unsigned int divisor = std::round(signal_period * TONE_ALARM_CLOCK);
|
||||
|
||||
// Pick the lowest prescaler value that we can use.
|
||||
// (Note that the effective prescale value is 1 greater.)
|
||||
unsigned int prescale = divisor / TONE_ALARM_COUNTER_PERIOD;
|
||||
|
||||
// Calculate the period for the selected prescaler value.
|
||||
unsigned int period = (divisor / (prescale + 1)) - 1;
|
||||
|
||||
// TODO - This is dead code.
|
||||
#if TONE_ALARM_NOT_DONE
|
||||
rPSC = prescale; // Load new prescaler.
|
||||
rARR = period; // Load new signal switching period.
|
||||
rEGR = GTIM_EGR_UG; // Force a reload of the period.
|
||||
rCCER |= TONE_CCER; // Enable the output.
|
||||
#else
|
||||
prescale++;
|
||||
period++;
|
||||
#endif
|
||||
|
||||
// Configure the GPIO to enable timer output.
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM);
|
||||
}
|
||||
|
||||
void ToneAlarmInterface::stop_note()
|
||||
{
|
||||
// Stop the current note.
|
||||
// TODO - This is dead code.
|
||||
#if TONE_ALARM_NOT_DONE
|
||||
rCCER &= ~TONE_CCER;
|
||||
#endif
|
||||
// Ensure the GPIO is not driving the speaker.
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM_IDLE);
|
||||
}
|
|
@ -39,6 +39,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/types.h>
|
||||
#include <stdbool.h>
|
||||
#include <inttypes.h>
|
||||
|
|
|
@ -57,17 +57,19 @@
|
|||
* via the ioctl.
|
||||
*/
|
||||
|
||||
#ifndef DRV_TONE_ALARM_H_
|
||||
#define DRV_TONE_ALARM_H_
|
||||
#pragma once
|
||||
|
||||
#include <sys/ioctl.h>
|
||||
#include <lib/tunes/tune_definition.h>
|
||||
#include <uORB/topics/tune_control.h>
|
||||
|
||||
#define TONEALARM0_DEVICE_PATH "/dev/tone_alarm0"
|
||||
#define CBRK_BUZZER_KEY 782097
|
||||
#define CBRK_OFF 0
|
||||
#define CBRK_ON 1
|
||||
#define CBRK_UNINIT 2
|
||||
|
||||
#define _TONE_ALARM_BASE 0x7400
|
||||
#define TONE_SET_ALARM _PX4_IOC(_TONE_ALARM_BASE, 1)
|
||||
#define _TONE_ALARM_BASE 0x7400
|
||||
#define TONE_SET_ALARM _PX4_IOC(_TONE_ALARM_BASE, 1)
|
||||
#define TONE_ALARM0_DEVICE_PATH "/dev/tone_alarm0"
|
||||
|
||||
// TODO: remove this once the tone_alarm driver is changed to the new tunelib
|
||||
enum {
|
||||
|
@ -89,5 +91,3 @@ enum {
|
|||
TONE_HOME_SET,
|
||||
TONE_NUMBER_OF_TUNES
|
||||
};
|
||||
|
||||
#endif /* DRV_TONE_ALARM_H_ */
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 2015-2019 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
|
||||
|
@ -31,12 +31,6 @@
|
|||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE drivers__tone_alarm
|
||||
MAIN tone_alarm
|
||||
SRCS
|
||||
tone_alarm.cpp
|
||||
DEPENDS
|
||||
circuit_breaker
|
||||
tunes
|
||||
)
|
||||
px4_add_library(tone_alarm_interface
|
||||
ToneAlarmInterface.cpp
|
||||
)
|
||||
|
|
|
@ -0,0 +1,183 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2017-2019 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 ToneAlarmInterface.cpp
|
||||
*/
|
||||
|
||||
#include "chip/kinetis_sim.h"
|
||||
#include "kinetis_tpm.h"
|
||||
|
||||
#include <drivers/device/device.h>
|
||||
#include <lib/drivers/tone_alarm/ToneAlarmInterface.h>
|
||||
#include <px4_defines.h>
|
||||
#include <systemlib/px4_macros.h>
|
||||
|
||||
|
||||
#define CAT3_(A, B, C) A##B##C
|
||||
#define CAT3(A, B, C) CAT3_(A, B, C)
|
||||
|
||||
/* Check that tone alarm and HRT timers are different */
|
||||
#if defined(TONE_ALARM_TIMER) && defined(HRT_TIMER)
|
||||
# if TONE_ALARM_TIMER == HRT_TIMER
|
||||
# error TONE_ALARM_TIMER and HRT_TIMER must use different timers.
|
||||
# endif
|
||||
#endif
|
||||
|
||||
#ifndef TONE_ALARM_CLOCK
|
||||
#define TONE_ALARM_CLOCK (BOARD_TPM_FREQ/(1 << (TONE_ALARM_TIMER_PRESCALE >> TPM_SC_PS_SHIFT)))
|
||||
#endif
|
||||
|
||||
/* Period of the free-running counter, in microseconds. */
|
||||
#ifndef TONE_ALARM_COUNTER_PERIOD
|
||||
#define TONE_ALARM_COUNTER_PERIOD 65536
|
||||
#endif
|
||||
|
||||
/* Tone Alarm configuration */
|
||||
#define TONE_ALARM_SIM_SCGC2_TPM CAT(SIM_SCGC2_TPM, TONE_ALARM_TIMER) /* The Clock Gating enable bit for this TPM */
|
||||
#define TONE_ALARM_TIMER_BASE CAT(CAT(KINETIS_TPM, TONE_ALARM_TIMER),_BASE) /* The Base address of the TPM */
|
||||
#define TONE_ALARM_TIMER_CLOCK BOARD_TPM_FREQ /* The input clock frequency to the TPM block */
|
||||
#define TONE_ALARM_TIMER_PRESCALE TPM_SC_PS_DIV16 /* The constant Prescaler */
|
||||
#define TONE_ALARM_TIMER_VECTOR CAT(KINETIS_IRQ_TPM, TONE_ALARM_TIMER) /* The TPM Interrupt vector */
|
||||
|
||||
#if TONE_ALARM_TIMER == 1 && defined(CONFIG_KINETIS_TPM1)
|
||||
# error must not set CONFIG_KINETIS_TPM1=y and TONE_ALARM_TIMER=1
|
||||
#elif TONE_ALARM_TIMER == 2 && defined(CONFIG_KINETIS_TPM2)
|
||||
# error must not set CONFIG_STM32_TIM2=y and TONE_ALARM_TIMER=2
|
||||
#endif
|
||||
|
||||
/* Tone Alarm clock must be a multiple of 1MHz greater than 1MHz. */
|
||||
#if (TONE_ALARM_TIMER_CLOCK % TONE_ALARM_CLOCK) != 0
|
||||
# error TONE_ALARM_TIMER_CLOCK must be a multiple of 1MHz
|
||||
#endif
|
||||
#if TONE_ALARM_TIMER_CLOCK <= TONE_ALARM_CLOCK
|
||||
# error TONE_ALARM_TIMER_CLOCK must be greater than 1MHz
|
||||
#endif
|
||||
|
||||
#if (TONE_ALARM_CHANNEL != 0) && (TONE_ALARM_CHANNEL != 1)
|
||||
# error TONE_ALARM_CHANNEL must be a value between 0 and 1
|
||||
#endif
|
||||
|
||||
|
||||
/* Register accessors */
|
||||
#define _REG(_addr) (*(volatile uint32_t *)(_addr))
|
||||
|
||||
/* Timer register accessors */
|
||||
#define REG(_reg) _REG(TONE_ALARM_TIMER_BASE + (_reg))
|
||||
|
||||
#define rC0SC REG(KINETIS_TPM_C0SC_OFFSET)
|
||||
#define rC0V REG(KINETIS_TPM_C0V_OFFSET)
|
||||
#define rC1SC REG(KINETIS_TPM_C1SC_OFFSET)
|
||||
#define rC1V REG(KINETIS_TPM_C1V_OFFSET)
|
||||
#define rCNT REG(KINETIS_TPM_CNT_OFFSET)
|
||||
#define rCOMBINE REG(KINETIS_TPM_COMBINE_OFFSET)
|
||||
#define rCONF REG(KINETIS_TPM_CONF_OFFSET)
|
||||
#define rFILTER REG(KINETIS_TPM_FILTER_OFFSET)
|
||||
#define rMOD REG(KINETIS_TPM_MOD_OFFSET)
|
||||
#define rPOL REG(KINETIS_TPM_POL_OFFSET)
|
||||
#define rQDCTRL REG(KINETIS_TPM_QDCTRL_OFFSET)
|
||||
#define rSC REG(KINETIS_TPM_SC_OFFSET)
|
||||
#define rSTATUS REG(KINETIS_TPM_STATUS_OFFSET)
|
||||
|
||||
/* Specific registers and bits used by Tone Alarm sub-functions. */
|
||||
# define rCNV CAT3(rC, TONE_ALARM_CHANNEL, V) /* Channel Value Register used by Tone alarm */
|
||||
# define rCNSC CAT3(rC, TONE_ALARM_CHANNEL, SC) /* Channel Status and Control Register used by Tone alarm */
|
||||
# define STATUS CAT3(TPM_STATUS_CH, TONE_ALARM_CHANNEL, F) /* Capture and Compare Status Register used by Tone alarm */
|
||||
|
||||
void ToneAlarmInterface::init()
|
||||
{
|
||||
#ifdef GPIO_TONE_ALARM_NEG
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM_NEG);
|
||||
#else
|
||||
// Configure the GPIO to the idle state.
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM_IDLE);
|
||||
#endif
|
||||
|
||||
// Select a the clock source to the TPM.
|
||||
uint32_t regval = _REG(KINETIS_SIM_SOPT2);
|
||||
|
||||
regval &= ~(SIM_SOPT2_TPMSRC_MASK);
|
||||
regval |= BOARD_TPM_CLKSRC;
|
||||
|
||||
_REG(KINETIS_SIM_SOPT2) = regval;
|
||||
|
||||
// Enabled System Clock Gating Control for TPM.
|
||||
regval = _REG(KINETIS_SIM_SCGC2);
|
||||
regval |= TONE_ALARM_SIM_SCGC2_TPM;
|
||||
_REG(KINETIS_SIM_SCGC2) = regval;
|
||||
|
||||
// Disable and configure the timer.
|
||||
rSC = TPM_SC_TOF;
|
||||
rCNT = 0;
|
||||
rMOD = TONE_ALARM_COUNTER_PERIOD - 1;
|
||||
rSTATUS = (TPM_STATUS_TOF | STATUS);
|
||||
|
||||
// Configure for output compare to toggle on over flow.
|
||||
rCNSC = (TPM_CnSC_CHF | TPM_CnSC_MSA | TPM_CnSC_ELSA);
|
||||
|
||||
rCOMBINE = 0;
|
||||
rPOL = 0;
|
||||
rFILTER = 0;
|
||||
rQDCTRL = 0;
|
||||
rCONF = TPM_CONF_DBGMODE_CONT;
|
||||
|
||||
rCNV = 0; // Toggle the CC output each time the count passes 0.
|
||||
rSC |= (TPM_SC_CMOD_LPTPM_CLK | TONE_ALARM_TIMER_PRESCALE); // Enable the timer.
|
||||
rMOD = 0; // Default the timer to a modulo value of 1; playing notes will change this.
|
||||
}
|
||||
|
||||
void ToneAlarmInterface::start_note(unsigned frequency)
|
||||
{
|
||||
// Calculate the signal switching period.
|
||||
// (Signal switching period is one half of the frequency period).
|
||||
float signal_period = (1.0f / frequency) * 0.5f;
|
||||
|
||||
// Calculate the hardware clock divisor rounded to the nearest integer.
|
||||
unsigned int divisor = std::round(signal_period * TONE_ALARM_CLOCK);
|
||||
|
||||
rCNT = 0;
|
||||
rMOD = divisor; // Load new signal switching period.
|
||||
rSC |= (TPM_SC_CMOD_LPTPM_CLK);
|
||||
|
||||
// Configure the GPIO to enable timer output.
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM);
|
||||
}
|
||||
|
||||
void ToneAlarmInterface::stop_note()
|
||||
{
|
||||
// Stop the current note.
|
||||
rSC &= ~TPM_SC_CMOD_MASK;
|
||||
|
||||
// Ensure the GPIO is not driving the speaker.
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM_IDLE);
|
||||
}
|
|
@ -1,500 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2017-2018 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* Low Level Driver for the PX4 audio alarm port. Subscribes to
|
||||
* tune_control and plays notes on this architecture specific
|
||||
* timer HW
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <px4_log.h>
|
||||
#include <systemlib/px4_macros.h>
|
||||
#include <debug.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 <math.h>
|
||||
#include <ctype.h>
|
||||
|
||||
#include <board_config.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include "kinetis.h"
|
||||
#include "chip/kinetis_sim.h"
|
||||
#include "kinetis_tpm.h"
|
||||
|
||||
#include <systemlib/err.h>
|
||||
#include <circuit_breaker/circuit_breaker.h>
|
||||
|
||||
#include <px4_workqueue.h>
|
||||
|
||||
#include <lib/tunes/tunes.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/tune_control.h>
|
||||
|
||||
#define CAT3_(A, B, C) A##B##C
|
||||
#define CAT3(A, B, C) CAT3_(A, B, C)
|
||||
|
||||
/* Check that tone alarm and HRT timers are different */
|
||||
#if defined(TONE_ALARM_TIMER) && defined(HRT_TIMER)
|
||||
# if TONE_ALARM_TIMER == HRT_TIMER
|
||||
# error TONE_ALARM_TIMER and HRT_TIMER must use different timers.
|
||||
# endif
|
||||
#endif
|
||||
|
||||
|
||||
/*
|
||||
* Period of the free-running counter, in microseconds.
|
||||
*/
|
||||
#define TONE_ALARM_COUNTER_PERIOD 65536
|
||||
|
||||
/* Tone Alarm configuration */
|
||||
|
||||
#define TONE_ALARM_TIMER_CLOCK BOARD_TPM_FREQ /* The input clock frequency to the TPM block */
|
||||
#define TONE_ALARM_TIMER_BASE CAT(CAT(KINETIS_TPM, TONE_ALARM_TIMER),_BASE) /* The Base address of the TPM */
|
||||
#define TONE_ALARM_TIMER_VECTOR CAT(KINETIS_IRQ_TPM, TONE_ALARM_TIMER) /* The TPM Interrupt vector */
|
||||
#define TONE_ALARM_SIM_SCGC2_TPM CAT(SIM_SCGC2_TPM, TONE_ALARM_TIMER) /* The Clock Gating enable bit for this TPM */
|
||||
#define TONE_ALARM_TIMER_PRESCALE TPM_SC_PS_DIV16 /* The constant Prescaler */
|
||||
|
||||
#if TONE_ALARM_TIMER == 1 && defined(CONFIG_KINETIS_TPM1)
|
||||
# error must not set CONFIG_KINETIS_TPM1=y and TONE_ALARM_TIMER=1
|
||||
#elif TONE_ALARM_TIMER == 2 && defined(CONFIG_KINETIS_TPM2)
|
||||
# error must not set CONFIG_STM32_TIM2=y and TONE_ALARM_TIMER=2
|
||||
#endif
|
||||
|
||||
|
||||
# define TONE_ALARM_TIMER_FREQ (BOARD_TPM_FREQ/(1 << (TONE_ALARM_TIMER_PRESCALE >> TPM_SC_PS_SHIFT)))
|
||||
|
||||
/*
|
||||
* Toan Alarm clock must be a multiple of 1MHz greater than 1MHz
|
||||
*/
|
||||
#if (TONE_ALARM_TIMER_CLOCK % TONE_ALARM_TIMER_FREQ) != 0
|
||||
# error TONE_ALARM_TIMER_CLOCK must be a multiple of 1MHz
|
||||
#endif
|
||||
#if TONE_ALARM_TIMER_CLOCK <= TONE_ALARM_TIMER_FREQ
|
||||
# error TONE_ALARM_TIMER_CLOCK must be greater than 1MHz
|
||||
#endif
|
||||
|
||||
#if (TONE_ALARM_CHANNEL != 0) && (TONE_ALARM_CHANNEL != 1)
|
||||
# error TONE_ALARM_CHANNEL must be a value between 0 and 1
|
||||
#endif
|
||||
|
||||
|
||||
/* Register accessors */
|
||||
|
||||
#define _REG(_addr) (*(volatile uint32_t *)(_addr))
|
||||
|
||||
/* Timer register accessors */
|
||||
|
||||
#define REG(_reg) _REG(TONE_ALARM_TIMER_BASE + (_reg))
|
||||
|
||||
#define rSC REG(KINETIS_TPM_SC_OFFSET)
|
||||
#define rCNT REG(KINETIS_TPM_CNT_OFFSET)
|
||||
#define rMOD REG(KINETIS_TPM_MOD_OFFSET)
|
||||
#define rC0SC REG(KINETIS_TPM_C0SC_OFFSET)
|
||||
#define rC0V REG(KINETIS_TPM_C0V_OFFSET)
|
||||
#define rC1SC REG(KINETIS_TPM_C1SC_OFFSET)
|
||||
#define rC1V REG(KINETIS_TPM_C1V_OFFSET)
|
||||
#define rSTATUS REG(KINETIS_TPM_STATUS_OFFSET)
|
||||
#define rCOMBINE REG(KINETIS_TPM_COMBINE_OFFSET)
|
||||
#define rPOL REG(KINETIS_TPM_POL_OFFSET)
|
||||
#define rFILTER REG(KINETIS_TPM_FILTER_OFFSET)
|
||||
#define rQDCTRL REG(KINETIS_TPM_QDCTRL_OFFSET)
|
||||
#define rCONF REG(KINETIS_TPM_CONF_OFFSET)
|
||||
|
||||
/*
|
||||
* Specific registers and bits used by Tone Alarm sub-functions
|
||||
*/
|
||||
|
||||
# define rCNV CAT3(rC, TONE_ALARM_CHANNEL, V) /* Channel Value Register used by Tone alarm */
|
||||
# define rCNSC CAT3(rC, TONE_ALARM_CHANNEL, SC) /* Channel Status and Control Register used by Tone alarm */
|
||||
# define STATUS CAT3(TPM_STATUS_CH, TONE_ALARM_CHANNEL, F) /* Capture and Compare Status Register used by Tone alarm */
|
||||
|
||||
#define CBRK_BUZZER_KEY 782097
|
||||
|
||||
class ToneAlarm : public device::CDev
|
||||
{
|
||||
public:
|
||||
ToneAlarm();
|
||||
~ToneAlarm();
|
||||
|
||||
virtual int init();
|
||||
void status();
|
||||
|
||||
enum {
|
||||
CBRK_OFF = 0,
|
||||
CBRK_ON,
|
||||
CBRK_UNINIT
|
||||
};
|
||||
|
||||
private:
|
||||
volatile bool _running;
|
||||
volatile bool _should_run;
|
||||
bool _play_tone;
|
||||
|
||||
Tunes _tunes;
|
||||
|
||||
unsigned _silence_length; // if nonzero, silence before next note
|
||||
|
||||
int _cbrk; ///< if true, no audio output
|
||||
int _tune_control_sub;
|
||||
|
||||
tune_control_s _tune;
|
||||
|
||||
static work_s _work;
|
||||
|
||||
// Convert a frequency value into a divisor for the configured timer's clock.
|
||||
//
|
||||
unsigned frequency_to_divisor(unsigned frequency);
|
||||
|
||||
// Start playing the note
|
||||
//
|
||||
void start_note(unsigned frequency);
|
||||
|
||||
// Stop playing the current note and make the player 'safe'
|
||||
//
|
||||
void stop_note();
|
||||
|
||||
// Parse the next note out of the string and play it
|
||||
//
|
||||
void next_note();
|
||||
|
||||
// work queue trampoline for next_note
|
||||
//
|
||||
static void next_trampoline(void *arg);
|
||||
|
||||
};
|
||||
|
||||
struct work_s ToneAlarm::_work = {};
|
||||
|
||||
/*
|
||||
* Driver 'main' command.
|
||||
*/
|
||||
extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]);
|
||||
|
||||
|
||||
ToneAlarm::ToneAlarm() :
|
||||
CDev("tone_alarm", TONEALARM0_DEVICE_PATH),
|
||||
_running(false),
|
||||
_should_run(true),
|
||||
_play_tone(false),
|
||||
_tunes(),
|
||||
_silence_length(0),
|
||||
_cbrk(CBRK_UNINIT),
|
||||
_tune_control_sub(-1)
|
||||
{
|
||||
// enable debug() calls
|
||||
//_debug_enabled = true;
|
||||
}
|
||||
|
||||
ToneAlarm::~ToneAlarm()
|
||||
{
|
||||
_should_run = false;
|
||||
int counter = 0;
|
||||
|
||||
while (_running && ++counter < 10) {
|
||||
usleep(100000);
|
||||
}
|
||||
}
|
||||
|
||||
int ToneAlarm::init()
|
||||
{
|
||||
int ret;
|
||||
|
||||
ret = CDev::init();
|
||||
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* configure the GPIO to the idle state */
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM_IDLE);
|
||||
|
||||
#ifdef GPIO_TONE_ALARM_NEG
|
||||
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM_NEG);
|
||||
#endif
|
||||
|
||||
|
||||
/* Select a the clock source to the TPM */
|
||||
|
||||
uint32_t regval = _REG(KINETIS_SIM_SOPT2);
|
||||
regval &= ~(SIM_SOPT2_TPMSRC_MASK);
|
||||
regval |= BOARD_TPM_CLKSRC;
|
||||
_REG(KINETIS_SIM_SOPT2) = regval;
|
||||
|
||||
|
||||
/* Enabled System Clock Gating Control for TPM */
|
||||
|
||||
regval = _REG(KINETIS_SIM_SCGC2);
|
||||
regval |= TONE_ALARM_SIM_SCGC2_TPM;
|
||||
_REG(KINETIS_SIM_SCGC2) = regval;
|
||||
|
||||
/* disable and configure the timer */
|
||||
|
||||
rSC = TPM_SC_TOF;
|
||||
|
||||
rCNT = 0;
|
||||
rMOD = TONE_ALARM_COUNTER_PERIOD - 1;
|
||||
|
||||
rSTATUS = (TPM_STATUS_TOF | STATUS);
|
||||
|
||||
/* Configure for output compare to toggle on over flow */
|
||||
|
||||
rCNSC = (TPM_CnSC_CHF | TPM_CnSC_MSA | TPM_CnSC_ELSA);
|
||||
|
||||
rCOMBINE = 0;
|
||||
rPOL = 0;
|
||||
rFILTER = 0;
|
||||
rQDCTRL = 0;
|
||||
rCONF = TPM_CONF_DBGMODE_CONT;
|
||||
|
||||
/* toggle the CC output each time the count passes 0 */
|
||||
|
||||
rCNV = 0;
|
||||
|
||||
/* enable the timer */
|
||||
|
||||
rSC |= (TPM_SC_CMOD_LPTPM_CLK | TONE_ALARM_TIMER_PRESCALE);
|
||||
|
||||
|
||||
/* default the timer to a modulo value of 1; playing notes will change this */
|
||||
rMOD = 0;
|
||||
|
||||
DEVICE_DEBUG("ready");
|
||||
_running = true;
|
||||
work_queue(HPWORK, &_work, (worker_t)&ToneAlarm::next_trampoline, this, 0);
|
||||
return OK;
|
||||
}
|
||||
|
||||
void ToneAlarm::status()
|
||||
{
|
||||
if (_running) {
|
||||
PX4_INFO("running");
|
||||
|
||||
} else {
|
||||
PX4_INFO("stopped");
|
||||
}
|
||||
}
|
||||
|
||||
unsigned ToneAlarm::frequency_to_divisor(unsigned frequency)
|
||||
{
|
||||
float period = 0.5f / frequency;
|
||||
|
||||
// and the divisor, rounded to the nearest integer
|
||||
unsigned divisor = (period * TONE_ALARM_TIMER_FREQ) + 0.5f;
|
||||
|
||||
return divisor;
|
||||
}
|
||||
|
||||
void ToneAlarm::start_note(unsigned frequency)
|
||||
{
|
||||
// check if circuit breaker is enabled
|
||||
if (_cbrk == CBRK_UNINIT) {
|
||||
_cbrk = circuit_breaker_enabled("CBRK_BUZZER", CBRK_BUZZER_KEY);
|
||||
}
|
||||
|
||||
if (_cbrk != CBRK_OFF) { return; }
|
||||
|
||||
// compute the divisor
|
||||
unsigned divisor = frequency_to_divisor(frequency);
|
||||
|
||||
rCNT = 0;
|
||||
rMOD = divisor; // load new toggle period
|
||||
|
||||
rSC |= (TPM_SC_CMOD_LPTPM_CLK);
|
||||
|
||||
// configure the GPIO to enable timer output
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM);
|
||||
}
|
||||
|
||||
void ToneAlarm::stop_note()
|
||||
{
|
||||
/* stop the current note */
|
||||
rSC &= ~TPM_SC_CMOD_MASK;
|
||||
|
||||
/*
|
||||
* Make sure the GPIO is not driving the speaker.
|
||||
*/
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM_IDLE);
|
||||
}
|
||||
|
||||
void ToneAlarm::next_note()
|
||||
{
|
||||
if (!_should_run) {
|
||||
if (_tune_control_sub >= 0) {
|
||||
orb_unsubscribe(_tune_control_sub);
|
||||
}
|
||||
|
||||
_running = false;
|
||||
return;
|
||||
}
|
||||
|
||||
// subscribe to tune_control
|
||||
if (_tune_control_sub < 0) {
|
||||
_tune_control_sub = orb_subscribe(ORB_ID(tune_control));
|
||||
}
|
||||
|
||||
// do we have an inter-note gap to wait for?
|
||||
if (_silence_length > 0) {
|
||||
stop_note();
|
||||
work_queue(HPWORK, &_work, (worker_t)&ToneAlarm::next_trampoline, this, USEC2TICK(_silence_length));
|
||||
_silence_length = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
// check for updates
|
||||
bool updated = false;
|
||||
orb_check(_tune_control_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(tune_control), _tune_control_sub, &_tune);
|
||||
_play_tone = _tunes.set_control(_tune) == 0;
|
||||
}
|
||||
|
||||
unsigned frequency = 0;
|
||||
unsigned duration = 0;
|
||||
|
||||
if (_play_tone) {
|
||||
_play_tone = false;
|
||||
int parse_ret_val = _tunes.get_next_tune(frequency, duration, _silence_length);
|
||||
|
||||
if (parse_ret_val >= 0) {
|
||||
// a frequency of 0 correspond to stop_note
|
||||
if (frequency > 0) {
|
||||
// start playing the note
|
||||
start_note(frequency);
|
||||
|
||||
} else {
|
||||
stop_note();
|
||||
}
|
||||
|
||||
|
||||
if (parse_ret_val > 0) {
|
||||
// continue playing
|
||||
_play_tone = true;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
// schedule a call with the tunes max interval
|
||||
duration = _tunes.get_maximum_update_interval();
|
||||
// stop playing the last note after the duration elapsed
|
||||
stop_note();
|
||||
}
|
||||
|
||||
// and arrange a callback when the note should stop
|
||||
work_queue(HPWORK, &_work, (worker_t)&ToneAlarm::next_trampoline, this, USEC2TICK(duration));
|
||||
}
|
||||
|
||||
void ToneAlarm::next_trampoline(void *arg)
|
||||
{
|
||||
ToneAlarm *ta = (ToneAlarm *)arg;
|
||||
ta->next_note();
|
||||
}
|
||||
|
||||
/**
|
||||
* Local functions in support of the shell command.
|
||||
*/
|
||||
namespace
|
||||
{
|
||||
|
||||
ToneAlarm *g_dev;
|
||||
|
||||
} // namespace
|
||||
|
||||
void tone_alarm_usage();
|
||||
|
||||
void tone_alarm_usage()
|
||||
{
|
||||
PX4_INFO("missing command, try 'start', status, 'stop'");
|
||||
}
|
||||
|
||||
int tone_alarm_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
if (argc > 1) {
|
||||
const char *argv1 = argv[1];
|
||||
|
||||
if (!strcmp(argv1, "start")) {
|
||||
if (g_dev != nullptr) {
|
||||
PX4_ERR("already started");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
g_dev = new ToneAlarm();
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
PX4_ERR("couldn't allocate the ToneAlarm driver");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (OK != g_dev->init()) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
PX4_ERR("ToneAlarm init failed");
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(argv1, "stop")) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(argv1, "status")) {
|
||||
g_dev->status();
|
||||
return 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
tone_alarm_usage();
|
||||
return 0;
|
||||
}
|
|
@ -0,0 +1,36 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2015-2019 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(tone_alarm_interface
|
||||
ToneAlarmInterface.cpp
|
||||
)
|
|
@ -0,0 +1,54 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013-2019 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 ToneAlarmInterface.cpp
|
||||
*/
|
||||
|
||||
#include <lib/drivers/tone_alarm/ToneAlarmInterface.h>
|
||||
#include <px4_defines.h>
|
||||
|
||||
void ToneAlarmInterface::init()
|
||||
{
|
||||
// Nothing to be done in simulation.
|
||||
}
|
||||
|
||||
void ToneAlarmInterface::start_note(unsigned frequency)
|
||||
{
|
||||
// Nothing to be done in simulation.
|
||||
}
|
||||
|
||||
void ToneAlarmInterface::stop_note()
|
||||
{
|
||||
// Nothing to be done in simulation.
|
||||
}
|
|
@ -1,6 +1,6 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 2015-2019 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
|
||||
|
@ -31,12 +31,6 @@
|
|||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE drivers__tone_alarm
|
||||
MAIN tone_alarm
|
||||
SRCS
|
||||
tone_alarm.cpp
|
||||
DEPENDS
|
||||
circuit_breaker
|
||||
tunes
|
||||
)
|
||||
px4_add_library(tone_alarm_interface
|
||||
ToneAlarmInterface.cpp
|
||||
)
|
||||
|
|
|
@ -0,0 +1,289 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013-2019 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 ToneAlarmInterface.cpp
|
||||
*/
|
||||
|
||||
#include <drivers/device/device.h>
|
||||
#include <lib/drivers/tone_alarm/ToneAlarmInterface.h>
|
||||
#include <px4_defines.h>
|
||||
|
||||
/* Check that tone alarm and HRT timers are different */
|
||||
#if defined(TONE_ALARM_TIMER) && defined(HRT_TIMER)
|
||||
# if TONE_ALARM_TIMER == HRT_TIMER
|
||||
# error TONE_ALARM_TIMER and HRT_TIMER must use different timers.
|
||||
# endif
|
||||
#endif
|
||||
|
||||
/* Period of the free-running counter, in microseconds. */
|
||||
#ifndef TONE_ALARM_COUNTER_PERIOD
|
||||
#define TONE_ALARM_COUNTER_PERIOD 65536
|
||||
#endif
|
||||
|
||||
/* Tone alarm configuration */
|
||||
#if TONE_ALARM_TIMER == 1
|
||||
# define TONE_ALARM_BASE STM32_TIM1_BASE
|
||||
# define TONE_ALARM_CLOCK STM32_APB2_TIM1_CLKIN
|
||||
# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM1EN
|
||||
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR
|
||||
# if defined(CONFIG_STM32_TIM1)
|
||||
# error Must not set CONFIG_STM32_TIM1 when TONE_ALARM_TIMER is 1
|
||||
# endif
|
||||
#elif 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
|
||||
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR
|
||||
# if defined(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
|
||||
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR
|
||||
# if defined(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
|
||||
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR
|
||||
# if defined(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
|
||||
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR
|
||||
# if defined(CONFIG_STM32_TIM5)
|
||||
# error Must not set CONFIG_STM32_TIM5 when TONE_ALARM_TIMER is 5
|
||||
# endif
|
||||
#elif TONE_ALARM_TIMER == 8
|
||||
# define TONE_ALARM_BASE STM32_TIM8_BASE
|
||||
# define TONE_ALARM_CLOCK STM32_APB2_TIM8_CLKIN
|
||||
# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM8EN
|
||||
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR
|
||||
# if defined(CONFIG_STM32_TIM8)
|
||||
# error Must not set CONFIG_STM32_TIM8 when TONE_ALARM_TIMER is 8
|
||||
# endif
|
||||
#elif TONE_ALARM_TIMER == 9
|
||||
# define TONE_ALARM_BASE STM32_TIM9_BASE
|
||||
# define TONE_ALARM_CLOCK STM32_APB2_TIM9_CLKIN
|
||||
# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM9EN
|
||||
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR
|
||||
# if defined(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_APB2_TIM10_CLKIN
|
||||
# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM10EN
|
||||
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR
|
||||
# if defined(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_APB2_TIM11_CLKIN
|
||||
# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM11EN
|
||||
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR
|
||||
# if defined(CONFIG_STM32_TIM11)
|
||||
# error Must not set CONFIG_STM32_TIM11 when TONE_ALARM_TIMER is 11
|
||||
# endif
|
||||
#elif TONE_ALARM_TIMER == 12
|
||||
# define TONE_ALARM_BASE STM32_TIM12_BASE
|
||||
# define TONE_ALARM_CLOCK STM32_APB1_TIM12_CLKIN
|
||||
# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM12EN
|
||||
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR
|
||||
# if defined(CONFIG_STM32_TIM12)
|
||||
# error Must not set CONFIG_STM32_TIM12 when TONE_ALARM_TIMER is 12
|
||||
# endif
|
||||
#elif TONE_ALARM_TIMER == 13
|
||||
# define TONE_ALARM_BASE STM32_TIM13_BASE
|
||||
# define TONE_ALARM_CLOCK STM32_APB1_TIM12_CLKIN
|
||||
# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM13EN
|
||||
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR
|
||||
# if defined(CONFIG_STM32_TIM13)
|
||||
# error Must not set CONFIG_STM32_TIM13 when TONE_ALARM_TIMER is 13
|
||||
# endif
|
||||
#elif TONE_ALARM_TIMER == 14
|
||||
# define TONE_ALARM_BASE STM32_TIM14_BASE
|
||||
# define TONE_ALARM_CLOCK STM32_APB1_TIM12_CLKIN
|
||||
# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM14EN
|
||||
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR
|
||||
# if defined(CONFIG_STM32_TIM14)
|
||||
# error Must not set CONFIG_STM32_TIM14 when TONE_ALARM_TIMER is 14
|
||||
# endif
|
||||
#else
|
||||
# error Must set TONE_ALARM_TIMER to one of the timers between 1 and 14 (inclusive) to use this driver.
|
||||
#endif // TONE_ALARM_TIMER
|
||||
|
||||
#if TONE_ALARM_CHANNEL == 1
|
||||
# define TONE_CCER (1 << 0)
|
||||
# define TONE_CCMR1 (3 << 4)
|
||||
# define TONE_CCMR2 0
|
||||
# define TONE_rCCR rCCR1
|
||||
#elif TONE_ALARM_CHANNEL == 2
|
||||
# define TONE_CCER (1 << 4)
|
||||
# define TONE_CCMR1 (3 << 12)
|
||||
# define TONE_CCMR2 0
|
||||
# define TONE_rCCR rCCR2
|
||||
#elif TONE_ALARM_CHANNEL == 3
|
||||
# define TONE_CCER (1 << 8)
|
||||
# define TONE_CCMR1 0
|
||||
# define TONE_CCMR2 (3 << 4)
|
||||
# define TONE_rCCR rCCR3
|
||||
#elif TONE_ALARM_CHANNEL == 4
|
||||
# define TONE_CCER (1 << 12)
|
||||
# define TONE_CCMR1 0
|
||||
# define TONE_CCMR2 (3 << 12)
|
||||
# define TONE_rCCR rCCR4
|
||||
#else
|
||||
# error Must set TONE_ALARM_CHANNEL to a value between 1 and 4 to use this driver.
|
||||
#endif // TONE_ALARM_CHANNEL
|
||||
|
||||
/* Timer register accessors. */
|
||||
#define REG(_reg) (*(volatile uint32_t *)(TONE_ALARM_BASE + _reg))
|
||||
|
||||
#if TONE_ALARM_TIMER == 1 || TONE_ALARM_TIMER == 8 // Note: If using TIM1 or TIM8, then you are using the ADVANCED timers and NOT the GENERAL TIMERS, therefore different registers
|
||||
# define rARR REG(STM32_ATIM_ARR_OFFSET)
|
||||
# define rBDTR REG(STM32_ATIM_BDTR_OFFSET)
|
||||
# define rCCER REG(STM32_ATIM_CCER_OFFSET)
|
||||
# define rCCMR1 REG(STM32_ATIM_CCMR1_OFFSET)
|
||||
# define rCCMR2 REG(STM32_ATIM_CCMR2_OFFSET)
|
||||
# define rCCR1 REG(STM32_ATIM_CCR1_OFFSET)
|
||||
# define rCCR2 REG(STM32_ATIM_CCR2_OFFSET)
|
||||
# define rCCR3 REG(STM32_ATIM_CCR3_OFFSET)
|
||||
# define rCCR4 REG(STM32_ATIM_CCR4_OFFSET)
|
||||
# define rCNT REG(STM32_ATIM_CNT_OFFSET)
|
||||
# define rCR1 REG(STM32_ATIM_CR1_OFFSET)
|
||||
# define rCR2 REG(STM32_ATIM_CR2_OFFSET)
|
||||
# define rDCR REG(STM32_ATIM_DCR_OFFSET)
|
||||
# define rDIER REG(STM32_ATIM_DIER_OFFSET)
|
||||
# define rDMAR REG(STM32_ATIM_DMAR_OFFSET)
|
||||
# define rEGR REG(STM32_ATIM_EGR_OFFSET)
|
||||
# define rPSC REG(STM32_ATIM_PSC_OFFSET)
|
||||
# define rRCR REG(STM32_ATIM_RCR_OFFSET)
|
||||
# define rSMCR REG(STM32_ATIM_SMCR_OFFSET)
|
||||
# define rSR REG(STM32_ATIM_SR_OFFSET)
|
||||
#else
|
||||
# define rARR REG(STM32_GTIM_ARR_OFFSET)
|
||||
# define rCCER REG(STM32_GTIM_CCER_OFFSET)
|
||||
# define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET)
|
||||
# define rCCMR2 REG(STM32_GTIM_CCMR2_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 rCNT REG(STM32_GTIM_CNT_OFFSET)
|
||||
# define rCR1 REG(STM32_GTIM_CR1_OFFSET)
|
||||
# define rCR2 REG(STM32_GTIM_CR2_OFFSET)
|
||||
# define rDCR REG(STM32_GTIM_DCR_OFFSET)
|
||||
# define rDIER REG(STM32_GTIM_DIER_OFFSET)
|
||||
# define rDMAR REG(STM32_GTIM_DMAR_OFFSET)
|
||||
# define rEGR REG(STM32_GTIM_EGR_OFFSET)
|
||||
# define rPSC REG(STM32_GTIM_PSC_OFFSET)
|
||||
# define rRCR REG(STM32_GTIM_RCR_OFFSET)
|
||||
# define rSMCR REG(STM32_GTIM_SMCR_OFFSET)
|
||||
# define rSR REG(STM32_GTIM_SR_OFFSET)
|
||||
#endif
|
||||
|
||||
void ToneAlarmInterface::init()
|
||||
{
|
||||
#ifdef GPIO_TONE_ALARM_NEG
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM_NEG);
|
||||
#else
|
||||
// Configure the GPIO to the idle state.
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM_IDLE);
|
||||
#endif
|
||||
|
||||
// Clock/power on our timer.
|
||||
modifyreg32(TONE_ALARM_CLOCK_POWER_REG, 0, TONE_ALARM_CLOCK_ENABLE);
|
||||
|
||||
// Initialize the timer.
|
||||
rCCER &= TONE_CCER; // Unlock CCMR* registers.
|
||||
rCCER = TONE_CCER;
|
||||
rCCMR1 = TONE_CCMR1;
|
||||
rCCMR2 = TONE_CCMR2;
|
||||
rCR1 = 0;
|
||||
rCR2 = 0;
|
||||
rDCR = 0;
|
||||
rDIER = 0;
|
||||
rSMCR = 0;
|
||||
|
||||
#ifdef rBDTR // If using an advanced timer, you need to activate the output.
|
||||
rBDTR = ATIM_BDTR_MOE; // Enable the main output of the advanced timer.
|
||||
#endif
|
||||
|
||||
TONE_rCCR = 1; // Toggle the CC output each time the count passes 1.
|
||||
rPSC = 0; // Default the timer to a prescale value of 1; playing notes will change this.
|
||||
rCR1 = GTIM_CR1_CEN; // Ensure the timer is running.
|
||||
}
|
||||
|
||||
void ToneAlarmInterface::start_note(unsigned frequency)
|
||||
{
|
||||
// Calculate the signal switching period.
|
||||
// (Signal switching period is one half of the frequency period).
|
||||
float signal_period = (1.0f / frequency) * 0.5f;
|
||||
|
||||
// Calculate the hardware clock divisor rounded to the nearest integer.
|
||||
unsigned int divisor = std::round(signal_period * TONE_ALARM_CLOCK);
|
||||
|
||||
// Pick the lowest prescaler value that we can use.
|
||||
// (Note that the effective prescale value is 1 greater.)
|
||||
unsigned int prescale = divisor / TONE_ALARM_COUNTER_PERIOD;
|
||||
|
||||
// Calculate the period for the selected prescaler value.
|
||||
unsigned int period = (divisor / (prescale + 1)) - 1;
|
||||
|
||||
rPSC = prescale; // Load new prescaler.
|
||||
rARR = period; // Load new toggle period.
|
||||
rEGR = GTIM_EGR_UG; // Force a reload of the period.
|
||||
rCCER |= TONE_CCER; // Enable the output.
|
||||
|
||||
// Configure the GPIO to enable timer output.
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM);
|
||||
}
|
||||
|
||||
void ToneAlarmInterface::stop_note()
|
||||
{
|
||||
// Stop the current note.
|
||||
rCCER &= ~TONE_CCER;
|
||||
|
||||
// Ensure the GPIO is not driving the speaker.
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM_IDLE);
|
||||
}
|
|
@ -1,586 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013, 2016, 2018 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* Low Level Driver for the PX4 audio alarm port. Subscribes to
|
||||
* tune_control and plays notes on this architecture specific
|
||||
* timer HW
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <px4_log.h>
|
||||
#include <debug.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 <math.h>
|
||||
#include <ctype.h>
|
||||
|
||||
#include <board_config.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <systemlib/err.h>
|
||||
#include <circuit_breaker/circuit_breaker.h>
|
||||
|
||||
#include <px4_workqueue.h>
|
||||
|
||||
#include <lib/tunes/tunes.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/tune_control.h>
|
||||
|
||||
/* Check that tone alarm and HRT timers are different */
|
||||
#if defined(TONE_ALARM_TIMER) && defined(HRT_TIMER)
|
||||
# if TONE_ALARM_TIMER == HRT_TIMER
|
||||
# error TONE_ALARM_TIMER and HRT_TIMER must use different timers.
|
||||
# endif
|
||||
#endif
|
||||
|
||||
/* Tone alarm configuration */
|
||||
#if TONE_ALARM_TIMER == 1
|
||||
# define TONE_ALARM_BASE STM32_TIM1_BASE
|
||||
# define TONE_ALARM_CLOCK STM32_APB2_TIM1_CLKIN
|
||||
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR
|
||||
# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM1EN
|
||||
# ifdef CONFIG_STM32_TIM1
|
||||
# error Must not set CONFIG_STM32_TIM1 when TONE_ALARM_TIMER is 1
|
||||
# endif
|
||||
#elif TONE_ALARM_TIMER == 2
|
||||
# define TONE_ALARM_BASE STM32_TIM2_BASE
|
||||
# define TONE_ALARM_CLOCK STM32_APB1_TIM2_CLKIN
|
||||
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR
|
||||
# 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_POWER_REG STM32_RCC_APB1ENR
|
||||
# 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_POWER_REG STM32_RCC_APB1ENR
|
||||
# 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_POWER_REG STM32_RCC_APB1ENR
|
||||
# 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 == 8
|
||||
# define TONE_ALARM_BASE STM32_TIM8_BASE
|
||||
# define TONE_ALARM_CLOCK STM32_APB2_TIM8_CLKIN
|
||||
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR
|
||||
# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM8EN
|
||||
# ifdef CONFIG_STM32_TIM8
|
||||
# error Must not set CONFIG_STM32_TIM8 when TONE_ALARM_TIMER is 8
|
||||
# endif
|
||||
#elif TONE_ALARM_TIMER == 9
|
||||
# define TONE_ALARM_BASE STM32_TIM9_BASE
|
||||
# define TONE_ALARM_CLOCK STM32_APB2_TIM9_CLKIN
|
||||
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR
|
||||
# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_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_APB2_TIM10_CLKIN
|
||||
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR
|
||||
# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_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_APB2_TIM11_CLKIN
|
||||
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR
|
||||
# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM11EN
|
||||
# ifdef CONFIG_STM32_TIM11
|
||||
# error Must not set CONFIG_STM32_TIM11 when TONE_ALARM_TIMER is 11
|
||||
# endif
|
||||
#elif TONE_ALARM_TIMER == 12
|
||||
# define TONE_ALARM_BASE STM32_TIM12_BASE
|
||||
# define TONE_ALARM_CLOCK STM32_APB1_TIM12_CLKIN
|
||||
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR
|
||||
# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM12EN
|
||||
# ifdef CONFIG_STM32_TIM12
|
||||
# error Must not set CONFIG_STM32_TIM12 when TONE_ALARM_TIMER is 12
|
||||
# endif
|
||||
#elif TONE_ALARM_TIMER == 13
|
||||
# define TONE_ALARM_BASE STM32_TIM13_BASE
|
||||
# define TONE_ALARM_CLOCK STM32_APB1_TIM12_CLKIN
|
||||
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR
|
||||
# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM13EN
|
||||
# ifdef CONFIG_STM32_TIM13
|
||||
# error Must not set CONFIG_STM32_TIM13 when TONE_ALARM_TIMER is 13
|
||||
# endif
|
||||
#elif TONE_ALARM_TIMER == 14
|
||||
# define TONE_ALARM_BASE STM32_TIM14_BASE
|
||||
# define TONE_ALARM_CLOCK STM32_APB1_TIM12_CLKIN
|
||||
# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR
|
||||
# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM14EN
|
||||
# ifdef CONFIG_STM32_TIM14
|
||||
# error Must not set CONFIG_STM32_TIM14 when TONE_ALARM_TIMER is 14
|
||||
# endif
|
||||
#else
|
||||
# error Must set TONE_ALARM_TIMER to one of the timers between 1 and 14 (inclusive) 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))
|
||||
|
||||
#if TONE_ALARM_TIMER == 1 || TONE_ALARM_TIMER == 8 // Note: If using TIM1 or TIM8, then you are using the ADVANCED timers and NOT the GENERAL TIMERS, therefore different registers
|
||||
# define rCR1 REG(STM32_ATIM_CR1_OFFSET)
|
||||
# define rCR2 REG(STM32_ATIM_CR2_OFFSET)
|
||||
# define rSMCR REG(STM32_ATIM_SMCR_OFFSET)
|
||||
# define rDIER REG(STM32_ATIM_DIER_OFFSET)
|
||||
# define rSR REG(STM32_ATIM_SR_OFFSET)
|
||||
# define rEGR REG(STM32_ATIM_EGR_OFFSET)
|
||||
# define rCCMR1 REG(STM32_ATIM_CCMR1_OFFSET)
|
||||
# define rCCMR2 REG(STM32_ATIM_CCMR2_OFFSET)
|
||||
# define rCCER REG(STM32_ATIM_CCER_OFFSET)
|
||||
# define rCNT REG(STM32_ATIM_CNT_OFFSET)
|
||||
# define rPSC REG(STM32_ATIM_PSC_OFFSET)
|
||||
# define rARR REG(STM32_ATIM_ARR_OFFSET)
|
||||
# define rRCR REG(STM32_ATIM_RCR_OFFSET)
|
||||
# define rCCR1 REG(STM32_ATIM_CCR1_OFFSET)
|
||||
# define rCCR2 REG(STM32_ATIM_CCR2_OFFSET)
|
||||
# define rCCR3 REG(STM32_ATIM_CCR3_OFFSET)
|
||||
# define rCCR4 REG(STM32_ATIM_CCR4_OFFSET)
|
||||
# define rBDTR REG(STM32_ATIM_BDTR_OFFSET)
|
||||
# define rDCR REG(STM32_ATIM_DCR_OFFSET)
|
||||
# define rDMAR REG(STM32_ATIM_DMAR_OFFSET)
|
||||
#else
|
||||
# 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)
|
||||
#endif
|
||||
|
||||
#define CBRK_BUZZER_KEY 782097
|
||||
|
||||
class ToneAlarm : public cdev::CDev
|
||||
{
|
||||
public:
|
||||
ToneAlarm();
|
||||
~ToneAlarm();
|
||||
|
||||
virtual int init();
|
||||
void status();
|
||||
|
||||
enum {
|
||||
CBRK_OFF = 0,
|
||||
CBRK_ON,
|
||||
CBRK_UNINIT
|
||||
};
|
||||
|
||||
private:
|
||||
volatile bool _running;
|
||||
volatile bool _should_run;
|
||||
bool _play_tone;
|
||||
|
||||
Tunes _tunes;
|
||||
|
||||
unsigned _silence_length; // if nonzero, silence before next note
|
||||
|
||||
int _cbrk; ///< if true, no audio output
|
||||
int _tune_control_sub;
|
||||
|
||||
tune_control_s _tune;
|
||||
|
||||
static work_s _work;
|
||||
|
||||
// Convert a frequency value into a divisor for the configured timer's clock.
|
||||
//
|
||||
unsigned frequency_to_divisor(unsigned frequency);
|
||||
|
||||
// Start playing the note
|
||||
//
|
||||
void start_note(unsigned frequency);
|
||||
|
||||
// Stop playing the current note and make the player 'safe'
|
||||
//
|
||||
void stop_note();
|
||||
|
||||
// Parse the next note out of the string and play it
|
||||
//
|
||||
void next_note();
|
||||
|
||||
// work queue trampoline for next_note
|
||||
//
|
||||
static void next_trampoline(void *arg);
|
||||
|
||||
};
|
||||
|
||||
struct work_s ToneAlarm::_work = {};
|
||||
|
||||
/*
|
||||
* Driver 'main' command.
|
||||
*/
|
||||
extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]);
|
||||
|
||||
|
||||
ToneAlarm::ToneAlarm() :
|
||||
CDev(TONEALARM0_DEVICE_PATH),
|
||||
_running(false),
|
||||
_should_run(true),
|
||||
_play_tone(false),
|
||||
_tunes(),
|
||||
_silence_length(0),
|
||||
_cbrk(CBRK_UNINIT),
|
||||
_tune_control_sub(-1)
|
||||
{
|
||||
// enable debug() calls
|
||||
//_debug_enabled = true;
|
||||
}
|
||||
|
||||
ToneAlarm::~ToneAlarm()
|
||||
{
|
||||
_should_run = false;
|
||||
int counter = 0;
|
||||
|
||||
while (_running && ++counter < 10) {
|
||||
px4_usleep(100000);
|
||||
}
|
||||
}
|
||||
|
||||
int ToneAlarm::init()
|
||||
{
|
||||
int ret;
|
||||
|
||||
ret = CDev::init();
|
||||
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* configure the GPIO to the idle state */
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM_IDLE);
|
||||
|
||||
#ifdef GPIO_TONE_ALARM_NEG
|
||||
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM_NEG);
|
||||
#endif
|
||||
|
||||
/* clock/power on our timer */
|
||||
modifyreg32(TONE_ALARM_CLOCK_POWER_REG, 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;
|
||||
|
||||
#ifdef rBDTR // If using an advanced timer, you need to activate the output
|
||||
rBDTR = ATIM_BDTR_MOE; // enable the main output of the advanced timer
|
||||
#endif
|
||||
|
||||
/* toggle the CC output each time the count passes 1 */
|
||||
TONE_rCCR = 1;
|
||||
|
||||
/* default the timer to a prescale value of 1; playing notes will change this */
|
||||
rPSC = 0;
|
||||
|
||||
/* make sure the timer is running */
|
||||
rCR1 = GTIM_CR1_CEN;
|
||||
|
||||
PX4_DEBUG("ready");
|
||||
|
||||
_running = true;
|
||||
work_queue(HPWORK, &_work, (worker_t)&ToneAlarm::next_trampoline, this, 0);
|
||||
return OK;
|
||||
}
|
||||
|
||||
void ToneAlarm::status()
|
||||
{
|
||||
if (_running) {
|
||||
PX4_INFO("running");
|
||||
|
||||
} else {
|
||||
PX4_INFO("stopped");
|
||||
}
|
||||
}
|
||||
|
||||
unsigned ToneAlarm::frequency_to_divisor(unsigned frequency)
|
||||
{
|
||||
float period = 0.5f / frequency;
|
||||
|
||||
// and the divisor, rounded to the nearest integer
|
||||
unsigned divisor = (period * TONE_ALARM_CLOCK) + 0.5f;
|
||||
|
||||
return divisor;
|
||||
}
|
||||
|
||||
void ToneAlarm::start_note(unsigned frequency)
|
||||
{
|
||||
// check if circuit breaker is enabled
|
||||
if (_cbrk == CBRK_UNINIT) {
|
||||
_cbrk = circuit_breaker_enabled("CBRK_BUZZER", CBRK_BUZZER_KEY);
|
||||
}
|
||||
|
||||
if (_cbrk != CBRK_OFF) { return; }
|
||||
|
||||
// compute the divisor
|
||||
unsigned divisor = frequency_to_divisor(frequency);
|
||||
|
||||
// pick the lowest prescaler value that we can use
|
||||
// (note that the effective prescale value is 1 greater)
|
||||
unsigned prescale = divisor / 65536;
|
||||
|
||||
// calculate the timer period for the selected prescaler value
|
||||
unsigned period = (divisor / (prescale + 1)) - 1;
|
||||
|
||||
rPSC = prescale; // load new prescaler
|
||||
rARR = period; // load new toggle period
|
||||
rEGR = GTIM_EGR_UG; // force a reload of the period
|
||||
rCCER |= TONE_CCER; // enable the output
|
||||
|
||||
// configure the GPIO to enable timer output
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM);
|
||||
}
|
||||
|
||||
void ToneAlarm::stop_note()
|
||||
{
|
||||
/* stop the current note */
|
||||
rCCER &= ~TONE_CCER;
|
||||
|
||||
/*
|
||||
* Make sure the GPIO is not driving the speaker.
|
||||
*/
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM_IDLE);
|
||||
}
|
||||
|
||||
void ToneAlarm::next_note()
|
||||
{
|
||||
if (!_should_run) {
|
||||
if (_tune_control_sub >= 0) {
|
||||
orb_unsubscribe(_tune_control_sub);
|
||||
}
|
||||
|
||||
_running = false;
|
||||
return;
|
||||
}
|
||||
|
||||
// subscribe to tune_control
|
||||
if (_tune_control_sub < 0) {
|
||||
_tune_control_sub = orb_subscribe(ORB_ID(tune_control));
|
||||
}
|
||||
|
||||
// do we have an inter-note gap to wait for?
|
||||
if (_silence_length > 0) {
|
||||
stop_note();
|
||||
work_queue(HPWORK, &_work, (worker_t)&ToneAlarm::next_trampoline, this, USEC2TICK(_silence_length));
|
||||
_silence_length = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
// check for updates
|
||||
bool updated = false;
|
||||
orb_check(_tune_control_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(tune_control), _tune_control_sub, &_tune);
|
||||
_play_tone = _tunes.set_control(_tune) == 0;
|
||||
}
|
||||
|
||||
unsigned frequency = 0;
|
||||
unsigned duration = 0;
|
||||
|
||||
if (_play_tone) {
|
||||
_play_tone = false;
|
||||
int parse_ret_val = _tunes.get_next_tune(frequency, duration, _silence_length);
|
||||
|
||||
if (parse_ret_val >= 0) {
|
||||
// a frequency of 0 correspond to stop_note
|
||||
if (frequency > 0) {
|
||||
// start playing the note
|
||||
start_note(frequency);
|
||||
|
||||
} else {
|
||||
stop_note();
|
||||
}
|
||||
|
||||
|
||||
if (parse_ret_val > 0) {
|
||||
// continue playing
|
||||
_play_tone = true;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
// schedule a call with the tunes max interval
|
||||
duration = _tunes.get_maximum_update_interval();
|
||||
// stop playing the last note after the duration elapsed
|
||||
stop_note();
|
||||
}
|
||||
|
||||
// and arrange a callback when the note should stop
|
||||
work_queue(HPWORK, &_work, (worker_t)&ToneAlarm::next_trampoline, this, USEC2TICK(duration));
|
||||
}
|
||||
|
||||
void ToneAlarm::next_trampoline(void *arg)
|
||||
{
|
||||
ToneAlarm *ta = (ToneAlarm *)arg;
|
||||
ta->next_note();
|
||||
}
|
||||
|
||||
/**
|
||||
* Local functions in support of the shell command.
|
||||
*/
|
||||
namespace
|
||||
{
|
||||
|
||||
ToneAlarm *g_dev;
|
||||
|
||||
} // namespace
|
||||
|
||||
void tone_alarm_usage();
|
||||
|
||||
void tone_alarm_usage()
|
||||
{
|
||||
PX4_INFO("missing command, try 'start', status, 'stop'");
|
||||
}
|
||||
|
||||
int tone_alarm_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
if (argc > 1) {
|
||||
const char *argv1 = argv[1];
|
||||
|
||||
if (!strcmp(argv1, "start")) {
|
||||
if (g_dev != nullptr) {
|
||||
PX4_ERR("already started");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
g_dev = new ToneAlarm();
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
PX4_ERR("couldn't allocate the ToneAlarm driver");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (OK != g_dev->init()) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
PX4_ERR("ToneAlarm init failed");
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(argv1, "stop")) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(argv1, "status")) {
|
||||
g_dev->status();
|
||||
return 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
tone_alarm_usage();
|
||||
return 0;
|
||||
}
|
|
@ -1,6 +1,6 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
# Copyright (C) 2015-2019 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
|
||||
|
@ -30,11 +30,14 @@
|
|||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE drivers__tone_alarm_sim
|
||||
MODULE drivers__tone_alarm
|
||||
MAIN tone_alarm
|
||||
SRCS
|
||||
tone_alarm.cpp
|
||||
ToneAlarm.cpp
|
||||
DEPENDS
|
||||
git_driverframework
|
||||
circuit_breaker
|
||||
tone_alarm_interface
|
||||
tunes
|
||||
)
|
|
@ -0,0 +1,242 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013-2019 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 ToneAlarm.cpp
|
||||
*/
|
||||
|
||||
#include "ToneAlarm.h"
|
||||
|
||||
#include <px4_time.h>
|
||||
|
||||
ToneAlarm::ToneAlarm() :
|
||||
CDev(TONE_ALARM0_DEVICE_PATH)
|
||||
{
|
||||
}
|
||||
|
||||
ToneAlarm::~ToneAlarm()
|
||||
{
|
||||
_should_run = false;
|
||||
int counter = 0;
|
||||
|
||||
while (_running && counter < 10) {
|
||||
px4_usleep(100000);
|
||||
counter++;
|
||||
}
|
||||
}
|
||||
|
||||
int ToneAlarm::init()
|
||||
{
|
||||
if (CDev::init() != OK) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
// NOTE: Implement hardware specific detail in the ToneAlarmInterface class implementation.
|
||||
ToneAlarmInterface::init();
|
||||
|
||||
_running = true;
|
||||
work_queue(HPWORK, &_work, (worker_t)&ToneAlarm::next_trampoline, this, 0);
|
||||
return OK;
|
||||
}
|
||||
|
||||
void ToneAlarm::next_note()
|
||||
{
|
||||
if (!_should_run) {
|
||||
if (_tune_control_sub >= 0) {
|
||||
orb_unsubscribe(_tune_control_sub);
|
||||
}
|
||||
|
||||
_running = false;
|
||||
return;
|
||||
}
|
||||
|
||||
// Subscribe to tune_control.
|
||||
if (_tune_control_sub < 0) {
|
||||
_tune_control_sub = orb_subscribe(ORB_ID(tune_control));
|
||||
}
|
||||
|
||||
// Check for updates
|
||||
orb_update();
|
||||
|
||||
unsigned int frequency = 0;
|
||||
unsigned int duration = 0;
|
||||
|
||||
// Does an inter-note silence occur?
|
||||
if (_silence_length > 0) {
|
||||
stop_note();
|
||||
duration = _silence_length;
|
||||
_silence_length = 0;
|
||||
|
||||
} else if (_play_tone) {
|
||||
int parse_ret_val = _tunes.get_next_tune(frequency, duration, _silence_length);
|
||||
|
||||
if (parse_ret_val > 0) {
|
||||
// Continue playing.
|
||||
_play_tone = true;
|
||||
|
||||
// A frequency of 0 corresponds to stop_note();
|
||||
if (frequency > 0) {
|
||||
// Start playing the note.
|
||||
start_note(frequency);
|
||||
}
|
||||
|
||||
} else {
|
||||
_play_tone = false;
|
||||
stop_note();
|
||||
}
|
||||
|
||||
} else {
|
||||
// Schedule a callback with the tunes max interval.
|
||||
duration = _tunes.get_maximum_update_interval();
|
||||
stop_note();
|
||||
}
|
||||
|
||||
// Schedule a callback when the note should stop.
|
||||
work_queue(HPWORK, &_work, (worker_t)&ToneAlarm::next_trampoline, this, USEC2TICK(duration));
|
||||
}
|
||||
|
||||
void ToneAlarm::next_trampoline(void *argv)
|
||||
{
|
||||
ToneAlarm *toneAlarm = (ToneAlarm *)argv;
|
||||
toneAlarm->next_note();
|
||||
}
|
||||
|
||||
void ToneAlarm::orb_update()
|
||||
{
|
||||
// Check for updates
|
||||
bool updated = false;
|
||||
orb_check(_tune_control_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(tune_control), _tune_control_sub, &_tune);
|
||||
_play_tone = _tunes.set_control(_tune) == 0;
|
||||
}
|
||||
}
|
||||
|
||||
void ToneAlarm::status()
|
||||
{
|
||||
if (_running) {
|
||||
PX4_INFO("running");
|
||||
|
||||
} else {
|
||||
PX4_INFO("stopped");
|
||||
}
|
||||
}
|
||||
|
||||
void ToneAlarm::start_note(unsigned frequency)
|
||||
{
|
||||
// Check if circuit breaker is enabled.
|
||||
if (_cbrk == CBRK_UNINIT) {
|
||||
_cbrk = circuit_breaker_enabled("CBRK_BUZZER", CBRK_BUZZER_KEY);
|
||||
}
|
||||
|
||||
if (_cbrk != CBRK_OFF) {
|
||||
return;
|
||||
}
|
||||
|
||||
// NOTE: Implement hardware specific detail in the ToneAlarmInterface class implementation.
|
||||
ToneAlarmInterface::start_note(frequency);
|
||||
}
|
||||
|
||||
void ToneAlarm::stop_note()
|
||||
{
|
||||
// NOTE: Implement hardware specific detail in the ToneAlarmInterface class implementation.
|
||||
ToneAlarmInterface::stop_note();
|
||||
}
|
||||
|
||||
|
||||
struct work_s ToneAlarm::_work = {};
|
||||
|
||||
/**
|
||||
* Local functions in support of the shell command.
|
||||
*/
|
||||
namespace
|
||||
{
|
||||
|
||||
ToneAlarm *g_dev;
|
||||
|
||||
} // namespace
|
||||
|
||||
/**
|
||||
* Tone alarm Driver 'main' command.
|
||||
* Entry point for the tone_alarm driver module.
|
||||
*/
|
||||
extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc > 1) {
|
||||
const char *argv1 = argv[1];
|
||||
|
||||
if (!strcmp(argv1, "start")) {
|
||||
if (g_dev == nullptr) {
|
||||
g_dev = new ToneAlarm();
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
PX4_ERR("could not allocate the driver.");
|
||||
}
|
||||
|
||||
if (g_dev->init() != OK) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
PX4_ERR("driver init failed.");
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_INFO("already started");
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(argv1, "stop")) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(argv1, "status")) {
|
||||
if (g_dev != nullptr) {
|
||||
g_dev->status();
|
||||
|
||||
} else {
|
||||
PX4_INFO("driver stopped");
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_INFO("missing command, try 'start', status, 'stop'");
|
||||
}
|
||||
|
||||
return 0;
|
||||
};
|
|
@ -0,0 +1,120 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2018-2019 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 ToneAlarm.h
|
||||
*
|
||||
* Low Level Driver for the PX4 audio alarm port. Subscribes to
|
||||
* tune_control and plays notes on this architecture specific timer HW.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <circuit_breaker/circuit_breaker.h>
|
||||
#include <drivers/device/device.h>
|
||||
#include <drivers/drv_tone_alarm.h>
|
||||
#include <lib/drivers/tone_alarm/ToneAlarmInterface.h>
|
||||
#include <lib/tunes/tunes.h>
|
||||
#include <px4_defines.h>
|
||||
#include <px4_workqueue.h>
|
||||
#include <string.h>
|
||||
|
||||
|
||||
#if !defined(UNUSED)
|
||||
# define UNUSED(a) ((void)(a))
|
||||
#endif
|
||||
|
||||
class ToneAlarm : public cdev::CDev
|
||||
{
|
||||
public:
|
||||
ToneAlarm();
|
||||
~ToneAlarm();
|
||||
|
||||
/**
|
||||
* @brief Initializes the character device and hardware registers.
|
||||
*/
|
||||
int init();
|
||||
|
||||
/**
|
||||
* @brief Prints the driver status to the console.
|
||||
*/
|
||||
void status();
|
||||
|
||||
protected:
|
||||
|
||||
/**
|
||||
* @brief Parses the next note out of the string and plays it.
|
||||
*/
|
||||
void next_note();
|
||||
|
||||
/**
|
||||
* @brief Trampoline for the work queue.
|
||||
* @param argv Pointer to the task startup arguments.
|
||||
*/
|
||||
static void next_trampoline(void *argv);
|
||||
|
||||
/**
|
||||
* @brief Updates the uORB topics for local subscribers.
|
||||
*/
|
||||
void orb_update();
|
||||
|
||||
/**
|
||||
* @brief Starts playing the note.
|
||||
*/
|
||||
void start_note(unsigned frequency);
|
||||
|
||||
/**
|
||||
* @brief Stops playing the current note and makes the player 'safe'.
|
||||
*/
|
||||
void stop_note();
|
||||
|
||||
volatile bool _running{false}; ///< Flag to indicate the current driver status.
|
||||
|
||||
int _cbrk{CBRK_UNINIT}; ///< If true, no audio output.
|
||||
|
||||
private:
|
||||
|
||||
volatile bool _should_run{true};
|
||||
|
||||
bool _play_tone{false};
|
||||
|
||||
unsigned int _silence_length{0}; ///< If nonzero, silence before next note.
|
||||
|
||||
int _tune_control_sub{-1};
|
||||
|
||||
tune_control_s _tune{};
|
||||
|
||||
Tunes _tunes = Tunes();
|
||||
|
||||
static work_s _work;
|
||||
};
|
|
@ -1,341 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013, 2017-2018 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* Simulated Low Level Driver for the PX4 audio alarm port. Subscribes to
|
||||
* tune_control and plays notes on this architecture specific
|
||||
* timer HW
|
||||
*/
|
||||
|
||||
#include <circuit_breaker/circuit_breaker.h>
|
||||
#include <drivers/device/device.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_tone_alarm.h>
|
||||
#include <lib/cdev/CDev.hpp>
|
||||
#include <lib/tunes/tunes.h>
|
||||
#include <px4_config.h>
|
||||
#include <px4_posix.h>
|
||||
#include <px4_workqueue.h>
|
||||
#include <uORB/topics/tune_control.h>
|
||||
|
||||
#if !defined(UNUSED)
|
||||
# define UNUSED(a) ((void)(a))
|
||||
#endif
|
||||
|
||||
#define CBRK_BUZZER_KEY 782097
|
||||
|
||||
class ToneAlarm : public cdev::CDev
|
||||
{
|
||||
public:
|
||||
ToneAlarm();
|
||||
~ToneAlarm();
|
||||
|
||||
virtual int init();
|
||||
void status();
|
||||
|
||||
enum {
|
||||
CBRK_OFF = 0,
|
||||
CBRK_ON,
|
||||
CBRK_UNINIT
|
||||
};
|
||||
|
||||
private:
|
||||
volatile bool _running;
|
||||
volatile bool _should_run;
|
||||
bool _play_tone;
|
||||
|
||||
Tunes _tunes;
|
||||
|
||||
unsigned _silence_length; // if nonzero, silence before next note
|
||||
|
||||
int _cbrk; ///< if true, no audio output
|
||||
int _tune_control_sub;
|
||||
|
||||
tune_control_s _tune;
|
||||
|
||||
static work_s _work;
|
||||
|
||||
// Convert a frequency value into a divisor for the configured timer's clock.
|
||||
//
|
||||
unsigned frequency_to_divisor(unsigned frequency);
|
||||
|
||||
// Start playing the note
|
||||
//
|
||||
void start_note(unsigned frequency);
|
||||
|
||||
// Stop playing the current note and make the player 'safe'
|
||||
//
|
||||
void stop_note();
|
||||
|
||||
// Parse the next note out of the string and play it
|
||||
//
|
||||
void next_note();
|
||||
|
||||
// work queue trampoline for next_note
|
||||
//
|
||||
static void next_trampoline(void *arg);
|
||||
|
||||
// Unused
|
||||
virtual void _measure() {}
|
||||
};
|
||||
|
||||
struct work_s ToneAlarm::_work = {};
|
||||
|
||||
/*
|
||||
* Driver 'main' command.
|
||||
*/
|
||||
extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]);
|
||||
|
||||
|
||||
ToneAlarm::ToneAlarm() :
|
||||
CDev(TONEALARM0_DEVICE_PATH),
|
||||
_running(false),
|
||||
_should_run(true),
|
||||
_play_tone(false),
|
||||
_tunes(),
|
||||
_silence_length(0),
|
||||
_cbrk(CBRK_UNINIT),
|
||||
_tune_control_sub(-1)
|
||||
{
|
||||
}
|
||||
|
||||
ToneAlarm::~ToneAlarm()
|
||||
{
|
||||
_should_run = false;
|
||||
int counter = 0;
|
||||
|
||||
while (_running && ++counter < 10) {
|
||||
px4_usleep(100000);
|
||||
}
|
||||
}
|
||||
|
||||
int ToneAlarm::init()
|
||||
{
|
||||
int ret = CDev::init();
|
||||
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
_running = true;
|
||||
work_queue(HPWORK, &_work, (worker_t)&ToneAlarm::next_trampoline, this, 0);
|
||||
return OK;
|
||||
}
|
||||
|
||||
void ToneAlarm::status()
|
||||
{
|
||||
if (_running) {
|
||||
PX4_INFO("running");
|
||||
|
||||
} else {
|
||||
PX4_INFO("stopped");
|
||||
}
|
||||
}
|
||||
|
||||
unsigned ToneAlarm::frequency_to_divisor(unsigned frequency)
|
||||
{
|
||||
const int TONE_ALARM_CLOCK = 120000000ul / 4;
|
||||
|
||||
float period = 0.5f / frequency;
|
||||
|
||||
// and the divisor, rounded to the nearest integer
|
||||
unsigned divisor = (period * TONE_ALARM_CLOCK) + 0.5f;
|
||||
|
||||
return divisor;
|
||||
}
|
||||
|
||||
void ToneAlarm::start_note(unsigned frequency)
|
||||
{
|
||||
// check if circuit breaker is enabled
|
||||
if (_cbrk == CBRK_UNINIT) {
|
||||
_cbrk = circuit_breaker_enabled("CBRK_BUZZER", CBRK_BUZZER_KEY);
|
||||
}
|
||||
|
||||
if (_cbrk != CBRK_OFF) { return; }
|
||||
|
||||
// compute the divisor
|
||||
unsigned divisor = frequency_to_divisor(frequency);
|
||||
|
||||
// pick the lowest prescaler value that we can use
|
||||
// (note that the effective prescale value is 1 greater)
|
||||
unsigned prescale = divisor / 65536;
|
||||
|
||||
// calculate the timer period for the selected prescaler value
|
||||
unsigned period = (divisor / (prescale + 1)) - 1;
|
||||
|
||||
// Silence warning of unused var
|
||||
UNUSED(period);
|
||||
PX4_DEBUG("ToneAlarm::start_note %u", period);
|
||||
}
|
||||
|
||||
void ToneAlarm::stop_note()
|
||||
{
|
||||
}
|
||||
|
||||
void ToneAlarm::next_note()
|
||||
{
|
||||
if (!_should_run) {
|
||||
if (_tune_control_sub >= 0) {
|
||||
orb_unsubscribe(_tune_control_sub);
|
||||
}
|
||||
|
||||
_running = false;
|
||||
return;
|
||||
}
|
||||
|
||||
// subscribe to tune_control
|
||||
if (_tune_control_sub < 0) {
|
||||
_tune_control_sub = orb_subscribe(ORB_ID(tune_control));
|
||||
}
|
||||
|
||||
// do we have an inter-note gap to wait for?
|
||||
if (_silence_length > 0) {
|
||||
stop_note();
|
||||
work_queue(HPWORK, &_work, (worker_t)&ToneAlarm::next_trampoline, this, USEC2TICK(_silence_length));
|
||||
_silence_length = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
// check for updates
|
||||
bool updated = false;
|
||||
orb_check(_tune_control_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(tune_control), _tune_control_sub, &_tune);
|
||||
}
|
||||
|
||||
unsigned frequency = 0;
|
||||
unsigned duration = 0;
|
||||
|
||||
if (_play_tone) {
|
||||
_play_tone = false;
|
||||
int parse_ret_val = _tunes.get_next_tune(frequency, duration, _silence_length);
|
||||
|
||||
if (parse_ret_val >= 0) {
|
||||
// a frequency of 0 correspond to stop_note
|
||||
if (frequency > 0) {
|
||||
// start playing the note
|
||||
start_note(frequency);
|
||||
|
||||
} else {
|
||||
stop_note();
|
||||
}
|
||||
|
||||
|
||||
if (parse_ret_val > 0) {
|
||||
// continue playing
|
||||
_play_tone = true;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
// schedule a call with the tunes max interval
|
||||
duration = _tunes.get_maximum_update_interval();
|
||||
// stop playing the last note after the duration elapsed
|
||||
stop_note();
|
||||
}
|
||||
|
||||
// and arrange a callback when the note should stop
|
||||
work_queue(HPWORK, &_work, (worker_t)&ToneAlarm::next_trampoline, this, USEC2TICK(duration));
|
||||
}
|
||||
|
||||
void ToneAlarm::next_trampoline(void *arg)
|
||||
{
|
||||
ToneAlarm *ta = (ToneAlarm *)arg;
|
||||
ta->next_note();
|
||||
}
|
||||
|
||||
/**
|
||||
* Local functions in support of the shell command.
|
||||
*/
|
||||
namespace
|
||||
{
|
||||
|
||||
ToneAlarm *g_dev;
|
||||
|
||||
} // namespace
|
||||
|
||||
void tone_alarm_usage();
|
||||
|
||||
void tone_alarm_usage()
|
||||
{
|
||||
PX4_INFO("missing command, try 'start', status, 'stop'");
|
||||
}
|
||||
|
||||
int tone_alarm_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
if (argc > 1) {
|
||||
const char *argv1 = argv[1];
|
||||
|
||||
if (!strcmp(argv1, "start")) {
|
||||
if (g_dev != nullptr) {
|
||||
PX4_ERR("already started");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
g_dev = new ToneAlarm();
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
PX4_ERR("couldn't allocate the ToneAlarm driver");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (OK != g_dev->init()) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
PX4_ERR("ToneAlarm init failed");
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(argv1, "stop")) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(argv1, "status")) {
|
||||
g_dev->status();
|
||||
return 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
tone_alarm_usage();
|
||||
return 0;
|
||||
}
|
|
@ -39,3 +39,4 @@ add_subdirectory(led)
|
|||
add_subdirectory(linux_gpio)
|
||||
add_subdirectory(magnetometer)
|
||||
add_subdirectory(smbus)
|
||||
add_subdirectory(tone_alarm)
|
||||
|
|
|
@ -0,0 +1,34 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2015-2019 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
# ToneAlarmInterface Library - Intentionally Blank
|
|
@ -0,0 +1,54 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013-2019 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 ToneAlarmInterface.cpp
|
||||
*/
|
||||
|
||||
namespace ToneAlarmInterface
|
||||
{
|
||||
/**
|
||||
* @brief Activates/configures the hardware registers.
|
||||
*/
|
||||
void init();
|
||||
|
||||
/**
|
||||
* @brief Starts playing the note.
|
||||
*/
|
||||
void start_note(unsigned frequency);
|
||||
|
||||
/**
|
||||
* @brief Stops playing the current note and makes the player 'safe'.
|
||||
*/
|
||||
void stop_note();
|
||||
}; // ToneAlarmInterface
|
|
@ -1696,7 +1696,7 @@ MavlinkReceiver::handle_message_play_tune(mavlink_message_t *msg)
|
|||
play_tune.target_component == 0)) {
|
||||
|
||||
if (*tune == 'M') {
|
||||
int fd = px4_open(TONEALARM0_DEVICE_PATH, PX4_F_WRONLY);
|
||||
int fd = px4_open(TONE_ALARM0_DEVICE_PATH, PX4_F_WRONLY);
|
||||
|
||||
if (fd >= 0) {
|
||||
px4_write(fd, tune, strlen(tune) + 1);
|
||||
|
|
|
@ -124,10 +124,10 @@ int test_tone(int argc, char *argv[])
|
|||
int fd, result;
|
||||
unsigned long tone;
|
||||
|
||||
fd = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY);
|
||||
fd = px4_open(TONE_ALARM0_DEVICE_PATH, O_WRONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
printf("failed opening " TONEALARM0_DEVICE_PATH "\n");
|
||||
printf("failed opening " TONE_ALARM0_DEVICE_PATH "\n");
|
||||
goto out;
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue