357 lines
11 KiB
C++
357 lines
11 KiB
C++
/***************************************************************************
|
|
* Copyright (C) 2014 by Terraneo Federico *
|
|
* *
|
|
* This program is free software; you can redistribute it and/or modify *
|
|
* it under the terms of the GNU General Public License as published by *
|
|
* the Free Software Foundation; either version 2 of the License, or *
|
|
* (at your option) any later version. *
|
|
* *
|
|
* This program is distributed in the hope that it will be useful, *
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
|
* GNU General Public License for more details. *
|
|
* *
|
|
* As a special exception, if other files instantiate templates or use *
|
|
* macros or inline functions from this file, or you compile this file *
|
|
* and link it with other works to produce a work based on this file, *
|
|
* this file does not by itself cause the resulting work to be covered *
|
|
* by the GNU General Public License. However the source code for this *
|
|
* file must still be made available in accordance with the GNU General *
|
|
* Public License. This exception does not invalidate any other reasons *
|
|
* why a work based on this file might be covered by the GNU General *
|
|
* Public License. *
|
|
* *
|
|
* You should have received a copy of the GNU General Public License *
|
|
* along with this program; if not, see <http://www.gnu.org/licenses/> *
|
|
***************************************************************************/
|
|
|
|
#include "servo_stm32.h"
|
|
#include "kernel/scheduler/scheduler.h"
|
|
#include <algorithm>
|
|
#include <cstdio>
|
|
#include <cmath>
|
|
|
|
using namespace std;
|
|
using namespace miosix;
|
|
|
|
typedef Gpio<GPIOB_BASE,6> servo1out;
|
|
typedef Gpio<GPIOB_BASE,7> servo2out;
|
|
typedef Gpio<GPIOB_BASE,8> servo3out;
|
|
typedef Gpio<GPIOB_BASE,9> servo4out;
|
|
static Thread *waiting=0;
|
|
|
|
/**
|
|
* Timer 4 interrupt handler actual implementation
|
|
*/
|
|
void __attribute__((used)) tim4impl()
|
|
{
|
|
TIM4->SR=0; //Clear interrupt flag
|
|
if(waiting==0) return;
|
|
waiting->IRQwakeup();
|
|
if(waiting->IRQgetPriority()>Thread::IRQgetCurrentThread()->IRQgetPriority())
|
|
Scheduler::IRQfindNextThread();
|
|
waiting=0;
|
|
}
|
|
|
|
/**
|
|
* Timer 4 interrupt handler
|
|
*/
|
|
void __attribute__((naked)) TIM4_IRQHandler()
|
|
{
|
|
saveContext();
|
|
asm volatile("bl _Z8tim4implv");
|
|
restoreContext();
|
|
}
|
|
|
|
namespace miosix {
|
|
|
|
/* TODO: find a better place for this */
|
|
unsigned int divideRoundToNearest(unsigned int a, unsigned int b)
|
|
{
|
|
const unsigned int quot=2*a/b;
|
|
return quot/2 + (quot & 1);
|
|
}
|
|
|
|
//
|
|
// class SynchronizedServo
|
|
//
|
|
|
|
SynchronizedServo& SynchronizedServo::instance()
|
|
{
|
|
static SynchronizedServo singleton;
|
|
return singleton;
|
|
}
|
|
|
|
void SynchronizedServo::enable(int channel)
|
|
{
|
|
Lock<FastMutex> l(mutex);
|
|
if(status!=STOPPED) return; // If timer enabled ignore the call
|
|
|
|
{
|
|
FastInterruptDisableLock dLock;
|
|
// Calling the mode() function on a GPIO is subject to race conditions
|
|
// between threads on the STM32, so we disable interrupts
|
|
switch(channel)
|
|
{
|
|
case 0:
|
|
TIM4->CCMR1 |= TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1PE;
|
|
TIM4->CCER |= TIM_CCER_CC1E;
|
|
#ifndef _ARCH_CORTEXM3_STM32 //Only stm32f2 and stm32f4 have it
|
|
servo1out::alternateFunction(2);
|
|
#endif //_ARCH_CORTEXM3_STM32
|
|
servo1out::mode(Mode::ALTERNATE);
|
|
break;
|
|
case 1:
|
|
TIM4->CCMR1 |= TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2PE;
|
|
TIM4->CCER |= TIM_CCER_CC2E;
|
|
#ifndef _ARCH_CORTEXM3_STM32 //Only stm32f2 and stm32f4 have it
|
|
servo2out::alternateFunction(2);
|
|
#endif //_ARCH_CORTEXM3_STM32
|
|
servo2out::mode(Mode::ALTERNATE);
|
|
break;
|
|
case 2:
|
|
TIM4->CCMR2 |= TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3PE;
|
|
TIM4->CCER |= TIM_CCER_CC3E;
|
|
#ifndef _ARCH_CORTEXM3_STM32 //Only stm32f2 and stm32f4 have it
|
|
servo3out::alternateFunction(2);
|
|
#endif //_ARCH_CORTEXM3_STM32
|
|
servo3out::mode(Mode::ALTERNATE);
|
|
break;
|
|
case 3:
|
|
TIM4->CCMR2 |= TIM_CCMR2_OC4M_2 | TIM_CCMR2_OC4M_1 | TIM_CCMR2_OC4PE;
|
|
TIM4->CCER |= TIM_CCER_CC4E;
|
|
#ifndef _ARCH_CORTEXM3_STM32 //Only stm32f2 and stm32f4 have it
|
|
servo4out::alternateFunction(2);
|
|
#endif //_ARCH_CORTEXM3_STM32
|
|
servo4out::mode(Mode::ALTERNATE);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
void SynchronizedServo::disable(int channel)
|
|
{
|
|
Lock<FastMutex> l(mutex);
|
|
if(status!=STOPPED) return; // If timer enabled ignore the call
|
|
|
|
{
|
|
FastInterruptDisableLock dLock;
|
|
// Calling the mode() function on a GPIO is subject to race conditions
|
|
// between threads on the STM32, so we disable interrupts
|
|
switch(channel)
|
|
{
|
|
case 0:
|
|
servo1out::mode(Mode::INPUT);
|
|
TIM4->CCER &= ~TIM_CCER_CC1E;
|
|
break;
|
|
case 1:
|
|
servo2out::mode(Mode::INPUT);
|
|
TIM4->CCER &= ~TIM_CCER_CC2E;
|
|
break;
|
|
case 2:
|
|
servo3out::mode(Mode::INPUT);
|
|
TIM4->CCER &= ~TIM_CCER_CC3E;
|
|
break;
|
|
case 3:
|
|
servo4out::mode(Mode::INPUT);
|
|
TIM4->CCER &= ~TIM_CCER_CC4E;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
void SynchronizedServo::setPosition(int channel, float value)
|
|
{
|
|
Lock<FastMutex> l(mutex);
|
|
if(status!=STARTED) return; // If timer disabled ignore the call
|
|
|
|
value=min(1.0f,max(0.0f,value));
|
|
switch(channel)
|
|
{
|
|
case 0:
|
|
TIM4->CCR1=a*value+b;
|
|
break;
|
|
case 1:
|
|
TIM4->CCR2=a*value+b;
|
|
break;
|
|
case 2:
|
|
TIM4->CCR3=a*value+b;
|
|
break;
|
|
case 3:
|
|
TIM4->CCR4=a*value+b;
|
|
break;
|
|
}
|
|
}
|
|
|
|
float SynchronizedServo::getPosition(int channel)
|
|
{
|
|
switch(channel)
|
|
{
|
|
case 0:
|
|
return TIM4->CCR1==0 ? NAN : TIM4->CCR1/a-b;
|
|
case 1:
|
|
return TIM4->CCR2==0 ? NAN : TIM4->CCR2/a-b;
|
|
case 2:
|
|
return TIM4->CCR3==0 ? NAN : TIM4->CCR3/a-b;
|
|
case 3:
|
|
return TIM4->CCR4==0 ? NAN : TIM4->CCR4/a-b;
|
|
default:
|
|
return NAN;
|
|
}
|
|
}
|
|
|
|
void SynchronizedServo::start()
|
|
{
|
|
Lock<FastMutex> l(mutex);
|
|
if(status!=STOPPED) return; // If timer enabled ignore the call
|
|
|
|
// While status is starting neither memeber function callable with timer
|
|
// started nor stopped are allowed
|
|
status=STARTED;
|
|
TIM4->CNT=0;
|
|
TIM4->EGR=TIM_EGR_UG;
|
|
TIM4->CR1=TIM_CR1_CEN;
|
|
}
|
|
|
|
void SynchronizedServo::stop()
|
|
{
|
|
Lock<FastMutex> l(mutex);
|
|
if(status!=STARTED) return; // If timer disabled ignore the call
|
|
|
|
status=STOPPED;
|
|
// Stopping the timer is a bit difficult because we don't want to
|
|
// accidentally create glitches on the outputs which may turn the servos
|
|
// to random positions. So, we set all PWM outputs to 0, then wait until the
|
|
// end of the period, and then disable the timer
|
|
TIM4->CCR1=0;
|
|
TIM4->CCR2=0;
|
|
TIM4->CCR3=0;
|
|
TIM4->CCR4=0;
|
|
{
|
|
FastInterruptDisableLock dLock;
|
|
// Wakeup an eventual thread waiting on waitForCycleBegin()
|
|
if(waiting) waiting->IRQwakeup();
|
|
IRQwaitForTimerOverflow(dLock);
|
|
}
|
|
TIM4->CR1=0;
|
|
}
|
|
|
|
bool SynchronizedServo::waitForCycleBegin()
|
|
{
|
|
// No need to lock the mutex because disabling interrupts is enough to avoid
|
|
// race conditions. Also, locking the mutex here would prevent other threads
|
|
// from calling other member functions of this class
|
|
FastInterruptDisableLock dLock;
|
|
if(status!=STARTED) return true;
|
|
IRQwaitForTimerOverflow(dLock);
|
|
return status!=STARTED;
|
|
}
|
|
|
|
void SynchronizedServo::setFrequency(unsigned int frequency)
|
|
{
|
|
Lock<FastMutex> l(mutex);
|
|
if(status!=STOPPED) return; // If timer enabled ignore the call
|
|
|
|
TIM4->PSC=divideRoundToNearest(getPrescalerInputFrequency(),frequency*65536)-1;
|
|
precomputeCoefficients();
|
|
}
|
|
|
|
float SynchronizedServo::getFrequency() const
|
|
{
|
|
float prescalerFreq=getPrescalerInputFrequency();
|
|
return prescalerFreq/((TIM4->PSC+1)*65536);
|
|
}
|
|
|
|
void SynchronizedServo::setMinPulseWidth(float minPulse)
|
|
{
|
|
Lock<FastMutex> l(mutex);
|
|
if(status!=STOPPED) return; // If timer enabled ignore the call
|
|
|
|
minWidth=1e-6f*min(1300.0f,max(500.0f,minPulse));
|
|
precomputeCoefficients();
|
|
}
|
|
|
|
void SynchronizedServo::setMaxPulseWidth(float maxPulse)
|
|
{
|
|
Lock<FastMutex> l(mutex);
|
|
if(status!=STOPPED) return; // If timer enabled ignore the call
|
|
|
|
maxWidth=1e-6f*min(2500.0f,max(1700.0f,maxPulse));
|
|
precomputeCoefficients();
|
|
}
|
|
|
|
SynchronizedServo::SynchronizedServo() : status(STOPPED)
|
|
{
|
|
{
|
|
FastInterruptDisableLock dLock;
|
|
// The RCC register should be written with interrupts disabled to
|
|
// prevent race conditions with other threads.
|
|
RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;
|
|
RCC_SYNC();
|
|
}
|
|
|
|
// Configure timer
|
|
TIM4->CR1=0;
|
|
TIM4->ARR=0xffff;
|
|
TIM4->CCR1=0;
|
|
TIM4->CCR2=0;
|
|
TIM4->CCR3=0;
|
|
TIM4->CCR4=0;
|
|
// Configure interrupt on timer overflow
|
|
TIM4->DIER=TIM_DIER_UIE;
|
|
NVIC_SetPriority(TIM4_IRQn,13); //Low priority for timer IRQ
|
|
NVIC_EnableIRQ(TIM4_IRQn);
|
|
// Set default parameters
|
|
setFrequency(50);
|
|
setMinPulseWidth(1000);
|
|
setMaxPulseWidth(2000);
|
|
}
|
|
|
|
void SynchronizedServo::precomputeCoefficients()
|
|
{
|
|
float realPeriod=1.0f/getFrequency();
|
|
a=65536.0f*(maxWidth-minWidth)/realPeriod;
|
|
b=65536.0f*minWidth/realPeriod;
|
|
}
|
|
|
|
unsigned int SynchronizedServo::getPrescalerInputFrequency()
|
|
{
|
|
// The global variable SystemCoreClock from ARM's CMSIS allows to know
|
|
// the CPU frequency.
|
|
unsigned int freq=SystemCoreClock;
|
|
|
|
//The position of the PPRE1 bit in RCC->CFGR is different in some stm32
|
|
#ifdef _ARCH_CORTEXM3_STM32
|
|
const unsigned int ppre1=8;
|
|
#else //stm32f2 and f4
|
|
const unsigned int ppre1=10;
|
|
#endif
|
|
|
|
// The timer frequency may however be a submultiple of the CPU frequency,
|
|
// due to the bus at whch the periheral is connected being slower. The
|
|
// RCC->CFGR register tells us how slower the APB1 bus is running.
|
|
// This formula takes into account that if the APB1 clock is divided by a
|
|
// factor of two or greater, the timer is clocked at twice the bus
|
|
// interface. After this, the freq variable contains the frequency in Hz
|
|
// at which the timer prescaler is clocked.
|
|
if(RCC->CFGR & RCC_CFGR_PPRE1_2) freq/=1<<((RCC->CFGR>>ppre1) & 0x3);
|
|
|
|
return freq;
|
|
}
|
|
|
|
void SynchronizedServo::IRQwaitForTimerOverflow(FastInterruptDisableLock& dLock)
|
|
{
|
|
waiting=Thread::IRQgetCurrentThread();
|
|
do {
|
|
Thread::IRQwait();
|
|
{
|
|
FastInterruptEnableLock eLock(dLock);
|
|
Thread::yield();
|
|
}
|
|
} while(waiting);
|
|
}
|
|
|
|
} //namespace miosix
|
|
|
|
|