[actuators] separate files for asctec_v2

conf/airframes/ENAC/quadrotor/hen1.xml

@@ -56,7 +56,7 @@
     <servo name="LEFT"   no="3"    min="0" neutral="20" max="255"/>
   </servos-->
 
-  <servos driver="Asctec">
+  <servos driver="Asctec_v2">
     <servo name="FRONT"  no="0"    min="0" neutral="1" max="200"/>
     <servo name="BACK"   no="1"    min="0" neutral="1" max="200"/>
     <servo name="LEFT"   no="2"    min="0" neutral="1" max="200"/>

conf/airframes/examples/lisa_asctec.xml

@@ -4,7 +4,7 @@
 
 <airframe name="lisa_asctec">
 
-  <servos driver="Asctec">
+  <servos driver="Asctec_v2">
     <servo name="FRONT"   no="0" min="0" neutral="3" max="200"/>
     <servo name="BACK"    no="1" min="0" neutral="3" max="200"/>
     <servo name="LEFT"    no="2" min="0" neutral="3" max="200"/>
@@ -72,9 +72,9 @@
     <define name="MAG_Y_SENS" value="1." integer="16"/>
     <define name="MAG_Z_SENS" value="1." integer="16"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="0. " unit="deg"/>
-    <define name="BODY_TO_IMU_THETA" value="0. " unit="deg"/>
-    <define name="BODY_TO_IMU_PSI"   value="45. " unit="deg"/>
+    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="45." unit="deg"/>
 
   </section>
 
@@ -198,7 +198,7 @@
       <define name="RADIO_CONTROL_SPEKTRUM_SIGNS" value="\{1,1,-1,1,-1,-1,-1,1,1,1,1,1\}"/>
       <define name="USE_I2C_ACTUATORS_REBOOT_HACK"/>
     </target>
-    <target name="sim" board="pc">
+    <target name="nps" board="pc">
       <subsystem name="fdm"           type="jsbsim"/>
       <subsystem name="radio_control" type="ppm"/>
       <subsystem name="motor_mixing"/>

conf/firmwares/subsystems/shared/actuators_asctec_v2.makefile

@@ -2,10 +2,24 @@
 #
 # required xml configuration:
 #
-#  servo section with driver="Asctec"
+#  servo section with driver="Asctec_v2"
 #  command_laws section to map motor_mixing commands to servos
 #
 
-include $(CFG_SHARED)/actuators_asctec.makefile
+$(TARGET).CFLAGS += -DACTUATORS
+ap.srcs += subsystems/actuators/actuators_asctec_v2.c
 
-ap.CFLAGS += -DACTUATORS_ASCTEC_V2_PROTOCOL
+ifeq ($(ARCH), lpc21)
+ap.CFLAGS += -DACTUATORS_ASCTEC_V2_DEVICE=i2c0
+ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150
+endif
+
+ifeq ($(ARCH), stm32)
+ap.CFLAGS += -DACTUATORS_ASCTEC_V2_DEVICE=i2c1
+ap.CFLAGS += -DUSE_I2C1
+endif
+
+
+# Simulator
+nps.srcs += subsystems/actuators/actuators_asctec_v2.c
+nps.CFLAGS += -DUSE_I2C0 -DACTUATORS_ASCTEC_DEVICE=i2c0

sw/airborne/subsystems/actuators/actuators_asctec.c

@@ -35,6 +35,8 @@
 #define ASCTEC_MIN_THROTTLE 0
 #define ASCTEC_MAX_THROTTLE 200
 
+#define ACTUATORS_ASCTEC_SLAVE_ADDR 0x02
+
 struct ActuatorsAsctec actuators_asctec;
 
 uint32_t actuators_delay_time;
