void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators) {
float roll_control = actuators->control[0];
float pitch_control = actuators->control[1];
float yaw_control = actuators->control[2];
float motor_thrust = actuators->control[3];
//printf("AMO: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",roll_control, pitch_control, yaw_control, motor_thrust);
const float min_thrust = 0.02f; /**< 2% minimum thrust */
const float max_thrust = 1.0f; /**< 100% max thrust */
const float scaling = 500.0f; /**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */
const float min_gas = min_thrust * scaling; /**< value range sent to motors, minimum */
const float max_gas = max_thrust * scaling; /**< value range sent to motors, maximum */
/* initialize all fields to zero */
uint16_t motor_pwm[4] = {0};
float motor_calc[4] = {0};
float output_band = 0.0f;
float band_factor = 0.75f;
const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */
float yaw_factor = 1.0f;
if (motor_thrust <= min_thrust) {
motor_thrust = min_thrust;
output_band = 0.0f;
} else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) {
output_band = band_factor * (motor_thrust - min_thrust);
} else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) {
output_band = band_factor * startpoint_full_control;
} else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) {
output_band = band_factor * (max_thrust - motor_thrust);
}
//add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer
// FRONT (MOTOR 1)
motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
// RIGHT (MOTOR 2)
motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
// BACK (MOTOR 3)
motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
// LEFT (MOTOR 4)
motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
// if we are not in the output band
if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band
&& motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band
&& motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band
&& motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) {
yaw_factor = 0.5f;
// FRONT (MOTOR 1)
motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control * yaw_factor);
// RIGHT (MOTOR 2)
motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control * yaw_factor);
// BACK (MOTOR 3)
motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control * yaw_factor);
// LEFT (MOTOR 4)
motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control * yaw_factor);
}
for (int i = 0; i < 4; i++) {
//check for limits
if (motor_calc[i] < motor_thrust - output_band) {
motor_calc[i] = motor_thrust - output_band;
}
if (motor_calc[i] > motor_thrust + output_band) {
motor_calc[i] = motor_thrust + output_band;
}
}
/* set the motor values */
/* scale up from 0..1 to 10..512) */
motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas);
motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas);
motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas);
motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas);
/* Keep motors spinning while armed and prevent overflows */
/* Failsafe logic - should never be necessary */
motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10;
motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10;
motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10;
motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10;
/* Failsafe logic - should never be necessary */
motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512;
motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512;
motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512;
motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512;
/* send motors via UART */
ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
}
int ardrone_interface_thread_main(int argc, char *argv[])
{
thread_running = true;
char *device = "/dev/ttyS1";
/* welcome user */
printf("[ardrone_interface] Control started, taking over motors\n");
/* File descriptors */
int gpios;
char *commandline_usage = "\tusage: ardrone_interface start|status|stop [-t for motor test (10%% thrust)]\n";
bool motor_test_mode = false;
int test_motor = -1;
/* read commandline arguments */
for (int i = 0; i < argc && argv[i]; i++) {
if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) {
motor_test_mode = true;
}
if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--motor") == 0) {
if (i+1 < argc) {
int motor = atoi(argv[i+1]);
if (motor > 0 && motor < 5) {
test_motor = motor;
} else {
thread_running = false;
errx(1, "supply a motor # between 1 and 4. Example: -m 1\n %s", commandline_usage);
}
} else {
thread_running = false;
errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
}
}
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
if (argc > i + 1) {
device = argv[i + 1];
} else {
thread_running = false;
errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
}
}
}
struct termios uart_config_original;
if (motor_test_mode) {
printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n");
}
/* Led animation */
int counter = 0;
int led_counter = 0;
/* declare and safely initialize all structs */
struct vehicle_status_s state;
memset(&state, 0, sizeof(state));
struct actuator_controls_s actuator_controls;
memset(&actuator_controls, 0, sizeof(actuator_controls));
struct actuator_armed_s armed;
armed.armed = false;
/* subscribe to attitude, motor setpoints and system state */
int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
printf("[ardrone_interface] Motors initialized - ready.\n");
fflush(stdout);
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
ardrone_write = ardrone_open_uart(device, &uart_config_original);
/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
gpios = ar_multiplexing_init();
if (ardrone_write < 0) {
fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
thread_running = false;
exit(ERROR);
}
/* initialize motors */
if (OK != ar_init_motors(ardrone_write, gpios)) {
close(ardrone_write);
fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
thread_running = false;
exit(ERROR);
}
ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
// XXX Re-done initialization to make sure it is accepted by the motors
// XXX should be removed after more testing, but no harm
/* close uarts */
close(ardrone_write);
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
ardrone_write = ardrone_open_uart(device, &uart_config_original);
/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
gpios = ar_multiplexing_init();
if (ardrone_write < 0) {
fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
thread_running = false;
exit(ERROR);
}
/* initialize motors */
if (OK != ar_init_motors(ardrone_write, gpios)) {
close(ardrone_write);
fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
thread_running = false;
exit(ERROR);
}
while (!thread_should_exit) {
if (motor_test_mode) {
/* set motors to idle speed */
if (test_motor > 0 && test_motor < 5) {
int motors[4] = {0, 0, 0, 0};
motors[test_motor - 1] = 10;
ardrone_write_motor_commands(ardrone_write, motors[0], motors[1], motors[2], motors[3]);
} else {
ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10);
}
} else {
/* MAIN OPERATION MODE */
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
/* get a local copy of the actuator controls */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
/* for now only spin if armed and immediately shut down
* if in failsafe
*/
if (armed.armed && !armed.lockdown) {
ardrone_mixing_and_output(ardrone_write, &actuator_controls);
} else {
/* Silently lock down motor speeds to zero */
ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
}
}
if (counter % 24 == 0) {
if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0);
if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0);
if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0);
if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0);
if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0);
if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0);
if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0);
if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0);
if (led_counter == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0);
if (led_counter == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1);
if (led_counter == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1);
if (led_counter == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0);
led_counter++;
if (led_counter == 12) led_counter = 0;
}
/* run at approximately 200 Hz */
usleep(4500);
counter++;
}
/* restore old UART config */
int termios_state;
if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) {
fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n");
}
printf("[ardrone_interface] Restored original UART config, exiting..\n");
/* close uarts */
close(ardrone_write);
ar_multiplexing_deinit(gpios);
fflush(stdout);
thread_running = false;
return OK;
}
int ar_init_motors(int ardrone_uart, int gpios)
{
/* Write ARDrone commands on UART2 */
uint8_t initbuf[] = {0xE0, 0x91, 0xA1, 0x00, 0x40};
uint8_t multicastbuf[] = {0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0};
/* deselect all motors */
ar_deselect_motor(gpios, 0);
/* initialize all motors
* - select one motor at a time
* - configure motor
*/
int i;
int errcounter = 0;
/* initial setup run */
for (i = 1; i < 5; ++i) {
/* Initialize motors 1-4 */
errcounter += ar_select_motor(gpios, i);
usleep(200);
/*
* write 0xE0 - request status
* receive one status byte
*/
write(ardrone_uart, &(initbuf[0]), 1);
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US*1);
/*
* write 0x91 - request checksum
* receive 120 status bytes
*/
write(ardrone_uart, &(initbuf[1]), 1);
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US*120);
/*
* write 0xA1 - set status OK
* receive one status byte - should be A0
* to confirm status is OK
*/
write(ardrone_uart, &(initbuf[2]), 1);
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US*1);
/*
* set as motor i, where i = 1..4
* receive nothing
*/
initbuf[3] = (uint8_t)i;
write(ardrone_uart, &(initbuf[3]), 1);
fsync(ardrone_uart);
/*
* write 0x40 - check version
* receive 11 bytes encoding the version
*/
write(ardrone_uart, &(initbuf[4]), 1);
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US*11);
ar_deselect_motor(gpios, i);
/* sleep 200 ms */
usleep(200000);
}
/* start the multicast part */
errcounter += ar_select_motor(gpios, 0);
usleep(200);
/*
* first round
* write six times A0 - enable broadcast
* receive nothing
*/
write(ardrone_uart, multicastbuf, sizeof(multicastbuf));
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf));
/*
* second round
* write six times A0 - enable broadcast
* receive nothing
*/
write(ardrone_uart, multicastbuf, sizeof(multicastbuf));
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf));
/* set motors to zero speed (fsync is part of the write command */
ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0);
if (errcounter != 0) {
fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter);
fflush(stdout);
}
return errcounter;
}
void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators) {
float roll_control = actuators->control[0];
float pitch_control = actuators->control[1];
float yaw_control = actuators->control[2];
float motor_thrust = actuators->control[3];
const float min_thrust = 0.02f; /**< 2% minimum thrust */
const float max_thrust = 1.0f; /**< 100% max thrust */
const float scaling = 510.0f; /**< 100% thrust equals a value of 510 which works, 512 leads to cutoff */
const float min_gas = min_thrust * scaling; /**< value range sent to motors, minimum */
const float max_gas = max_thrust * scaling; /**< value range sent to motors, maximum */
/* initialize all fields to zero */
uint16_t motor_pwm[4] = {0};
float motor_calc[4] = {0};
float output_band = 0.0f;
const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */
/* linearly scale the control inputs from 0 to startpoint_full_control */
if (motor_thrust < startpoint_full_control) {
output_band = motor_thrust/startpoint_full_control; // linear from 0 to 1
} else {
output_band = 1.0f;
}
roll_control *= output_band;
pitch_control *= output_band;
yaw_control *= output_band;
//add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer
// FRONT (MOTOR 1)
motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
// RIGHT (MOTOR 2)
motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
// BACK (MOTOR 3)
motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
// LEFT (MOTOR 4)
motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
/* if one motor is saturated, reduce throttle */
float saturation = fmaxf(fmaxf(motor_calc[0], motor_calc[1]),fmaxf(motor_calc[2], motor_calc[3])) - max_thrust;
if (saturation > 0.0f) {
/* reduce the motor thrust according to the saturation */
motor_thrust = motor_thrust - saturation;
// FRONT (MOTOR 1)
motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
// RIGHT (MOTOR 2)
motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
// BACK (MOTOR 3)
motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
// LEFT (MOTOR 4)
motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
}
/* set the motor values */
/* scale up from 0..1 to 10..500) */
motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas);
motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas);
motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas);
motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas);
/* scale up from 0..1 to 10..500) */
motor_pwm[0] = (uint16_t) (motor_calc[0] * (float)((max_gas - min_gas) + min_gas));
motor_pwm[1] = (uint16_t) (motor_calc[1] * (float)((max_gas - min_gas) + min_gas));
motor_pwm[2] = (uint16_t) (motor_calc[2] * (float)((max_gas - min_gas) + min_gas));
motor_pwm[3] = (uint16_t) (motor_calc[3] * (float)((max_gas - min_gas) + min_gas));
/* Failsafe logic for min values - should never be necessary */
motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : min_gas;
motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : min_gas;
motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : min_gas;
motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : min_gas;
/* Failsafe logic for max values - should never be necessary */
motor_pwm[0] = (motor_pwm[0] <= max_gas) ? motor_pwm[0] : max_gas;
motor_pwm[1] = (motor_pwm[1] <= max_gas) ? motor_pwm[1] : max_gas;
motor_pwm[2] = (motor_pwm[2] <= max_gas) ? motor_pwm[2] : max_gas;
motor_pwm[3] = (motor_pwm[3] <= max_gas) ? motor_pwm[3] : max_gas;
/* send motors via UART */
ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
}