void actuators_set(bool_t motors_on) {
int32_t pwm_commands[COMMANDS_NB];
int32_t pwm_commands_pprz[COMMANDS_NB];
int32_t booz2_commands[COMMANDS_NB];
pwm_commands[COMMAND_ROLL] = commands[COMMAND_ROLL] * PWM_GAIN_SCALE;
pwm_commands[COMMAND_PITCH] = commands[COMMAND_PITCH] * PWM_GAIN_SCALE;
pwm_commands[COMMAND_YAW] = commands[COMMAND_YAW] * PWM_GAIN_SCALE;
pwm_commands[COMMAND_THRUST] = (commands[COMMAND_THRUST] * ((SUPERVISION_MAX_MOTOR - SUPERVISION_MIN_MOTOR) / 200)) + SUPERVISION_MIN_MOTOR;
pwm_commands_pprz[COMMAND_ROLL] = commands[COMMAND_ROLL] * (MAX_PPRZ / 100);
pwm_commands_pprz[COMMAND_PITCH] = commands[COMMAND_PITCH] * (MAX_PPRZ / 100);
pwm_commands_pprz[COMMAND_YAW] = commands[COMMAND_YAW] * (MAX_PPRZ / 100);
#ifdef USE_CAMERA_MOUNT
pwm_commands_pprz[COMMAND_CAMERA] = commands[COMMAND_CAMERA];
#endif
supervision_run(motors_on, FALSE, pwm_commands);
#ifdef USE_TOYTRONICS
SetCommandsFromRC(pwm_commands_pprz, radio_control.values);
#endif
SetActuatorsFromCommands(pwm_commands_pprz);
if (motors_on) {
for (int i = 0; i < SUPERVISION_NB_MOTOR; i++)
actuators_pwm_values[i] = supervision.commands[i];
} else {
for (int i = 0; i < SUPERVISION_NB_MOTOR; i++)
actuators_pwm_values[i] = PWM_OFF;
}
actuators_pwm_commit();
}
/** @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 nps_autopilot_run_step(double time __attribute__ ((unused))) {
if (nps_radio_control_available(time)) {
radio_control_feed();
main_event();
}
if (nps_sensors_gyro_available()) {
imu_feed_gyro_accel();
main_event();
}
if (nps_sensors_mag_available()) {
imu_feed_mag();
main_event();
}
if (nps_sensors_baro_available()) {
baro_feed_value(sensors.baro.value);
main_event();
}
if (nps_sensors_gps_available()) {
booz_gps_feed_value();
main_event();
}
if (nps_bypass_ahrs) {
sim_overwrite_ahrs();
}
main_periodic();
if (time < 8) { /* start with a little bit of hovering */
int32_t init_cmd[4];
init_cmd[COMMAND_THRUST] = 0.253*SUPERVISION_MAX_MOTOR;
init_cmd[COMMAND_ROLL] = 0;
init_cmd[COMMAND_PITCH] = 0;
init_cmd[COMMAND_YAW] = 0;
supervision_run(TRUE, FALSE, init_cmd);
}
for (uint8_t i=0; i<ACTUATORS_MKK_NB; i++)
autopilot.commands[i] = (double)supervision.commands[i] / SUPERVISION_MAX_MOTOR;
}
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]);
}
}