@@ -46,12 +48,8 @@ void actuators_asctec_init(void) {
   actuators_asctec.new_addr = FRONT;
   actuators_asctec.i2c_trans.status = I2CTransSuccess;
   actuators_asctec.i2c_trans.type = I2CTransTx;
-  actuators_asctec.i2c_trans.slave_addr = 0x02;
-#ifdef ACTUATORS_ASCTEC_V2_PROTOCOL
-  actuators_asctec.i2c_trans.len_w = 5;
-#else
+  actuators_asctec.i2c_trans.slave_addr = ACTUATORS_ASCTEC_SLAVE_ADDR;
   actuators_asctec.i2c_trans.len_w = 4;
-#endif
   actuators_asctec.nb_err = 0;
 
 #if defined ACTUATORS_START_DELAY && ! defined SITL
@@ -64,7 +62,6 @@ void actuators_asctec_init(void) {
 
 }
 
-#ifndef ACTUATORS_ASCTEC_V2_PROTOCOL
 void actuators_asctec_set(bool_t motors_on) {
 #if defined ACTUATORS_START_DELAY && ! defined SITL
   if (!actuators_delay_done) {
@@ -97,7 +94,7 @@ void actuators_asctec_set(bool_t motors_on) {
   Bound(actuators_asctec.cmds[ROLL], ASCTEC_MIN_CMD, ASCTEC_MAX_CMD);
   Bound(actuators_asctec.cmds[YAW],  ASCTEC_MIN_CMD, ASCTEC_MAX_CMD);
   if (motors_on) {
-    Bound(actuators_asctec.cmds[THRUST],  ASCTEC_MIN_THROTTLE + 1, ASCTEC_MAX_THROTTLE);
+    Bound(actuators_asctec.cmds[THRUST], ASCTEC_MIN_THROTTLE + 1, ASCTEC_MAX_THROTTLE);
   }
   else
     actuators_asctec.cmds[THRUST] = 0;
@@ -120,7 +117,8 @@ void actuators_asctec_set(bool_t motors_on) {
     actuators_asctec.i2c_trans.buf[0] = 250;
     actuators_asctec.i2c_trans.buf[1] = actuators_asctec.cur_addr;
     actuators_asctec.i2c_trans.buf[2] = actuators_asctec.new_addr;
-    actuators_asctec.i2c_trans.buf[3] = 230 + actuators_asctec.cur_addr + actuators_asctec.new_addr;
+    actuators_asctec.i2c_trans.buf[3] = 230 + actuators_asctec.cur_addr +
+                                        actuators_asctec.new_addr;
     actuators_asctec.cur_addr = actuators_asctec.new_addr;
     break;
   case NONE:
@@ -134,87 +132,7 @@ void actuators_asctec_set(bool_t motors_on) {
   }
   actuators_asctec.cmd = NONE;
 
-  actuators_asctec.i2c_trans.type = I2CTransTx;   // Can be reset I2C driver
-  i2c_submit(&ACTUATORS_ASCTEC_DEVICE, &actuators_asctec.i2c_trans);
+  I2CTransmit(ACTUATORS_ASCTEC_DEVICE, actuators_asctec.i2c_trans,
+              ACTUATORS_ASCTEC_SLAVE_ADDR, 4);
 
 }
-#else /* ! ACTUATORS_ASCTEC_V2_PROTOCOL */
-void actuators_asctec_set(bool_t motors_on) {
-#if defined ACTUATORS_START_DELAY && ! defined SITL
-  if (!actuators_delay_done) {
-    if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) {
-#ifdef USE_I2C_ACTUATORS_REBOOT_HACK
-      //Lisa-L with Asctech v2 motors only start after reflashing when a bus error was sensed on stm32-i2c.
-      //multiple re-init solves the problem.
-      i2c1_init();
-#endif
-      return;
-    }
-    else actuators_delay_done = TRUE;
-  }
-#endif
-
-  switch (actuators_asctec.i2c_trans.status) {
-    case I2CTransFailed:
-      actuators_asctec.nb_err++;
-      actuators_asctec.i2c_trans.status = I2CTransDone;
-      break;
-    case I2CTransSuccess:
-    case I2CTransDone:
-      actuators_asctec.i2c_trans.status = I2CTransDone;
-      break;
-    default:
-      actuators_asctec.nb_err++;
-      return;
-  }
-
-#ifdef KILL_MOTORS
-  actuators_asctec.i2c_trans.buf[0] = 0;
-  actuators_asctec.i2c_trans.buf[1] = 0;
-  actuators_asctec.i2c_trans.buf[2] = 0;
-  actuators_asctec.i2c_trans.buf[3] = 0;
-  actuators_asctec.i2c_trans.buf[4] = 0xAA;
-#else
-  switch (actuators_asctec.cmd) {
-  case TEST:
-    actuators_asctec.i2c_trans.buf[0] = 251;
-    actuators_asctec.i2c_trans.buf[1] = actuators_asctec.cur_addr;
-    actuators_asctec.i2c_trans.buf[2] = 0;
-    actuators_asctec.i2c_trans.buf[3] = 231 + actuators_asctec.cur_addr;
-    actuators_asctec.i2c_trans.len_w = 4;
-    break;
-  case REVERSE:
-    actuators_asctec.i2c_trans.buf[0] = 254;
-    actuators_asctec.i2c_trans.buf[1] = actuators_asctec.cur_addr;
-    actuators_asctec.i2c_trans.buf[2] = 0;
-    actuators_asctec.i2c_trans.buf[3] = 234 + actuators_asctec.cur_addr;
-    actuators_asctec.i2c_trans.len_w = 4;
-    break;
-  case SET_ADDR:
-    actuators_asctec.i2c_trans.buf[0] = 250;
-    actuators_asctec.i2c_trans.buf[1] = actuators_asctec.cur_addr;
-    actuators_asctec.i2c_trans.buf[2] = actuators_asctec.new_addr;
-    actuators_asctec.i2c_trans.buf[3] = 230 + actuators_asctec.cur_addr + actuators_asctec.new_addr;
-    actuators_asctec.cur_addr = actuators_asctec.new_addr;
-    actuators_asctec.i2c_trans.len_w = 4;
-    break;
-  case NONE:
-    actuators_asctec.i2c_trans.buf[0] = actuators_asctec.cmds[SERVO_FRONT];
-    actuators_asctec.i2c_trans.buf[1] = actuators_asctec.cmds[SERVO_BACK];
-    actuators_asctec.i2c_trans.buf[2] = actuators_asctec.cmds[SERVO_LEFT];
-    actuators_asctec.i2c_trans.buf[3] = actuators_asctec.cmds[SERVO_RIGHT];
-    actuators_asctec.i2c_trans.buf[4] = 0xAA + actuators_asctec.i2c_trans.buf[0] + actuators_asctec.i2c_trans.buf[1] +
-      actuators_asctec.i2c_trans.buf[2] + actuators_asctec.i2c_trans.buf[3];
-    actuators_asctec.i2c_trans.len_w = 5;
-    break;
-  default:
-    break;
-  }
-  actuators_asctec.cmd = NONE;
-#endif
-
-  actuators_asctec.i2c_trans.type = I2CTransTx;   // Can be reset I2C driver
-  i2c_submit(&ACTUATORS_ASCTEC_DEVICE, &actuators_asctec.i2c_trans);
-
-}
-#endif /* ACTUATORS_ASCTEC_V2_PROTOCOL */

sw/airborne/subsystems/actuators/actuators_asctec.h

@@ -31,21 +31,21 @@
 #include "generated/airframe.h"
 
 enum actuators_asctec_cmd { NONE,
-                           TEST,
-                           REVERSE,
-                           SET_ADDR };
+                            TEST,
+                            REVERSE,
+                            SET_ADDR };
 
 enum actuators_asctec_addr { FRONT,
-                            BACK,
-                            LEFT,
-                            RIGHT };
+                             BACK,
+                             LEFT,
+                             RIGHT };
 
 /* this is for the v1 protocol which does its own mixing */
 enum actuators_asctec_cmds { PITCH,
-                            ROLL,
-                            YAW,
-                            THRUST,
-                            CMD_NB };
+                             ROLL,
+                             YAW,
+                             THRUST,
+                             CMD_NB };
 
 struct ActuatorsAsctec {
   enum actuators_asctec_cmd cmd;

sw/airborne/subsystems/actuators/actuators_asctec_v2.c

@@ -0,0 +1,141 @@
+/*
+ * Copyright (C) 2013 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.
+ */
+
+/** @file actuators_asctec_v2.c
+ *  Actuators driver for Asctec v2 motor controllers.
+ */
+
+#include "subsystems/actuators.h"
+#include "subsystems/actuators/actuators_asctec_v2.h"
+
+#include "mcu_periph/i2c.h"
+#include "mcu_periph/sys_time.h"
+
+#define ACTUATORS_ASCTEC_V2_SLAVE_ADDR 0x02
+
+struct ActuatorsAsctecV2 actuators_asctec_v2;
+
+uint32_t actuators_delay_time;
+bool_t   actuators_delay_done;
+
+void actuators_asctec_v2_init(void) {
+  actuators_asctec_v2.cmd = NONE;
+  actuators_asctec_v2.cur_addr = FRONT;
+  actuators_asctec_v2.new_addr = FRONT;
+  actuators_asctec_v2.i2c_trans.status = I2CTransSuccess;
+  actuators_asctec_v2.i2c_trans.type = I2CTransTx;
+  actuators_asctec_v2.i2c_trans.slave_addr = ACTUATORS_ASCTEC_V2_SLAVE_ADDR;
+  actuators_asctec_v2.i2c_trans.len_w = 5;
+  actuators_asctec_v2.nb_err = 0;
+
+#if defined ACTUATORS_START_DELAY && ! defined SITL
+  actuators_delay_done = FALSE;
+  SysTimeTimerStart(actuators_delay_time);
+#else
+  actuators_delay_done = TRUE;
+  actuators_delay_time = 0;
+#endif
+
+}
+
+
+void actuators_asctec_v2_set(bool_t motors_on) {
+#if defined ACTUATORS_START_DELAY && ! defined SITL
+  if (!actuators_delay_done) {
+    if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) {
+#ifdef USE_I2C_ACTUATORS_REBOOT_HACK
+      //Lisa-L with Asctech v2 motors only start after reflashing when a bus error was sensed on stm32-i2c.
+      //multiple re-init solves the problem.
+      i2c1_init();
+#endif
+      return;
+    }
+    else actuators_delay_done = TRUE;
+  }
+#endif
+
+  switch (actuators_asctec_v2.i2c_trans.status) {
+    case I2CTransFailed:
+      actuators_asctec_v2.nb_err++;
+      actuators_asctec_v2.i2c_trans.status = I2CTransDone;
+      break;
+    case I2CTransSuccess:
+    case I2CTransDone:
+      actuators_asctec_v2.i2c_trans.status = I2CTransDone;
+      break;
+    default:
+      actuators_asctec_v2.nb_err++;
+      return;
+  }
+
+#ifdef KILL_MOTORS
+  actuators_asctec_v2.i2c_trans.buf[0] = 0;
+  actuators_asctec_v2.i2c_trans.buf[1] = 0;
+  actuators_asctec_v2.i2c_trans.buf[2] = 0;
+  actuators_asctec_v2.i2c_trans.buf[3] = 0;
+  actuators_asctec_v2.i2c_trans.buf[4] = 0xAA;
+#else
+  switch (actuators_asctec_v2.cmd) {
+    case TEST:
+      actuators_asctec_v2.i2c_trans.buf[0] = 251;
+      actuators_asctec_v2.i2c_trans.buf[1] = actuators_asctec_v2.cur_addr;
+      actuators_asctec_v2.i2c_trans.buf[2] = 0;
+      actuators_asctec_v2.i2c_trans.buf[3] = 231 + actuators_asctec_v2.cur_addr;
+      actuators_asctec_v2.i2c_trans.len_w = 4;
+      break;
+    case REVERSE:
+      actuators_asctec_v2.i2c_trans.buf[0] = 254;
+      actuators_asctec_v2.i2c_trans.buf[1] = actuators_asctec_v2.cur_addr;
+      actuators_asctec_v2.i2c_trans.buf[2] = 0;
+      actuators_asctec_v2.i2c_trans.buf[3] = 234 + actuators_asctec_v2.cur_addr;
+      actuators_asctec_v2.i2c_trans.len_w = 4;
+      break;
+    case SET_ADDR:
+      actuators_asctec_v2.i2c_trans.buf[0] = 250;
+      actuators_asctec_v2.i2c_trans.buf[1] = actuators_asctec_v2.cur_addr;
+      actuators_asctec_v2.i2c_trans.buf[2] = actuators_asctec_v2.new_addr;
+      actuators_asctec_v2.i2c_trans.buf[3] = 230 + actuators_asctec_v2.cur_addr +
+                                             actuators_asctec_v2.new_addr;
+      actuators_asctec_v2.cur_addr = actuators_asctec_v2.new_addr;
+      actuators_asctec_v2.i2c_trans.len_w = 4;
+      break;
+    case NONE:
+      actuators_asctec_v2.i2c_trans.buf[0] = actuators_asctec_v2.cmds[SERVO_FRONT];
+      actuators_asctec_v2.i2c_trans.buf[1] = actuators_asctec_v2.cmds[SERVO_BACK];
+      actuators_asctec_v2.i2c_trans.buf[2] = actuators_asctec_v2.cmds[SERVO_LEFT];
+      actuators_asctec_v2.i2c_trans.buf[3] = actuators_asctec_v2.cmds[SERVO_RIGHT];
+      actuators_asctec_v2.i2c_trans.buf[4] = 0xAA + actuators_asctec_v2.i2c_trans.buf[0] +
+                                             actuators_asctec_v2.i2c_trans.buf[1] +
+                                             actuators_asctec_v2.i2c_trans.buf[2] +
+                                             actuators_asctec_v2.i2c_trans.buf[3];
+      actuators_asctec_v2.i2c_trans.len_w = 5;
+      break;
+    default:
+      break;
+  }
+  actuators_asctec_v2.cmd = NONE;
+#endif
+
+  actuators_asctec_v2.i2c_trans.type = I2CTransTx;   // Can be reset I2C driver
+  i2c_submit(&ACTUATORS_ASCTEC_V2_DEVICE, &actuators_asctec_v2.i2c_trans);
+
+}
+

