/** @file actuators_asctec.c
* Actuators driver for Asctec motor controllers.
*/
#include "firmwares/rotorcraft/actuators.h"
#include "firmwares/rotorcraft/actuators/actuators_asctec.h"
#ifdef ACTUATORS_ASCTEC_V2_PROTOCOL
#include "firmwares/rotorcraft/actuators/supervision.h"
#endif
#include "firmwares/rotorcraft/commands.h"
#include "mcu_periph/i2c.h"
#include "mcu_periph/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 ACTUATORS_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 ACTUATORS_START_DELAY && ! defined SITL
if (!actuators_delay_done) {
if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_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] = commands[COMMAND_PITCH] + SUPERVISION_TRIM_E;
actuators_asctec.cmds[ROLL] = commands[COMMAND_ROLL] + SUPERVISION_TRIM_A;
actuators_asctec.cmds[YAW] = commands[COMMAND_YAW] + SUPERVISION_TRIM_R;
actuators_asctec.cmds[THRUST] = 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 defined ACTUATORS_START_DELAY && ! defined SITL
if (!actuators_delay_done) {
if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) return;
else actuators_delay_done = TRUE;
}
#endif
supervision_run(motors_on, FALSE, 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);
}
void actuators_mkk_v2_set(void)
{
#if defined ACTUATORS_START_DELAY && ! defined SITL
if (!actuators_delay_done) {
if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) { return; }
else { actuators_delay_done = TRUE; }
}
#endif
// Read result
for (uint8_t i = 0; i < ACTUATORS_MKK_V2_NB; i++) {
if (actuators_mkk_v2.trans[i].type != I2CTransTx) {
actuators_mkk_v2.trans[i].type = I2CTransTx;
actuators_mkk_v2.data[i].Current = actuators_mkk_v2.trans[i].buf[0];
actuators_mkk_v2.data[i].MaxPWM = actuators_mkk_v2.trans[i].buf[1];
actuators_mkk_v2.data[i].Temperature = actuators_mkk_v2.trans[i].buf[2];
}
}
RunOnceEvery(10, actuators_mkk_v2_read());
for (uint8_t i = 0; i < ACTUATORS_MKK_V2_NB; i++) {
#ifdef KILL_MOTORS
actuators_mkk_v2.trans[i].buf[0] = 0;
actuators_mkk_v2.trans[i].buf[1] = 0;
#else
actuators_mkk_v2.trans[i].buf[0] = (actuators_mkk_v2.setpoint[i] >> 3);
actuators_mkk_v2.trans[i].buf[1] = actuators_mkk_v2.setpoint[i] & 0x07;
#endif
i2c_submit(&ACTUATORS_MKK_V2_DEVICE, &actuators_mkk_v2.trans[i]);
}
}
void baro_ets_read_periodic( void ) {
// Initiate next read
if (!baro_ets_delay_done) {
if (SysTimeTimer(baro_ets_delay_time) < USEC_OF_SEC(BARO_ETS_START_DELAY)) return;
else baro_ets_delay_done = TRUE;
}
if (baro_ets_i2c_trans.status == I2CTransDone) {
i2c_receive(&BARO_ETS_I2C_DEV, &baro_ets_i2c_trans, BARO_ETS_ADDR, 2);
}
}
void gps_sim_hitl_event(void)
{
if (SysTimeTimer(gps_sim_hitl_timer) > 100000) {
SysTimeTimerStart(gps_sim_hitl_timer);
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
if (state.ned_initialized_i) {
if (!autopilot_in_flight) {
struct Int32Vect2 zero_vector;
INT_VECT2_ZERO(zero_vector);
gh_set_ref(zero_vector, zero_vector, zero_vector);
INT_VECT2_ZERO(guidance_h.ref.pos);
INT_VECT2_ZERO(guidance_h.ref.speed);
INT_VECT2_ZERO(guidance_h.ref.accel);
gv_set_ref(0, 0, 0);
guidance_v_z_ref = 0;
guidance_v_zd_ref = 0;
guidance_v_zdd_ref = 0;
}
struct NedCoor_i ned_c;
ned_c.x = guidance_h.ref.pos.x * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM;
ned_c.y = guidance_h.ref.pos.y * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM;
ned_c.z = guidance_v_z_ref * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM;
ecef_of_ned_point_i(&gps.ecef_pos, &state.ned_origin_i, &ned_c);
gps.lla_pos.alt = state.ned_origin_i.lla.alt - ned_c.z;
gps.hmsl = state.ned_origin_i.hmsl - ned_c.z;
ned_c.x = guidance_h.ref.speed.x * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM;
ned_c.y = guidance_h.ref.speed.y * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM;
ned_c.z = guidance_v_zd_ref * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM;
ecef_of_ned_vect_i(&gps.ecef_vel, &state.ned_origin_i, &ned_c);
gps.fix = GPS_FIX_3D;
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
gps_available = true;
}
else {
struct Int32Vect2 zero_vector;
INT_VECT2_ZERO(zero_vector);
gh_set_ref(zero_vector, zero_vector, zero_vector);
gv_set_ref(0, 0, 0);
}
// publish gps data
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
if (gps.fix == GPS_FIX_3D) {
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
}
AbiSendMsgGPS(GPS_SIM_ID, now_ts, &gps);
}
}
void airspeed_ets_read_periodic( void ) {
#ifndef SITL
if (!airspeed_ets_delay_done) {
if (SysTimeTimer(airspeed_ets_delay_time) < USEC_OF_SEC(AIRSPEED_ETS_START_DELAY)) return;
else airspeed_ets_delay_done = TRUE;
}
if (airspeed_ets_i2c_trans.status == I2CTransDone)
i2c_receive(&AIRSPEED_ETS_I2C_DEV, &airspeed_ets_i2c_trans, AIRSPEED_ETS_ADDR, 2);
#elif !defined USE_NPS
extern float sim_air_speed;
stateSetAirspeed_f(&sim_air_speed);
#endif //SITL
}
void i2c_event(void) {
static uint32_t i2c_wd_timer;
if (SysTimeTimer(i2c_wd_timer) > 2000) { // 2ms (500Hz) periodic watchdog check
SysTimeTimerStart(i2c_wd_timer);
#if USE_I2C1
i2c_wd_check(&i2c1);
#endif
#if USE_I2C2
i2c_wd_check(&i2c2);
#endif
#if USE_I2C3
i2c_wd_check(&i2c3);
#endif
}
}
void actuators_set(bool_t motors_on) {
#if defined ACTUATORS_START_DELAY && ! defined SITL
if (!actuators_delay_done) {
if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) return;
else actuators_delay_done = TRUE;
}
#endif
supervision_run(motors_on, FALSE, 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]);
}
}
void actuators_mkk_set(void) {
const uint8_t actuators_addr[ACTUATORS_MKK_NB] = ACTUATORS_MKK_ADDR;
#if defined ACTUATORS_START_DELAY && ! defined SITL
if (!actuators_delay_done) {
if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) return;
else actuators_delay_done = TRUE;
}
#endif
for (uint8_t i=0; i<ACTUATORS_MKK_NB; i++) {
#ifdef KILL_MOTORS
actuators_mkk.trans[i].buf[0] = 0;
#endif
i2c_transmit(&ACTUATORS_MKK_DEVICE, &actuators_mkk.trans[i], actuators_addr[i], 1);
}
}
void actuators_asctec_set(bool_t motors_on) {
#if defined ACTUATORS_START_DELAY && ! defined SITL
if (!actuators_delay_done) {
if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) return;
else actuators_delay_done = TRUE;
}
#endif
switch (actuators_asctec.i2c_trans.status) {
case I2CTransFailed:
actuators_asctec.nb_err++;
actuators_asctec.i2c_trans.status = I2CTransDone;
break;
case I2CTransSuccess:
case I2CTransDone:
actuators_asctec.i2c_trans.status = I2CTransDone;
break;
default:
actuators_asctec.nb_err++;
return;
}
#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 */
Bound(actuators_asctec.cmds[PITCH],ASCTEC_MIN_CMD, ASCTEC_MAX_CMD);
Bound(actuators_asctec.cmds[ROLL], ASCTEC_MIN_CMD, ASCTEC_MAX_CMD);
Bound(actuators_asctec.cmds[YAW], ASCTEC_MIN_CMD, ASCTEC_MAX_CMD);
if (motors_on) {
Bound(actuators_asctec.cmds[THRUST], ASCTEC_MIN_THROTTLE + 1, ASCTEC_MAX_THROTTLE);
}
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_transmit(&ACTUATORS_ASCTEC_I2C_DEV, &actuators_asctec.i2c_trans,
ACTUATORS_ASCTEC_SLAVE_ADDR, 4);
}
void actuators_asctec_v2_set(void) {
#if defined ACTUATORS_START_DELAY && ! defined SITL
if (!actuators_delay_done) {
if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) {
#ifdef USE_I2C_ACTUATORS_REBOOT_HACK
//Lisa-L with Asctech v2 motors only start after reflashing when a bus error was sensed on stm32-i2c.
//multiple re-init solves the problem.
i2c1_init();
#endif
return;
}
else actuators_delay_done = TRUE;
}
#endif
switch (actuators_asctec_v2.i2c_trans.status) {
case I2CTransFailed:
actuators_asctec_v2.nb_err++;
actuators_asctec_v2.i2c_trans.status = I2CTransDone;
break;
case I2CTransSuccess:
case I2CTransDone:
actuators_asctec_v2.i2c_trans.status = I2CTransDone;
break;
default:
actuators_asctec_v2.nb_err++;
return;
}
#ifdef KILL_MOTORS
actuators_asctec_v2.i2c_trans.buf[0] = 0;
actuators_asctec_v2.i2c_trans.buf[1] = 0;
actuators_asctec_v2.i2c_trans.buf[2] = 0;
actuators_asctec_v2.i2c_trans.buf[3] = 0;
actuators_asctec_v2.i2c_trans.buf[4] = 0xAA;
#else
switch (actuators_asctec_v2.cmd) {
case TEST:
actuators_asctec_v2.i2c_trans.buf[0] = 251;
actuators_asctec_v2.i2c_trans.buf[1] = actuators_asctec_v2.cur_addr;
actuators_asctec_v2.i2c_trans.buf[2] = 0;
actuators_asctec_v2.i2c_trans.buf[3] = 231 + actuators_asctec_v2.cur_addr;
actuators_asctec_v2.i2c_trans.len_w = 4;
break;
case REVERSE:
actuators_asctec_v2.i2c_trans.buf[0] = 254;
actuators_asctec_v2.i2c_trans.buf[1] = actuators_asctec_v2.cur_addr;
actuators_asctec_v2.i2c_trans.buf[2] = 0;
actuators_asctec_v2.i2c_trans.buf[3] = 234 + actuators_asctec_v2.cur_addr;
actuators_asctec_v2.i2c_trans.len_w = 4;
break;
case SET_ADDR:
actuators_asctec_v2.i2c_trans.buf[0] = 250;
actuators_asctec_v2.i2c_trans.buf[1] = actuators_asctec_v2.cur_addr;
actuators_asctec_v2.i2c_trans.buf[2] = actuators_asctec_v2.new_addr;
actuators_asctec_v2.i2c_trans.buf[3] = 230 + actuators_asctec_v2.cur_addr +
actuators_asctec_v2.new_addr;
actuators_asctec_v2.cur_addr = actuators_asctec_v2.new_addr;
actuators_asctec_v2.i2c_trans.len_w = 4;
break;
case NONE:
actuators_asctec_v2.i2c_trans.buf[0] = actuators_asctec_v2.cmds[SERVO_FRONT];
actuators_asctec_v2.i2c_trans.buf[1] = actuators_asctec_v2.cmds[SERVO_BACK];
actuators_asctec_v2.i2c_trans.buf[2] = actuators_asctec_v2.cmds[SERVO_LEFT];
actuators_asctec_v2.i2c_trans.buf[3] = actuators_asctec_v2.cmds[SERVO_RIGHT];
actuators_asctec_v2.i2c_trans.buf[4] = 0xAA + actuators_asctec_v2.i2c_trans.buf[0] +
actuators_asctec_v2.i2c_trans.buf[1] +
actuators_asctec_v2.i2c_trans.buf[2] +
actuators_asctec_v2.i2c_trans.buf[3];
actuators_asctec_v2.i2c_trans.len_w = 5;
break;
default:
break;
}
actuators_asctec_v2.cmd = NONE;
#endif
actuators_asctec_v2.i2c_trans.type = I2CTransTx; // Can be reset I2C driver
i2c_submit(&ACTUATORS_ASCTEC_V2_DEVICE, &actuators_asctec_v2.i2c_trans);
}
void actuators_set(bool_t motors_on) {
#if defined ACTUATORS_START_DELAY && ! defined SITL
if (!actuators_delay_done) {
if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_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] = commands[COMMAND_PITCH] + SUPERVISION_TRIM_E;
actuators_asctec.cmds[ROLL] = commands[COMMAND_ROLL] + SUPERVISION_TRIM_A;
actuators_asctec.cmds[YAW] = commands[COMMAND_YAW] + SUPERVISION_TRIM_R;
actuators_asctec.cmds[THRUST] = 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);
}