actuators_dualpwm_arch.c

Go to the documentation of this file.

/*
 * Copyright (C) 2010 The Paparazzi Team
 *
 * This file is part of Paparazzi.
 *
 * Paparazzi 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, or (at your option)
 * any later version.
 *
 * Paparazzi 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.
 *
 * You should have received a copy of the GNU General Public License
 * along with Paparazzi; see the file COPYING.  If not, write to
 * the Free Software Foundation, 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 */
 
//VALID TIMERS IS TIM5 ON THE LISA/M
 
#include "subsystems/actuators/actuators_shared_arch.h"
#include "subsystems/actuators/actuators_dualpwm_arch.h"
#include "subsystems/actuators/actuators_dualpwm.h"
 
#include <libopencm3/stm32/gpio.h>
#include <libopencm3/stm32/rcc.h>
#include <libopencm3/stm32/timer.h>
#include <libopencm3/cm3/nvic.h>
 
#include "mcu_periph/gpio_arch.h"
 
uint32_t ratio_4ms, ratio_16ms;
 
 
uint32_t actuators_dualpwm_values[ACTUATORS_DUALPWM_NB];
 
void actuators_dualpwm_arch_init(void)
{
 
  /*-----------------------------------
   * Configure timer peripheral clocks
   *-----------------------------------*/
#if PWM_USE_TIM1
  rcc_periph_clock_enable(RCC_TIM1);
#endif
#if PWM_USE_TIM2
  rcc_periph_clock_enable(RCC_TIM2);
#endif
#if PWM_USE_TIM3
  rcc_periph_clock_enable(RCC_TIM3);
#endif
#if PWM_USE_TIM4
  rcc_periph_clock_enable(RCC_TIM4);
#endif
#if PWM_USE_TIM5
  rcc_periph_clock_enable(RCC_TIM5);
#endif
#if PWM_USE_TIM8
  rcc_periph_clock_enable(RCC_TIM8);
#endif
#if PWM_USE_TIM9
  rcc_periph_clock_enable(RCC_TIM9);
#endif
#if PWM_USE_TIM12
  rcc_periph_clock_enable(RCC_TIM12);
#endif
 
  /*----------------
   * Configure GPIO
   *----------------*/
#ifdef DUAL_PWM_SERVO_5
  gpio_setup_pin_af(DUAL_PWM_SERVO_5_GPIO, DUAL_PWM_SERVO_5_PIN, DUAL_PWM_SERVO_5_AF, TRUE);
#endif
#ifdef DUAL_PWM_SERVO_6
  gpio_setup_pin_af(DUAL_PWM_SERVO_6_GPIO, DUAL_PWM_SERVO_6_PIN, DUAL_PWM_SERVO_6_AF, TRUE);
#endif
 
#if DUAL_PWM_USE_TIM5
  set_servo_timer(TIM5, TIM5_SERVO_HZ, PWM_TIM5_CHAN_MASK);
 
  nvic_set_priority(NVIC_TIM5_IRQ, 2);
  nvic_enable_irq(NVIC_TIM5_IRQ);
  timer_enable_irq(TIM5, TIM_DIER_CC1IE);
#endif
 
  //calculation the values to put into the timer registers to generate pulses every 4ms and 16ms.
  ratio_4ms = (ONE_MHZ_CLK / 250) - 1;
  ratio_16ms = (ONE_MHZ_CLK / 62.5) - 1;
 
}
 
 
#if DUAL_PWM_USE_TIM5
void tim5_isr(void)
{
 
  dual_pwm_isr();
}
#endif
 
 
 
 
void clear_timer_flag(void)
{
 
#if DUAL_PWM_USE_TIM5
  timer_clear_flag(TIM5, TIM_SR_CC1IF);
#endif
}
 
 
void set_dual_pwm_timer_s_period(uint32_t period)
{
 
#if DUAL_PWM_USE_TIM5
  timer_set_period(TIM5, period);
#endif
}
 
 
void set_dual_pwm_timer_s_oc(uint32_t oc_value, uint32_t oc_value2)
{
 
#if DUAL_PWM_USE_TIM5
  timer_set_oc_value(DUAL_PWM_SERVO_5_TIMER, DUAL_PWM_SERVO_5_OC, oc_value);
  timer_set_oc_value(DUAL_PWM_SERVO_6_TIMER, DUAL_PWM_SERVO_6_OC, oc_value2);
#endif
}
 
 
 
 
void dual_pwm_isr(void)
{
 
  static int num_pulse = 0;  //status of the timer. Are we controling the first or the second servo
 
  clear_timer_flag();
 
  if (num_pulse == 1) {
 
    set_dual_pwm_timer_s_period(ratio_16ms);
    set_dual_pwm_timer_s_oc(actuators_dualpwm_values[DUAL_PWM_SERVO_5_P1],actuators_dualpwm_values[DUAL_PWM_SERVO_5_P2]);
 
    num_pulse = 0;
  } else {
 
    set_dual_pwm_timer_s_period(ratio_4ms);
    set_dual_pwm_timer_s_oc(actuators_dualpwm_values[DUAL_PWM_SERVO_6_P1],actuators_dualpwm_values[DUAL_PWM_SERVO_6_P2]);
 
    num_pulse = 1;
  }
}
 
 
void actuators_dualpwm_commit(void)
{
 
  //we don't need to commit the values into this function as far as it's done in the interuption
  //(wich is called every 4ms and 16ms alternatively (twice every 20ms))
 
}