sw/airborne/subsystems/actuators/actuators_asctec_v2.h

@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 2009-2013 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.
+ */
+
+/** @file actuators_asctec_v2.h
+ *  Actuators driver for Asctec v2 motor controllers.
+ */
+
+#ifndef ACTUATORS_ASCTEC_V2_H
+#define ACTUATORS_ASCTEC_V2_H
+
+#include "mcu_periph/i2c.h"
+
+#include "generated/airframe.h"
+
+enum actuators_asctec_v2_cmd { NONE,
+                            TEST,
+                            REVERSE,
+                            SET_ADDR };
+
+enum actuators_asctec_v2_addr { FRONT,
+                             BACK,
+                             LEFT,
+                             RIGHT };
+
+struct ActuatorsAsctecV2 {
+  enum actuators_asctec_v2_cmd cmd;
+  enum actuators_asctec_v2_addr cur_addr;
+  enum actuators_asctec_v2_addr new_addr;
+  int32_t cmds[4];
+  struct i2c_transaction i2c_trans;
+  volatile uint32_t nb_err;
+};
+
+
+extern struct ActuatorsAsctecV2 actuators_asctec_v2;
+
+#define actuators_asctec_v2_SetCommand(_v) {       \
+    actuators_asctec_v2.cmd = _v;                  \
+  }
+
+#define actuators_asctec_v2_SetNewAddr(_v) {       \
+    actuators_asctec_v2.new_addr = _v;             \
+  }
+
+#define actuators_asctec_v2_SetCurAddr(_v) {       \
+    actuators_asctec_v2.cur_addr = _v;             \
+  }
+
+extern void actuators_asctec_v2_init(void);
+extern void actuators_asctec_v2_set(bool_t motors_on);
+
+#define ActuatorAsctec_v2Set(_i, _v) { actuators_asctec_v2.cmds[_i] = _v; }
+#define ActuatorsAsctec_v2Init() actuators_asctec_v2_init()
+#define ActuatorsAsctec_v2Commit() actuators_asctec_v2_set(autopilot_motors_on)
+
+
+#endif /* ACTUATORS_ASCTEC_H */