Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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 *
5 * This file is part of paparazzi.
6 *
7 * paparazzi is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2, or (at your option)
10 * any later version.
11 *
12 * paparazzi is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with paparazzi; see the file COPYING. If not, write to
19 * the Free Software Foundation, 59 Temple Place - Suite 330,
20 * Boston, MA 02111-1307, USA.
21 */
22
27
30#include "actuators.h"
31#include "led_hw.h"
32#include "autopilot.h"
33#include "modules/core/abi.h"
34
35#if PERIODIC_TELEMETRY
38
39#ifdef STABILIZATION_INDI_COMMANDS
40static uint8_t act_to_commands[INDI_NUM_ACT] = STABILIZATION_INDI_COMMANDS;
41#endif
42
43static void send_bebop_actuators(struct transport_tx *trans, struct link_device *dev)
44{
45#ifdef STABILIZATION_INDI_COMMANDS
46 int32_t cmd0 = commands[act_to_commands[0]];
47 int32_t cmd1 = commands[act_to_commands[1]];
48 int32_t cmd2 = commands[act_to_commands[2]];
49 int32_t cmd3 = commands[act_to_commands[3]];
50#else
51 int32_t cmd0 = stabilization.cmd[COMMAND_THRUST];
54 int32_t cmd3 = stabilization.cmd[COMMAND_YAW];
55#endif
56 pprz_msg_send_BEBOP_ACTUATORS(trans, dev, AC_ID,
57 &cmd0, &cmd1, &cmd2, &cmd3,
58 &actuators_bebop.rpm_ref[0],
59 &actuators_bebop.rpm_ref[1],
60 &actuators_bebop.rpm_ref[2],
61 &actuators_bebop.rpm_ref[3],
62 &actuators_bebop.rpm_obs[0],
63 &actuators_bebop.rpm_obs[1],
64 &actuators_bebop.rpm_obs[2],
65 &actuators_bebop.rpm_obs[3]);
66}
67#endif
68
69#define ACTUATORS_BEBOP_I2C_TIMEOUT 1.f
70
74
76{
77 /* Initialize the I2C connection */
78 actuators_bebop.i2c_trans.slave_addr = ACTUATORS_BEBOP_ADDR;
79 actuators_bebop.i2c_trans.status = I2CTransDone;
80 actuators_bebop.led = 0;
81
82#if PERIODIC_TELEMETRY
84#endif
85}
86
88{
89 actuators_bebop.rpm_ref[idx] = value;
90}
91
93{
94 // Receive the status
96 i2c_blocking_transceive(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1, 13, ACTUATORS_BEBOP_I2C_TIMEOUT);
97
98 // Update status
99 electrical.vsupply = (float)(actuators_bebop.i2c_trans.buf[9] + (actuators_bebop.i2c_trans.buf[8] << 8)) / 1000.f;
100 // The 15th bit contains saturation information, so it needs to be removed to get the rpm
101 actuators_bebop.rpm_obs[0] = (actuators_bebop.i2c_trans.buf[1] + (actuators_bebop.i2c_trans.buf[0] << 8)) & ~(1<<15);
102 actuators_bebop.rpm_obs[1] = (actuators_bebop.i2c_trans.buf[3] + (actuators_bebop.i2c_trans.buf[2] << 8)) & ~(1<<15);
103 actuators_bebop.rpm_obs[2] = (actuators_bebop.i2c_trans.buf[5] + (actuators_bebop.i2c_trans.buf[4] << 8)) & ~(1<<15);
104 actuators_bebop.rpm_obs[3] = (actuators_bebop.i2c_trans.buf[7] + (actuators_bebop.i2c_trans.buf[6] << 8)) & ~(1<<15);
105
106 // When detected a suicide
107 actuators_bebop.i2c_trans.buf[10] = actuators_bebop.i2c_trans.buf[10] & 0x7;
108 if (actuators_bebop.i2c_trans.buf[11] == 2 && actuators_bebop.i2c_trans.buf[10] != 1) {
110 }
111
112 // Start the motors
113 if (actuators_bebop.i2c_trans.buf[10] != 4 && actuators_bebop.i2c_trans.buf[10] != 2 && autopilot_get_motors_on()) {
114 // Reset the error
116 i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1, ACTUATORS_BEBOP_I2C_TIMEOUT);
117
118 // Start the motors
120#if BEBOP_VERSION2
121 // For Bebop version 2 some motors are reversed (FIXME: test final version)
122 actuators_bebop.i2c_trans.buf[1] = 0b00001010;
123#else
124 actuators_bebop.i2c_trans.buf[1] = 0b00000101;
125#endif
126 i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 2, ACTUATORS_BEBOP_I2C_TIMEOUT);
127 }
128 // Stop the motors
129 else if (actuators_bebop.i2c_trans.buf[10] == 4 && !autopilot_get_motors_on()) {
131 i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1, ACTUATORS_BEBOP_I2C_TIMEOUT);
132 } else if (actuators_bebop.i2c_trans.buf[10] == 4 && autopilot_get_motors_on()) {
133 // Send the commands
135 actuators_bebop.i2c_trans.buf[1] = actuators_bebop.rpm_ref[0] >> 8;
136 actuators_bebop.i2c_trans.buf[2] = actuators_bebop.rpm_ref[0] & 0xFF;
137 actuators_bebop.i2c_trans.buf[3] = actuators_bebop.rpm_ref[1] >> 8;
138 actuators_bebop.i2c_trans.buf[4] = actuators_bebop.rpm_ref[1] & 0xFF;
139 actuators_bebop.i2c_trans.buf[5] = actuators_bebop.rpm_ref[2] >> 8;
140 actuators_bebop.i2c_trans.buf[6] = actuators_bebop.rpm_ref[2] & 0xFF;
141 actuators_bebop.i2c_trans.buf[7] = actuators_bebop.rpm_ref[3] >> 8;
142 actuators_bebop.i2c_trans.buf[8] = actuators_bebop.rpm_ref[3] & 0xFF;
143 actuators_bebop.i2c_trans.buf[9] = 0x00; //UNK enable security?
144#pragma GCC diagnostic push
145#pragma GCC diagnostic ignored "-Wcast-qual"
146 actuators_bebop.i2c_trans.buf[10] = actuators_bebop_checksum((uint8_t *)actuators_bebop.i2c_trans.buf, 9);
147#pragma GCC diagnostic pop
148 i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 11, ACTUATORS_BEBOP_I2C_TIMEOUT);
149 }
150
151 // Update the LEDs
152 if (actuators_bebop.led != (led_hw_values & 0x3)) {
154 actuators_bebop.i2c_trans.buf[1] = (led_hw_values & 0x3);
155 i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 2, ACTUATORS_BEBOP_I2C_TIMEOUT);
156
157 actuators_bebop.led = led_hw_values & 0x3;
158 }
159
160 // Send ABI message
161 struct act_feedback_t feedback[4] = {{0}};
162 for (int i=0;i<4;i++) {
163 feedback[i].idx = get_servo_idx(i);
164 feedback[i].rpm = actuators_bebop.rpm_obs[i];
165 feedback[i].set.rpm = true;
166 }
167 AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_BOARD_ID, feedback, 4);
168}
169
171{
172 uint8_t checksum = 0;
173 for (int i = 0; i < size; i++) {
174 checksum = checksum ^ bytes[i];
175 }
176
177 return checksum;
178}
Main include for ABI (AirBorneInterface).
#define ACT_FEEDBACK_BOARD_ID
static uint8_t checksum
bool autopilot_set_motors_on(bool motors_on)
turn motors on/off, eventually depending of the current mode set kill_throttle accordingly: kill_thro...
Definition autopilot.c:258
bool autopilot_get_motors_on(void)
get motors status
Definition autopilot.c:303
Core autopilot interface common to all firmwares.
static void send_bebop_actuators(struct transport_tx *trans, struct link_device *dev)
Definition actuators.c:43
void actuators_bebop_init(void)
Definition actuators.c:75
uint32_t led_hw_values
Definition actuators.c:71
#define ACTUATORS_BEBOP_I2C_TIMEOUT
Definition actuators.c:69
static uint8_t actuators_bebop_checksum(uint8_t *bytes, uint8_t size)
Definition actuators.c:170
void actuators_bebop_commit(void)
Definition actuators.c:92
struct ActuatorsBebop actuators_bebop
Definition actuators.c:72
void actuators_bebop_set(uint8_t idx, int16_t value)
Definition actuators.c:87
Actuator driver for the bebop.
#define ACTUATORS_BEBOP_TOGGLE_GPIO
Toggle GPIO (reset, red led, green led).
Definition actuators.h:41
#define ACTUATORS_BEBOP_STOP_PROP
Stop the propellers.
Definition actuators.h:42
#define ACTUATORS_BEBOP_ADDR
Definition actuators.h:35
#define ACTUATORS_BEBOP_START_PROP
Start the propellers.
Definition actuators.h:40
#define ACTUATORS_BEBOP_SET_REF_SPEED
Set reference speed.
Definition actuators.h:38
#define ACTUATORS_BEBOP_CLEAR_ERROR
Clear all current erros.
Definition actuators.h:43
#define ACTUATORS_BEBOP_GET_OBS_DATA
Get observation data.
Definition actuators.h:39
static struct uart_periph * dev
#define COMMAND_PITCH
#define COMMAND_ROLL
struct Electrical electrical
Definition electrical.c:92
Interface for electrical status: supply voltage, current, battery status, etc.
enum I2CTransactionStatus i2c_blocking_transmit(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len, float timeout)
Submit a write only transaction and wait for it to complete.
Definition i2c.c:245
enum I2CTransactionStatus i2c_blocking_transceive(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len_w, uint16_t len_r, float timeout)
Submit a write/read transaction and wait for it to complete.
Definition i2c.c:265
@ I2CTransDone
transaction set to done by user level
Definition i2c.h:60
Hardware independent API for actuators (servos, motor controllers).
int32_t rpm
RPM.
Definition actuators.h:54
struct act_feedback_t::act_feedback_set_t set
Bitset registering what is set as feedback.
uint8_t idx
General index of the actuators (generated in airframe.h).
Definition actuators.h:45
static uint32_t idx
static const ShellCommand commands[]
Definition shell_arch.c:77
struct Stabilization stabilization
General stabilization interface for rotorcrafts.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.