Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
actuators_asctec_v2_new.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 The Paparazzi Team
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 
26 #include "subsystems/actuators.h"
28 
29 #include "mcu_periph/i2c.h"
30 #include "mcu_periph/sys_time.h"
31 
32 #define ACTUATORS_ASCTEC_V2_SLAVE_ADDR 0x00
33 
34 PRINT_CONFIG_VAR(ACTUATORS_ASCTEC_V2_I2C_DEV)
35 
37 
38 static uint16_t crc_update(uint16_t crc, uint8_t data);
39 
41 {
42  actuators_asctec_v2.cmd = NONE;
43  actuators_asctec_v2.cur_addr = FRONT;
44  actuators_asctec_v2.new_addr = FRONT;
45  actuators_asctec_v2.i2c_trans.status = I2CTransSuccess;
46  actuators_asctec_v2.i2c_trans.type = I2CTransTx;
48  actuators_asctec_v2.i2c_trans.len_w = 5;
49  actuators_asctec_v2.nb_err = 0;
50 }
51 
52 
54 {
55 #if defined ACTUATORS_START_DELAY && ! defined SITL
56  if (!actuators_delay_done) {
57  if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) {
58 #ifdef USE_I2C_ACTUATORS_REBOOT_HACK
59  //Lisa-L with Asctech v2 motors only start after reflashing when a bus error was sensed on stm32-i2c.
60  //multiple re-init solves the problem.
61  i2c1_init();
62 #endif
63  return;
64  } else { actuators_delay_done = true; }
65  }
66 #endif
67 
69  case I2CTransFailed:
72  break;
73  case I2CTransSuccess:
74  case I2CTransDone:
76  break;
77  default:
79  return;
80  }
81 
82  uint16_t crc=0xff;
83 
84 #ifdef KILL_MOTORS
90 
91  for (uint8_t i=0;i<5;i++)
93 
96 #else
97  switch (actuators_asctec_v2.cmd) {
98  case TEST:
103 
104  i2c_transmit(&ACTUATORS_ASCTEC_V2_I2C_DEV, &actuators_asctec_v2.i2c_trans, ACTUATORS_ASCTEC_V2_SLAVE_ADDR, 4);
106  break;
107  case REVERSE:
112 
113  i2c_transmit(&ACTUATORS_ASCTEC_V2_I2C_DEV, &actuators_asctec_v2.i2c_trans, ACTUATORS_ASCTEC_V2_SLAVE_ADDR, 4);
115  break;
116  case SET_ADDR: {
117  static uint8_t set_addr = 0;
118  static uint8_t serial_num[2];
119  switch(set_addr) {
120 
121  // Request address
122  case 0:
123  case 2:
124  i2c_receive(&ACTUATORS_ASCTEC_V2_I2C_DEV, &actuators_asctec_v2.i2c_trans, 0x06 + actuators_asctec_v2.cur_addr*2, 3);
125  set_addr++;
126  break;
127  case 1:
128  if(actuators_asctec_v2.i2c_trans.buf[0] == 2) {
129  serial_num[0] = actuators_asctec_v2.i2c_trans.buf[1];
130  set_addr++;
131  break;
132  }
133  set_addr--;
134  break;
135  case 3:
136  if(actuators_asctec_v2.i2c_trans.buf[0] == 3) {
137  serial_num[1] = actuators_asctec_v2.i2c_trans.buf[1];
138  set_addr++;
139  break;
140  }
141 
142  set_addr--;
143  break;
144  case 4:
145  //update ID of the motor based on the address
147  actuators_asctec_v2.i2c_trans.buf[1] = serial_num[0];
148  actuators_asctec_v2.i2c_trans.buf[2] = serial_num[1];
150 
151  i2c_transmit(&ACTUATORS_ASCTEC_V2_I2C_DEV, &actuators_asctec_v2.i2c_trans, ACTUATORS_ASCTEC_V2_SLAVE_ADDR, 4);
152  set_addr++;
153  default:
154  set_addr = 0;
157  break;
158  }
159 
160  break;
161  }
162  case NONE:
168 
169  for (uint8_t i=0;i<5;i++)
171 
173  i2c_transmit(&ACTUATORS_ASCTEC_V2_I2C_DEV, &actuators_asctec_v2.i2c_trans, ACTUATORS_ASCTEC_V2_SLAVE_ADDR, 6);
174  break;
175  default:
176  break;
177  }
178 #endif
179 }
180 
182 {
183  data ^= (crc & 0xff);
184  data ^= data << 4;
185 
186  return ((((uint16_t)data << 8) | ((crc>>8)&0xff)) ^ (uint8_t)(data >> 4) ^ ((uint16_t)data << 3));
187 }
unsigned short uint16_t
Definition: types.h:16
void actuators_asctec_v2_set(void)
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
transaction successfully finished by I2C driver
Definition: i2c.h:57
enum actuators_asctec_v2_addr cur_addr
struct ActuatorsAsctecV2 actuators_asctec_v2
#define ACTUATORS_ASCTEC_V2_SLAVE_ADDR
enum actuators_asctec_v2_addr new_addr
uint8_t len_w
Number of bytes to write/transmit.
Definition: i2c.h:116
Hardware independent API for actuators (servos, motor controllers).
transaction set to done by user level
Definition: i2c.h:59
Architecture independent timing functions.
transaction failed
Definition: i2c.h:58
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
#define USEC_OF_SEC(sec)
Definition: sys_time.h:210
bool i2c_transmit(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len)
Submit a write only transaction.
Definition: i2c.c:258
#define SysTimeTimer(_t)
Definition: sys_time.h:219
struct i2c_transaction i2c_trans
uint8_t slave_addr
Slave address.
Definition: i2c.h:104
volatile uint32_t nb_err
unsigned char uint8_t
Definition: types.h:14
Actuators driver for Asctec v2 motor controllers with the new I2C protocol.
bool i2c_receive(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint16_t len)
Submit a read only transaction.
Definition: i2c.c:268
transmit only transaction
Definition: i2c.h:47
void actuators_asctec_v2_init(void)
static uint16_t crc_update(uint16_t crc, uint8_t data)
enum I2CTransactionType type
Transaction type.
Definition: i2c.h:98
enum actuators_asctec_v2_cmd cmd
Architecture independent I2C (Inter-Integrated Circuit Bus) API.