mark griffin servo driving code

2026-05-31 12:23:23 +08:00 · 2008-12-04 19:39:01 +00:00
parent b137c87cb8
commit e2a59d60c3
3 changed files with 152 additions and 0 deletions
@@ -0,0 +1,61 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Mark Griffin
+ *
+ * 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. 
+ *
+ */
+/** \file servos_ppm_out.c
+ *  \Efficient driving of MAT0.1 (SERVO_CLOCK_PIN) using TIMER0 to produce PPM 
+ *  \ for a R/C receiver which has a microcontroller to drive the servos
+ *  \(not a 4015 or 4017 decade counter chip).
+ */
+#include "actuators.h"
+#include "paparazzi.h"
+#include "airframe.h"
+
+uint8_t servos_PPM_idx;
+uint8_t ppm_pulse;
+uint32_t servos_delay = SERVO_REFRESH_TICS;
+
+#define START_TIMEOUT 0xFFFF;
+
+void actuators_init ( void ) {
+  /* select ppm output pin as MAT0.1 output */
+  SERVO_CLOCK_PINSEL |= SERVO_CLOCK_PINSEL_VAL << SERVO_CLOCK_PINSEL_BIT;
+
+  /* enable match 1 interrupt */
+  T0MCR |= TMCR_MR1_I;
+  
+  /* lower clock         */
+  T0EMR &= ~TEMR_EM1;	
+  /* set high on match 1 */
+  T0EMR |= TEMR_EMC11;
+
+  /* set first pulse in a while */
+  T0MR1 = START_TIMEOUT;
+  servos_PPM_idx = _PPM_NB_CHANNELS;
+  /* Set all servos to their midpoints */
+  /* compulsory for unused servos  */
+  uint8_t i;
+  for( i=0 ; i < _PPM_NB_CHANNELS ; i++ )
+    servos_values[i] = SERVOS_TICS_OF_USEC(1500);
+}
+uint16_t servos_values[_PPM_NB_CHANNELS];
+
@@ -0,0 +1,84 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Mark Griffin
+ *
+ * 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. 
+ *
+ */
+/** \file servos_ppm_out.h
+ *  \Efficient driving of the MAT0.1 pin (SERVO_CLOCK_PIN) using TIMER0 to produce 
+ *  \PPM for a R/C receiver which has a microcontroller to drive the servos
+ *  \(not a 4015 or 4017 decade counter chip).
+ */
+#ifndef SERVOS_PPM_HW_H
+#define SERVOS_PPM_HW_H
+
+#include "std.h"
+#include "LPC21xx.h"
+#include "sys_time.h"
+
+#include CONFIG
+
+#define SERVOS_TICS_OF_USEC(s) SYS_TICS_OF_USEC(s)
+#define ChopServo(x,a,b) Chop(x, a, b)
+
+#define _PPM_NB_CHANNELS 8
+extern uint16_t servos_values[_PPM_NB_CHANNELS];
+#define Actuator(i) servos_values[i]
+
+#define ActuatorsCommit() {}
+extern uint8_t servos_PPM_idx;
+extern uint32_t servos_delay;
+extern uint8_t ppm_pulse; /* 1=start of pulse, 0=end of pulse */
+
+/* define the ppm frame rate. 22msec is typical for JR/Graupner */
+#define SERVO_REFRESH_TICS SERVOS_TICS_OF_USEC(22000)
+/* define the ppm pulse width. 550usec is typical for JR/Graupner */
+#define PPM_WIDTH SERVOS_TICS_OF_USEC(550)
+
+#define ServosPPMMat_ISR() {				\
+  if (servos_PPM_idx == 0) {				\
+    servos_delay = SERVO_REFRESH_TICS;			\
+  }							\
+  if (servos_PPM_idx < _PPM_NB_CHANNELS ) {		\
+    if (ppm_pulse) { 					\
+	T0MR1 += PPM_WIDTH;				\
+        servos_delay -= PPM_WIDTH; 			\
+    } else {						\
+	T0MR1 += servos_values[servos_PPM_idx] - PPM_WIDTH;		\
+    	servos_delay -= servos_values[servos_PPM_idx] - PPM_WIDTH;	\
+    	servos_PPM_idx++;					\
+    }                                                   \
+  } else { /* servos_PPM_idx=_PPM_NB_CHANNELS */ \
+    if (ppm_pulse) { 					\
+	T0MR1 += PPM_WIDTH;				\
+        servos_delay -= PPM_WIDTH; 			\
+    } else {						\
+    servos_PPM_idx = 0;				\
+    T0MR1 += servos_delay - PPM_WIDTH;				\
+    }                                                   \
+  }							\
+  if (!ppm_pulse) {                                     \
+  /* lower clock pin */					\
+  T0EMR &= ~TEMR_EM1;                                   \
+  }					                \
+  /* toggle ppm_pulse flag */				\
+  ppm_pulse ^= 1;                                       \
+}
+#endif /* SERVOS_PPM_HW_H */
@@ -52,6 +52,13 @@ void TIMER0_ISR ( void ) {
      T0IR = TIR_MR1I; 
    }
 #endif
+#ifdef SERVOS_PPM_MAT
+    if (T0IR&TIR_MR1I) {
+      ServosPPMMat_ISR();
+      /* clear interrupt */
+      T0IR = TIR_MR1I; 
+    }
+#endif
 #ifdef MB_SCALE
    if (T0IR&TIR_CR3I) {
       MB_SCALE_ICP_ISR();