Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
actuators_asctec.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
28 
29 #ifdef ACTUATORS_ASCTEC_V2_PROTOCOL
31 #endif
32 
34 #include "mcu_periph/i2c.h"
35 #include "mcu_periph/sys_time.h"
36 
37 #define ASCTEC_MIN_CMD -100
38 #define ASCTEC_MAX_CMD 100
39 
40 #define ASCTEC_MIN_THROTTLE 0
41 #define ASCTEC_MAX_THROTTLE 200
42 
44 
47 
48 void actuators_init(void) {
55 #ifdef ACTUATORS_ASCTEC_V2_PROTOCOL
57 #else
59 #endif
61 
62 #if defined ACTUATORS_START_DELAY && ! defined SITL
65 #else
68 #endif
69 
70 #ifdef ACTUATORS_ASCTEC_V2_PROTOCOL
72 #endif
73 
74 }
75 
76 #ifndef ACTUATORS_ASCTEC_V2_PROTOCOL
77 void actuators_set(bool_t motors_on) {
78 #if defined ACTUATORS_START_DELAY && ! defined SITL
79  if (!actuators_delay_done) {
80  if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) return;
82  }
83 #endif
84 
86  case I2CTransFailed:
89  break;
90  case I2CTransSuccess:
91  case I2CTransDone:
93  break;
94  default:
96  return;
97  }
98 
99 #ifdef KILL_MOTORS
104 #else /* ! KILL_MOTORS */
105  actuators_asctec.cmds[PITCH] = ((commands[COMMAND_PITCH] + SUPERVISION_TRIM_E) * ASCTEC_MAX_CMD) / MAX_PPRZ;
106  actuators_asctec.cmds[ROLL] = ((commands[COMMAND_ROLL] + SUPERVISION_TRIM_A) * ASCTEC_MAX_CMD) / MAX_PPRZ;
107  actuators_asctec.cmds[YAW] = ((commands[COMMAND_YAW] + SUPERVISION_TRIM_R) * ASCTEC_MAX_CMD) / MAX_PPRZ;
112  if (motors_on) {
114  }
115  else
117 #endif /* KILL_MOTORS */
118 
119  switch (actuators_asctec.cmd) {
120  case TEST:
121  actuators_asctec.i2c_trans.buf[0] = 251;
125  break;
126  case REVERSE:
127  actuators_asctec.i2c_trans.buf[0] = 254;
131  break;
132  case SET_ADDR:
133  actuators_asctec.i2c_trans.buf[0] = 250;
138  break;
139  case NONE:
144  break;
145  default:
146  break;
147  }
149 
150  i2c_submit(&ACTUATORS_ASCTEC_DEVICE, &actuators_asctec.i2c_trans);
151 
152 }
153 #else /* ! ACTUATORS_ASCTEC_V2_PROTOCOL */
154 void actuators_set(bool_t motors_on) {
155 #if defined ACTUATORS_START_DELAY && ! defined SITL
156  if (!actuators_delay_done) {
157  if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) {
158  //Lisa-L with Asctech v2 motors only start after reflashing when a bus error was sensed on stm32-i2c.
159  //multiple re-init solves the problem.
160  i2c1_init();
161  return;
162  }
163  else actuators_delay_done = TRUE;
164  }
165 #endif
166 
168  case I2CTransFailed:
171  break;
172  case I2CTransSuccess:
173  case I2CTransDone:
175  break;
176  default:
178  return;
179  }
180 
181  supervision_run(motors_on, FALSE, commands);
182 #ifdef KILL_MOTORS
187  actuators_asctec.i2c_trans.buf[4] = 0xAA;
188 #else
195 #endif
196 
197  i2c_submit(&ACTUATORS_ASCTEC_DEVICE, &actuators_asctec.i2c_trans);
198 
199 }
200 #endif /* ACTUATORS_ASCTEC_V2_PROTOCOL */
void actuators_set(bool_t motors_on)
void actuators_init(void)
int32_t cmds[CMD_NB]
enum actuators_astec_cmd cmd
void supervision_run(bool_t motors_on, bool_t override_on, int32_t in_cmd[])
Definition: supervision.c:142
bool_t actuators_delay_done
uint32_t actuators_delay_time
#define ASCTEC_MIN_THROTTLE
uint8_t slave_addr
Definition: i2c.h:43
#define FALSE
Definition: imu_chimu.h:141
struct ActuatorsAsctec actuators_asctec
bool_t i2c_submit(struct i2c_periph *p, struct i2c_transaction *t)
Definition: i2c_arch.c:321
Architecture independent timing functions.
enum actuators_astec_addr cur_addr
void supervision_init(void)
Definition: supervision.c:68
unsigned long uint32_t
Definition: types.h:18
enum actuators_astec_addr new_addr
#define ASCTEC_MAX_THROTTLE
#define SysTimeTimer(_t)
Definition: sys_time_arch.h:71
enum I2CTransactionStatus status
Definition: i2c.h:47
Supervision.
#define USEC_OF_SEC(sec)
Definition: sys_time.h:124
#define TRUE
Definition: imu_chimu.h:144
volatile uint8_t buf[I2C_BUF_LEN]
Definition: i2c.h:46
uint8_t len_w
Definition: i2c.h:45
Actuators driver for Asctec motor controllers.
pprz_t commands[COMMANDS_NB]
Definition: commands.c:48
int32_t commands[SUPERVISION_NB_MOTOR]
Definition: supervision.h:35
struct i2c_transaction i2c_trans
enum I2CTransactionType type
Definition: i2c.h:42
#define SysTimeTimerStart(_t)
Definition: sys_time_arch.h:70
#define MAX_PPRZ
Definition: paparazzi.h:8
struct Supervision supervision
Definition: supervision.c:66
#define ASCTEC_MAX_CMD
#define ASCTEC_MIN_CMD
Definition: i2c.h:9
volatile uint32_t nb_err