Paparazzi UAS  v5.15_devel-81-gd13dafb
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
actuators.c
Go to the documentation of this file.
1 /*
2  *
3  * Copyright (C) 2014-2015 Freek van Tienen <freek.v.tienen@gmail.com>
4  * Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, see
20  * <http://www.gnu.org/licenses/>.
21  */
22 
33 #include "subsystems/actuators.h"
34 #include "subsystems/electrical.h"
35 #include "actuators.h"
36 #include "autopilot.h"
37 #include "subsystems/abi.h"
38 #include <endian.h>
39 #include <string.h>
40 
41 #include <stdio.h>
42 
46 struct __attribute__((__packed__)) disco_bldc_obs {
53  /* bit 0 indicates an overcurrent on the RC receiver port when high
54  * bits #1-#6 indicate an overcurrent on the #1-#6 PPM servos
55  */
58 } obs_data;
59 
69 static uint8_t disco_channels[] = { 4, 5, 6, 1, 2, 3 };
70 
71 #define DISCO_BLDC_STATUS_STOPPED 1
72 #define DISCO_BLDC_STATUS_RAMPUP 2
73 #define DISCO_BLDC_STATUS_RUNNING 4
74 #define DISCO_BLDC_STATUS_RAMPDOWN 5
75 
76 // motor start threshold in RPM
77 // RPM range on disco is [1000, 12500]
78 // start and 1100
79 #define DISCO_BLDC_START_MOTOR_THRESHOLD 1100
80 
82 static uint8_t actuators_disco_checksum(uint8_t *bytes, uint8_t size);
83 
85 {
86  /* Initialize the I2C connection */
90  int i = 0;
91  for (i = 0; i < ACTUATORS_DISCO_PWM_NB; i++) {
92  pwm_sysfs_init(&actuators_disco.pwm[i], "/sys/class/pwm", "export", "run", "duty_ns", "period_ns", disco_channels[i]);
93  }
94 }
95 
97 {
98  if (idx == ACTUATORS_DISCO_MOTOR_IDX) {
100  } else if (idx > ACTUATORS_DISCO_PWM_NB) {
101  // wrong index, do nothing
102  } else {
103  // val is a PWM value in ms, convert to ns
104  pwm_sysfs_set_duty(&actuators_disco.pwm[idx-1], val * 1000);
105  }
106 }
107 
109 {
110  // Handle PWM outputs
111  // already done in set function (FIXME ?)
112 
113  // Handle motor speed controller
114 
115  // Receive the status
118  // copy data from buffer
119 #pragma GCC diagnostic push
120 #pragma GCC diagnostic ignored "-Wcast-qual"
121  memcpy(&obs_data, (uint8_t*)actuators_disco.i2c_trans.buf, sizeof(obs_data));
122 #pragma GCC diagnostic pop
123 
124  // Update status
125  electrical.vsupply = (float)(be16toh(obs_data.batt_mv)) / 1000.f;
126  // extract 'rpm saturation bit'
127  actuators_disco.rpm_saturated = (obs_data.rpm & (1 << 7)) ? true : false;
128  // clear 'rpm saturation bit' and fix endianess
129  obs_data.rpm &= (uint16_t)(~(1 << 7));
130  obs_data.rpm = be16toh(obs_data.rpm);
131  if (obs_data.rpm == 0) {
133  }
134 
135  // When detected a suicide
136  uint8_t bldc_status = obs_data.status & 0x07;
137  if (obs_data.error == 2 && bldc_status != DISCO_BLDC_STATUS_STOPPED) {
138  autopilot_set_kill_throttle(true); //FIXME: make behaviour definable low flying should not stop it
139  }
140 
141  // Start the motors
142  if (bldc_status != DISCO_BLDC_STATUS_RUNNING &&
143  bldc_status != DISCO_BLDC_STATUS_RAMPUP && //FIXME also on rampdown?
145  // Reset the error
148 
149  // Start the motors
152  }
153  // Stop the motors
154  else if ((bldc_status == DISCO_BLDC_STATUS_RUNNING || bldc_status == DISCO_BLDC_STATUS_RAMPUP) &&
158  } else if (bldc_status == DISCO_BLDC_STATUS_RUNNING) {
159  // Send the commands
163  actuators_disco.i2c_trans.buf[3] = 0x00; // enable security
164 #pragma GCC diagnostic push
165 #pragma GCC diagnostic ignored "-Wcast-qual"
167 #pragma GCC diagnostic pop
169  }
170 
171  // Send ABI message
172  AbiSendMsgRPM(RPM_SENSOR_ID, &actuators_disco.rpm_obs, 1);//FIXME & or not
173 }
174 
176 {
177  uint8_t checksum = 0;
178  for (int i = 0; i < size; i++) {
179  checksum = checksum ^ bytes[i];
180  }
181 
182  return checksum;
183 }
unsigned short uint16_t
Definition: types.h:16
struct disco_bldc_obs obs_data
static uint32_t idx
uint8_t error
Definition: actuators.c:50
uint8_t overrcurrent
Definition: actuators.c:56
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
struct i2c_transaction i2c_trans
I2C transaction for communicating with the Disco BLDC driver.
Definition: actuators.h:55
#define ACTUATORS_DISCO_CLEAR_ERROR
Clear all current erros.
Definition: actuators.h:46
Main include for ABI (AirBorneInterface).
void actuators_disco_set(uint8_t idx, uint16_t val)
Definition: actuators.c:96
#define DISCO_BLDC_START_MOTOR_THRESHOLD
Definition: actuators.c:79
#define ACTUATORS_DISCO_MOTOR_IDX
Index for motor BLDC.
Definition: actuators.h:52
float vsupply
supply voltage in V
Definition: electrical.h:45
#define RPM_SENSOR_ID
void actuators_disco_commit(void)
Definition: actuators.c:108
bool rpm_saturated
RPM saturation flag (bit 15 in obs data)
Definition: actuators.h:60
uint16_t batt_mv
Definition: actuators.c:48
private observation structure
Definition: actuators.c:46
bool i2c_blocking_transceive(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len_w, uint16_t len_r)
Submit a write/read transaction and wait for it to complete.
Definition: i2c.c:385
Hardware independent API for actuators (servos, motor controllers).
struct ActuatorsDisco actuators_disco
Definition: actuators.c:81
static uint8_t checksum
Definition: airspeed_uADC.c:60
uint8_t status
Definition: actuators.c:49
transaction set to done by user level
Definition: i2c.h:59
Interface for electrical status: supply voltage, current, battery status, etc.
#define ACTUATORS_DISCO_GET_OBS_DATA
Get observation data.
Definition: actuators.h:42
#define ACTUATORS_DISCO_STOP_PROP
Stop the propellers.
Definition: actuators.h:45
void autopilot_set_kill_throttle(bool kill)
set kill throttle
Definition: autopilot.c:219
uint16_t val[TCOUPLE_NB]
uint16_t motor_rpm
Motor RPM setpoint.
Definition: actuators.h:56
uint16_t rpm
Definition: actuators.c:47
#define DISCO_BLDC_STATUS_RAMPUP
Definition: actuators.c:72
#define ACTUATORS_DISCO_SET_REF_SPEED
Set reference speed.
Definition: actuators.h:41
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
static uint8_t disco_channels[]
Internal mapping of the PWM with output index servo rail 1 <-> linux pwm_4 servo rail 2 <-> linux pwm...
Definition: actuators.c:69
#define ACTUATORS_DISCO_START_PROP
Start the propellers.
Definition: actuators.h:43
uint8_t checksum
Definition: actuators.c:57
static uint8_t actuators_disco_checksum(uint8_t *bytes, uint8_t size)
Definition: actuators.c:175
#define DISCO_BLDC_STATUS_STOPPED
Definition: actuators.c:71
uint8_t motors_err
Definition: actuators.c:51
Core autopilot interface common to all firmwares.
uint8_t slave_addr
Slave address.
Definition: i2c.h:104
void actuators_disco_init(void)
Definition: actuators.c:84
unsigned char uint8_t
Definition: types.h:14
int pwm_sysfs_init(struct PWM_Sysfs *pwm, char *base_path, char *_export, char *_enable, char *_duty, char *_period, uint8_t channel)
Definition: pwm_sysfs.c:52
struct Electrical electrical
Definition: electrical.c:66
uint8_t temp
Definition: actuators.c:52
#define ACTUATORS_DISCO_PWM_NB
Max number of PWM channels.
Definition: actuators.h:51
uint16_t rpm_obs
Measured RPM.
Definition: actuators.h:57
#define DISCO_BLDC_STATUS_RUNNING
Definition: actuators.c:73
void pwm_sysfs_set_duty(struct PWM_Sysfs *pwm, uint32_t duty)
Definition: pwm_sysfs.c:101
bool i2c_blocking_transmit(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len)
Submit a write only transaction and wait for it to complete.
Definition: i2c.c:343
#define ACTUATORS_DISCO_ADDR
Definition: actuators.h:38
struct PWM_Sysfs pwm[ACTUATORS_DISCO_PWM_NB]
Array of PWM outputs.
Definition: actuators.h:58
Actuator driver for the disco.