[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5977] moved actuators from booz to firmwares/rotorc
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [5977] moved actuators from booz to firmwares/rotorcraft |
|
Date: |
Mon, 27 Sep 2010 22:56:47 +0000 |
Revision: 5977
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5977
Author: flixr
Date: 2010-09-27 22:56:47 +0000 (Mon, 27 Sep 2010)
Log Message:
-----------
moved actuators from booz to firmwares/rotorcraft
Added Paths:
-----------
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/arch/
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/supervision.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/supervision.h
Removed Paths:
-------------
paparazzi3/trunk/sw/airborne/booz/actuators/
paparazzi3/trunk/sw/airborne/booz/arch/stm32/actuators/
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c
(from rev 5976,
paparazzi3/trunk/sw/airborne/booz/actuators/booz_actuators_asctec.c)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c
2010-09-27 22:56:47 UTC (rev 5977)
@@ -0,0 +1,134 @@
+#include "booz/booz_actuators.h"
+#include "booz/actuators/booz_actuators_asctec.h"
+
+#ifdef ACTUATORS_ASCTEC_V2_PROTOCOL
+#include "booz/actuators/booz_supervision.h"
+#endif
+
+#include "booz/booz2_commands.h"
+#include "i2c.h"
+#include "sys_time.h"
+
+
+struct ActuatorsAsctec actuators_asctec;
+
+uint32_t actuators_delay_time;
+bool_t actuators_delay_done;
+
+void actuators_init(void) {
+ actuators_asctec.cmd = NONE;
+ actuators_asctec.cur_addr = FRONT;
+ 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.len_w = 4;
+#endif
+ actuators_asctec.nb_err = 0;
+
+#if defined BOOZ_START_DELAY && ! defined SITL
+ actuators_delay_done = FALSE;
+ SysTimeTimerStart(actuators_delay_time);
+#else
+ actuators_delay_done = TRUE;
+ actuators_delay_time = 0;
+#endif
+
+#ifdef ACTUATORS_ASCTEC_V2_PROTOCOL
+ supervision_init();
+#endif
+
+}
+
+#ifndef ACTUATORS_ASCTEC_V2_PROTOCOL
+void actuators_set(bool_t motors_on) {
+#if defined BOOZ_START_DELAY && ! defined SITL
+ if (!actuators_delay_done) {
+ if (SysTimeTimer(actuators_delay_time) <
SYS_TICS_OF_SEC(BOOZ_START_DELAY)) return;
+ else actuators_delay_done = TRUE;
+ }
+#endif
+
+ if (!actuators_asctec.i2c_trans.status == I2CTransSuccess)
+ actuators_asctec.nb_err++;
+
+#ifdef KILL_MOTORS
+ actuators_asctec.cmds[PITCH] = 0;
+ actuators_asctec.cmds[ROLL] = 0;
+ actuators_asctec.cmds[YAW] = 0;
+ actuators_asctec.cmds[THRUST] = 0;
+#else /* ! KILL_MOTORS */
+ actuators_asctec.cmds[PITCH] = booz2_commands[COMMAND_PITCH] +
SUPERVISION_TRIM_E;
+ actuators_asctec.cmds[ROLL] = booz2_commands[COMMAND_ROLL] +
SUPERVISION_TRIM_A;
+ actuators_asctec.cmds[YAW] = booz2_commands[COMMAND_YAW] +
SUPERVISION_TRIM_R;
+ actuators_asctec.cmds[THRUST] = booz2_commands[COMMAND_THRUST];
+ Bound(actuators_asctec.cmds[PITCH],-100, 100);
+ Bound(actuators_asctec.cmds[ROLL], -100, 100);
+ Bound(actuators_asctec.cmds[YAW], -100, 100);
+ if (motors_on) {
+ Bound(actuators_asctec.cmds[THRUST], 1, 200);
+ }
+ else
+ actuators_asctec.cmds[THRUST] = 0;
+#endif /* KILL_MOTORS */
+
+ 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;
+ 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;
+ 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;
+ break;
+ case NONE:
+ actuators_asctec.i2c_trans.buf[0] = 100 - actuators_asctec.cmds[PITCH];
+ actuators_asctec.i2c_trans.buf[1] = 100 + actuators_asctec.cmds[ROLL];
+ actuators_asctec.i2c_trans.buf[2] = 100 - actuators_asctec.cmds[YAW];
+ actuators_asctec.i2c_trans.buf[3] = actuators_asctec.cmds[THRUST];
+ break;
+ default:
+ break;
+ }
+ actuators_asctec.cmd = NONE;
+
+ i2c_submit(&ACTUATORS_ASCTEC_DEVICE, &actuators_asctec.i2c_trans);
+
+}
+#else /* ! ACTUATORS_ASCTEC_V2_PROTOCOL */
+void actuators_set(bool_t motors_on) {
+ if (!cpu_time_sec) return; // FIXME
+ supervision_run(motors_on, FALSE, booz2_commands);
+#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
+ actuators_asctec.i2c_trans.buf[0] = supervision.commands[SERVO_FRONT];
+ actuators_asctec.i2c_trans.buf[1] = supervision.commands[SERVO_BACK];
+ actuators_asctec.i2c_trans.buf[2] = supervision.commands[SERVO_LEFT];
+ actuators_asctec.i2c_trans.buf[3] = supervision.commands[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];
+#endif
+
+ i2c_submit(&ACTUATORS_ASCTEC_DEVICE, &actuators_asctec.i2c_trans);
+
+}
+#endif /* ACTUATORS_ASCTEC_V2_PROTOCOL */
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.h
(from rev 5976,
paparazzi3/trunk/sw/airborne/booz/actuators/booz_actuators_asctec.h)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.h
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.h
2010-09-27 22:56:47 UTC (rev 5977)
@@ -0,0 +1,71 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#ifndef BOOZ_ACTUATORS_ASCTEC_H
+#define BOOZ_ACTUATORS_ASCTEC_H
+
+#include "i2c.h"
+
+enum actuators_astec_cmd { NONE,
+ TEST,
+ REVERSE,
+ SET_ADDR };
+
+enum actuators_astec_addr { FRONT,
+ BACK,
+ LEFT,
+ RIGHT };
+
+/* this is for the v1 protocol which does its own mixing */
+enum actuators_astec_cmds { PITCH,
+ ROLL,
+ YAW,
+ THRUST,
+ CMD_NB };
+
+struct ActuatorsAsctec {
+ enum actuators_astec_cmd cmd;
+ enum actuators_astec_addr cur_addr;
+ enum actuators_astec_addr new_addr;
+ int32_t cmds[CMD_NB];
+ struct i2c_transaction i2c_trans;
+ volatile uint32_t nb_err;
+};
+
+
+extern struct ActuatorsAsctec actuators_asctec;
+
+#define booz_actuators_asctec_SetCommand(_v) { \
+ actuators_asctec.cmd = _v; \
+}
+
+#define booz_actuators_asctec_SetNewAddr(_v) { \
+ actuators_asctec.new_addr = _v; \
+}
+
+#define booz_actuators_asctec_SetCurAddr(_v) { \
+ actuators_asctec.cur_addr = _v; \
+}
+
+
+#endif /* BOOZ_ACTUATORS_ASCTEC_H */
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c
(from rev 5976,
paparazzi3/trunk/sw/airborne/booz/actuators/booz_actuators_heli.c)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c
2010-09-27 22:56:47 UTC (rev 5977)
@@ -0,0 +1,53 @@
+/*
+ * $Id$
+ *
+ * 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.
+ */
+
+#include "booz/booz_actuators.h"
+#include "booz/actuators/booz_actuators_heli.h"
+#include "booz/booz2_commands.h"
+
+/* let's start butchery now and use the actuators_pwm arch functions */
+#include "booz/actuators/booz_actuators_pwm.h"
+
+/* get SetActuatorsFromCommands() macro */
+#include "airframe.h"
+
+/* define the glue between control and SetActuatorsFromCommands */
+#define actuators booz_actuators_pwm_values
+#define SERVOS_TICS_OF_USEC(_v) (_v)
+#define ESC_STOPPED SERVOS_TICS_OF_USEC(1000)
+#define ESC_HOVER SERVOS_TICS_OF_USEC(1750)
+#define Actuator(_x) booz_actuators_pwm_values[_x]
+#define ChopServo(x,a,b) Chop(x, a, b)
+#define ActuatorsCommit booz_actuators_pwm_commit
+
+int32_t booz_actuators_pwm_values[BOOZ_ACTUATORS_PWM_NB];
+
+void actuators_init(void) { booz_actuators_pwm_arch_init(); }
+
+
+void actuators_set(bool_t motors_on) {
+
+ SetActuatorsFromCommands(booz2_commands);
+
+}
+
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.h
(from rev 5976,
paparazzi3/trunk/sw/airborne/booz/actuators/booz_actuators_heli.h)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.h
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.h
2010-09-27 22:56:47 UTC (rev 5977)
@@ -0,0 +1,30 @@
+/*
+ * $Id: actuators_heli$
+ *
+ * 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.
+ *
+ */
+
+#ifndef BOOZ_ACTUATORS_HELI_H
+#define BOOZ_ACTUATORS_HELI_H
+
+
+#endif /* BOOZ_ACTUATORS_HELI_H */
+
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c
(from rev 5976,
paparazzi3/trunk/sw/airborne/booz/actuators/booz_actuators_mkk.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c
2010-09-27 22:56:47 UTC (rev 5977)
@@ -0,0 +1,78 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#include "booz/booz_actuators.h"
+#include "booz/actuators/booz_actuators_mkk.h"
+
+#include "booz/booz2_commands.h"
+#include "i2c.h"
+#include "sys_time.h"
+
+
+struct ActuatorsMkk actuators_mkk;
+
+
+uint32_t actuators_delay_time;
+bool_t actuators_delay_done;
+
+void actuators_init(void) {
+
+ supervision_init();
+ const uint8_t actuators_addr[ACTUATORS_MKK_NB] = ACTUATORS_MKK_ADDR;
+ for (uint8_t i=0; i<ACTUATORS_MKK_NB; i++) {
+ actuators_mkk.trans[i].type = I2CTransTx;
+ actuators_mkk.trans[i].len_w = 1;
+ actuators_mkk.trans[i].slave_addr = actuators_addr[i];
+ actuators_mkk.trans[i].stop_after_transmit = TRUE;
+ actuators_mkk.trans[i].status = I2CTransSuccess;
+ }
+
+#if defined BOOZ_START_DELAY && ! defined SITL
+ actuators_delay_done = FALSE;
+ SysTimeTimerStart(actuators_delay_time);
+#else
+ actuators_delay_done = TRUE;
+ actuators_delay_time = 0;
+#endif
+
+}
+
+
+void actuators_set(bool_t motors_on) {
+#if defined BOOZ_START_DELAY && ! defined SITL
+ if (!actuators_delay_done) {
+ if (SysTimeTimer(actuators_delay_time) <
SYS_TICS_OF_SEC(BOOZ_START_DELAY)) return;
+ else actuators_delay_done = TRUE;
+ }
+#endif
+
+ supervision_run(motors_on, FALSE, booz2_commands);
+ for (uint8_t i=0; i<ACTUATORS_MKK_NB; i++) {
+#ifdef KILL_MOTORS
+ actuators_mkk.trans[i].buf[0] = 0;
+#else
+ actuators_mkk.trans[i].buf[0] = supervision.commands[i];
+#endif
+ i2c_submit(&ACTUATORS_MKK_DEVICE, &actuators_mkk.trans[i]);
+ }
+}
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.h
(from rev 5976,
paparazzi3/trunk/sw/airborne/booz/actuators/booz_actuators_mkk.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.h
2010-09-27 22:56:47 UTC (rev 5977)
@@ -0,0 +1,44 @@
+/*
+ * $Id: actuators_buss_twi_blmc_hw.h 3847 2009-08-02 21:47:31Z poine $
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#ifndef BOOZ_ACTUATORS_MKK_H
+#define BOOZ_ACTUATORS_MKK_H
+
+#include "std.h"
+#include "i2c.h"
+
+#include "airframe.h"
+
+
+struct ActuatorsMkk {
+ struct i2c_transaction trans[ACTUATORS_MKK_NB];
+};
+
+extern struct ActuatorsMkk actuators_mkk;
+
+
+
+#include "actuators/booz_supervision.h"
+
+
+#endif /* BOOZ_ACTUATORS_MKK_H */
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm.c
(from rev 5976,
paparazzi3/trunk/sw/airborne/booz/actuators/booz_actuators_pwm.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm.c
2010-09-27 22:56:47 UTC (rev 5977)
@@ -0,0 +1,7 @@
+#include "actuators/booz_actuators_pwm.h"
+
+int32_t booz_actuators_pwm_values[BOOZ_ACTUATORS_PWM_NB];
+
+void booz_actuators_init(void) {
+ booz_actuators_pwm_arch_init();
+}
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm.h
(from rev 5976,
paparazzi3/trunk/sw/airborne/booz/actuators/booz_actuators_pwm.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm.h
2010-09-27 22:56:47 UTC (rev 5977)
@@ -0,0 +1,36 @@
+/*
+ * $Id$
+ *
+ * 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.
+ */
+
+#ifndef BOOZ_ACTUATORS_PWM_H
+#define BOOZ_ACTUATORS_PWM_H
+
+#include "std.h"
+
+#define BOOZ_ACTUATORS_PWM_NB 6
+extern int32_t booz_actuators_pwm_values[BOOZ_ACTUATORS_PWM_NB];
+
+extern void booz_actuators_init(void);
+
+#include "actuators/booz_actuators_pwm_arch.h"
+
+#endif /* BOOZ_ACTUATORS_PWM_H */
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.c
(from rev 5976,
paparazzi3/trunk/sw/airborne/booz/arch/stm32/actuators/booz_actuators_pwm_arch.c)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.c
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.c
2010-09-27 22:56:47 UTC (rev 5977)
@@ -0,0 +1,124 @@
+/*
+ * $Id$
+ *
+ * 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.
+ *
+ */
+
+#include "booz/actuators/booz_actuators_pwm.h"
+
+#include <stm32/gpio.h>
+#include <stm32/rcc.h>
+#include <stm32/flash.h>
+#include <stm32/misc.h>
+#include <stm32/tim.h>
+
+#define PCLK 72000000
+#define ONE_MHZ_CLK 1000000
+#ifndef SERVO_HZ
+#define SERVO_HZ 40
+#endif
+
+void booz_actuators_pwm_arch_init(void) {
+
+ /* TIM3 and TIM4 clock enable */
+ RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
+ RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
+
+ /* GPIOB and GPIOC clock enable */
+ RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB |
RCC_APB2Periph_GPIOC |
+ RCC_APB2Periph_AFIO, ENABLE);
+ /* GPIO C */
+ /* PC6=servo1 PC7=servo2 PC8=servo3 PC9=servo4 */
+ GPIO_InitTypeDef GPIO_InitStructure;
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 |
GPIO_Pin_9;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_Init(GPIOC, &GPIO_InitStructure);
+ /* need to remate alternate function, pins 37, 38, 39, 40 on LQFP64 */
+ GPIO_PinRemapConfig(GPIO_FullRemap_TIM3, ENABLE);
+
+ /* GPIO B */
+ /* PB8=servo5 PB9=servo6 */
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9;
+ GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+ /* Time base configuration */
+ TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+ TIM_TimeBaseStructure.TIM_Prescaler = (PCLK / ONE_MHZ_CLK) - 1; // 1uS
+ TIM_TimeBaseStructure.TIM_Period = (ONE_MHZ_CLK / SERVO_HZ) - 1;
+ TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+ TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+
+ TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
+ TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
+
+ /* PWM1 Mode configuration: All Channels */
+ TIM_OCInitTypeDef TIM_OCInitStructure;
+ TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
+ TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
+ TIM_OCInitStructure.TIM_Pulse = 0; // default low (no pulse)
+ TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
+
+ /* PWM1 Mode configuration: TIM3 Channel1 */
+ TIM_OC1Init(TIM3, &TIM_OCInitStructure);
+ TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);
+
+ /* PWM1 Mode configuration: TIM3 Channel2 */
+ TIM_OC2Init(TIM3, &TIM_OCInitStructure);
+ TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
+
+ /* PWM1 Mode configuration: TIM3 Channel3 */
+ TIM_OC3Init(TIM3, &TIM_OCInitStructure);
+ TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable);
+
+ /* PWM1 Mode configuration: TIM3 Channel4 */
+ TIM_OC4Init(TIM3, &TIM_OCInitStructure);
+ TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable);
+
+ /* PWM1 Mode configuration: TIM4 Channel3 */
+ TIM_OC3Init(TIM4, &TIM_OCInitStructure);
+ TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable);
+
+ /* PWM1 Mode configuration: TIM4 Channel4 */
+ TIM_OC4Init(TIM4, &TIM_OCInitStructure);
+ TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable);
+
+ /* TIM3 enable */
+ TIM_ARRPreloadConfig(TIM3, ENABLE);
+ TIM_CtrlPWMOutputs(TIM3, ENABLE);
+ TIM_Cmd(TIM3, ENABLE);
+
+ /* TIM4 enable */
+ TIM_ARRPreloadConfig(TIM4, ENABLE);
+ TIM_CtrlPWMOutputs(TIM4, ENABLE);
+ TIM_Cmd(TIM4, ENABLE);
+
+}
+
+/* set pulse widths from actuator values, assumed to be in us */
+void booz_actuators_pwm_commit(void) {
+ TIM_SetCompare1(TIM3, booz_actuators_pwm_values[0]);
+ TIM_SetCompare2(TIM3, booz_actuators_pwm_values[1]);
+ TIM_SetCompare3(TIM3, booz_actuators_pwm_values[2]);
+ TIM_SetCompare4(TIM3, booz_actuators_pwm_values[3]);
+ TIM_SetCompare3(TIM4, booz_actuators_pwm_values[4]);
+ TIM_SetCompare4(TIM4, booz_actuators_pwm_values[5]);
+}
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.h
(from rev 5976,
paparazzi3/trunk/sw/airborne/booz/arch/stm32/actuators/booz_actuators_pwm_arch.h)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.h
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.h
2010-09-27 22:56:47 UTC (rev 5977)
@@ -0,0 +1,35 @@
+/*
+ * $Id$
+ *
+ * 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.
+ *
+ */
+
+/*
+ * STM32 PWM servos handling
+ */
+
+#ifndef BOOZ_ACTUATORS_PWM_ARCH_H
+#define BOOZ_ACTUATORS_PWM_ARCH_H
+
+extern void booz_actuators_pwm_arch_init(void);
+extern void booz_actuators_pwm_commit(void);
+
+#endif /* BOOZ_ACTUATORS_PWM_ARCH_H */
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/supervision.c (from
rev 5976, paparazzi3/trunk/sw/airborne/booz/actuators/booz_supervision.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/supervision.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/supervision.c
2010-09-27 22:56:47 UTC (rev 5977)
@@ -0,0 +1,132 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-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.
+ *
+ */
+
+#include "actuators/booz_supervision.h"
+
+//#include <stdint.h>
+#ifndef INT32_MIN
+#define INT32_MIN (-2147483647-1)
+#endif
+
+#ifndef INT32_MAX
+#define INT32_MAX (2147483647)
+#endif
+
+#ifndef SUPERVISION_MIN_MOTOR_STARTUP
+#define SUPERVISION_MIN_MOTOR_STARTUP SUPERVISION_MIN_MOTOR
+#endif
+
+static const int32_t roll_coef[SUPERVISION_NB_MOTOR] = SUPERVISION_ROLL_COEF;
+static const int32_t pitch_coef[SUPERVISION_NB_MOTOR] =
SUPERVISION_PITCH_COEF;
+static const int32_t yaw_coef[SUPERVISION_NB_MOTOR] = SUPERVISION_YAW_COEF;
+static const int32_t thrust_coef[SUPERVISION_NB_MOTOR] =
SUPERVISION_THRUST_COEF;
+
+struct BoozSupervision supervision;
+
+void supervision_init(void) {
+ uint8_t i;
+ for (i=0; i<SUPERVISION_NB_MOTOR; i++) {
+ supervision.commands[i] = 0;
+ supervision.trim[i] =
+ roll_coef[i] * SUPERVISION_TRIM_A +
+ pitch_coef[i] * SUPERVISION_TRIM_E +
+ yaw_coef[i] * SUPERVISION_TRIM_R;
+ supervision.override_enabled[i] = FALSE;
+ supervision.override_value[i] = SUPERVISION_MIN_MOTOR;
+ }
+ supervision.nb_failure = 0;
+}
+
+static inline void offset_commands(int32_t offset) {
+ uint8_t j;
+ for (j=0; j<SUPERVISION_NB_MOTOR; j++)
+ supervision.commands[j] += (offset);
+}
+
+static inline void bound_commands(void) {
+ uint8_t j;
+ for (j=0; j<SUPERVISION_NB_MOTOR; j++)
+ Bound(supervision.commands[j],
+ SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR);
+}
+
+void supervision_run_spinup(uint32_t counter, uint32_t max_counter)
+{
+ int i;
+ for (i = 0; i < SUPERVISION_NB_MOTOR; i++) {
+#ifdef SUPERVISION_STARTUP_DELAY
+ if (counter > i * max_counter / (SUPERVISION_NB_MOTOR +
SUPERVISION_STARTUP_DELAY)) {
+ if (counter > SUPERVISION_NB_MOTOR * max_counter / (SUPERVISION_NB_MOTOR
+ SUPERVISION_STARTUP_DELAY)) {
+ supervision.commands[i] = SUPERVISION_MIN_MOTOR_STARTUP +
(SUPERVISION_MIN_MOTOR - SUPERVISION_MIN_MOTOR_STARTUP) * counter / max_counter;
+ } else {
+ supervision.commands[i] = SUPERVISION_MIN_MOTOR_STARTUP;
+ }
+ } else {
+ supervision.commands[i] = 0;
+ }
+#else
+ if (counter < i * max_counter / SUPERVISION_NB_MOTOR) {
+ supervision.commands[i] = SUPERVISION_MIN_MOTOR_STARTUP;
+ }
+#endif
+ }
+}
+
+void supervision_run(bool_t motors_on, bool_t override_on, int32_t in_cmd[] ) {
+ uint8_t i;
+ if (motors_on) {
+ int32_t min_cmd = INT32_MAX;
+ int32_t max_cmd = INT32_MIN;
+ for (i=0; i<SUPERVISION_NB_MOTOR; i++) {
+ supervision.commands[i] =
+ (thrust_coef[i] * in_cmd[COMMAND_THRUST] +
+ roll_coef[i] * in_cmd[COMMAND_ROLL] +
+ pitch_coef[i] * in_cmd[COMMAND_PITCH] +
+ yaw_coef[i] * in_cmd[COMMAND_YAW] +
+ supervision.trim[i]) / SUPERVISION_SCALE;
+ if (supervision.commands[i] < min_cmd)
+ min_cmd = supervision.commands[i];
+ if (supervision.commands[i] > max_cmd)
+ max_cmd = supervision.commands[i];
+ }
+ if (min_cmd < SUPERVISION_MIN_MOTOR && max_cmd > SUPERVISION_MAX_MOTOR)
+ supervision.nb_failure++;
+ if (min_cmd < SUPERVISION_MIN_MOTOR)
+ offset_commands(-(min_cmd - SUPERVISION_MIN_MOTOR));
+ if (max_cmd > SUPERVISION_MAX_MOTOR)
+ offset_commands(-(max_cmd - SUPERVISION_MAX_MOTOR));
+
+ /* For testing motor failure */
+ if (motors_on && override_on) {
+ for (i = 0; i < SUPERVISION_NB_MOTOR; i++) {
+ if (supervision.override_enabled[i])
+ supervision.commands[i] = supervision.override_value[i];
+ }
+ }
+ bound_commands();
+ }
+ else
+ for (i=0; i<SUPERVISION_NB_MOTOR; i++)
+ supervision.commands[i] = 0;
+}
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/supervision.h (from
rev 5976, paparazzi3/trunk/sw/airborne/booz/actuators/booz_supervision.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/supervision.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/actuators/supervision.h
2010-09-27 22:56:47 UTC (rev 5977)
@@ -0,0 +1,21 @@
+#ifndef BOOZ_SUPERVISION_H
+#define BOOZ_SUPERVISION_H
+
+#include "std.h"
+#include "airframe.h"
+
+struct BoozSupervision {
+ int32_t commands[SUPERVISION_NB_MOTOR];
+ int32_t trim[SUPERVISION_NB_MOTOR];
+ bool_t override_enabled[SUPERVISION_NB_MOTOR];
+ int32_t override_value[SUPERVISION_NB_MOTOR];
+ uint32_t nb_failure;
+};
+
+extern struct BoozSupervision supervision;
+
+extern void supervision_init(void);
+extern void supervision_run(bool_t motors_on, bool_t override_on, int32_t
in_cmd[]);
+extern void supervision_run_spinup(uint32_t counter, uint32_t max_counter);
+
+#endif /* BOOZ_SUPERVISION_H */
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5977] moved actuators from booz to firmwares/rotorcraft,
Felix Ruess <=