From ab8d1b3b3bd7e945e2a4948be5d453266f2e4de7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Aug 2012 09:08:31 +0200 Subject: Reworked ardrone / multirotor control --- apps/ardrone_control/ardrone_control.c | 44 ++++++++- apps/ardrone_control/ardrone_motor_control.c | 17 ++++ apps/ardrone_control/ardrone_motor_control.h | 12 +++ apps/ardrone_control/attitude_control.c | 135 ++++++++++++++------------- apps/ardrone_control/attitude_control.h | 7 +- 5 files changed, 146 insertions(+), 69 deletions(-) (limited to 'apps') diff --git a/apps/ardrone_control/ardrone_control.c b/apps/ardrone_control/ardrone_control.c index d5c31dc70..d457fc2ec 100644 --- a/apps/ardrone_control/ardrone_control.c +++ b/apps/ardrone_control/ardrone_control.c @@ -114,6 +114,8 @@ int ardrone_control_main(int argc, char *argv[]) char *commandline_usage = "\tusage: ardrone_control -d ardrone-devicename -m mode\n\tmodes are:\n\t\trates\n\t\tattitude\n"; + bool motor_test_mode = false; + /* read commandline arguments */ for (int i = 1; i < argc; i++) { if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //ardrone set @@ -141,6 +143,9 @@ int ardrone_control_main(int argc, char *argv[]) printf(commandline_usage); return ERROR; } + + } else if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) { + motor_test_mode = true; } } @@ -195,7 +200,8 @@ int ardrone_control_main(int argc, char *argv[]) state.state_machine == SYSTEM_STATE_STABILIZED || state.state_machine == SYSTEM_STATE_AUTO || state.state_machine == SYSTEM_STATE_MISSION_ABORT || - state.state_machine == SYSTEM_STATE_EMCY_LANDING) { + state.state_machine == SYSTEM_STATE_EMCY_LANDING || + motor_test_mode) { if (control_mode == CONTROL_MODE_RATES) { orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); @@ -218,13 +224,45 @@ int ardrone_control_main(int argc, char *argv[]) att_sp.roll_body = -manual.roll * M_PI_F / 8.0f; att_sp.pitch_body = -manual.pitch * M_PI_F / 8.0f; att_sp.yaw_body = -manual.yaw * M_PI_F; - att_sp.thrust = manual.throttle/2.0f; + if (motor_test_mode) { + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.3f; + } else { + if (state.state_machine == SYSTEM_STATE_MANUAL || + state.state_machine == SYSTEM_STATE_GROUND_READY || + state.state_machine == SYSTEM_STATE_STABILIZED || + state.state_machine == SYSTEM_STATE_AUTO || + state.state_machine == SYSTEM_STATE_MISSION_ABORT || + state.state_machine == SYSTEM_STATE_EMCY_LANDING) { + att_sp.thrust = manual.throttle/2.0f; + + } else if (state.state_machine == SYSTEM_STATE_EMCY_CUTOFF) { + /* immediately cut off motors */ + att_sp.thrust = 0.0f; + + } else { + /* limit motor throttle to zero for an unknown mode */ + att_sp.thrust = 0.0f; + } + + } + + float roll_control, pitch_control, yaw_control, thrust_control; + + multirotor_control_attitude(&att_sp, &att, &state, motor_test_mode, + &roll_control, &pitch_control, &yaw_control, &thrust_control); + ardrone_mixing_and_output(ardrone_write, roll_control, pitch_control, yaw_control, thrust_control, motor_test_mode); - control_attitude(ardrone_write, &att_sp, &att, &state); } else { /* invalid mode, complain */ if (counter % 200 == 0) printf("[multirotor control] INVALID CONTROL MODE, locking down propulsion\n"); + ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0); } + } else { + /* Silently lock down motor speeds to zero */ + ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0); } if (counter % 30 == 0) { diff --git a/apps/ardrone_control/ardrone_motor_control.c b/apps/ardrone_control/ardrone_motor_control.c index 414ec671f..e9733a1a0 100644 --- a/apps/ardrone_control/ardrone_motor_control.c +++ b/apps/ardrone_control/ardrone_motor_control.c @@ -280,3 +280,20 @@ void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | ((led1_green & 0x01) << 1); write(ardrone_uart, leds, 2); } + +int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) { + const int min_motor_interval = 20000; + static uint64_t last_motor_time = 0; + if (hrt_absolute_time() - last_motor_time > min_motor_interval) { + uint8_t buf[5] = {0}; + ar_get_motor_packet(buf, motor1, motor2, motor3, motor4); + int ret; + if ((ret = write(ardrone_fd, buf, sizeof(buf))) > 0) { + return OK; + } else { + return ret; + } + } else { + return -ERROR; + } +} diff --git a/apps/ardrone_control/ardrone_motor_control.h b/apps/ardrone_control/ardrone_motor_control.h index 8b8aa777b..2e6df0e69 100644 --- a/apps/ardrone_control/ardrone_motor_control.h +++ b/apps/ardrone_control/ardrone_motor_control.h @@ -48,6 +48,9 @@ */ void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4); +/** + * Select a motor in the multiplexing. + */ int ar_select_motor(int fd, uint8_t motor); void ar_enable_broadcast(int fd); @@ -55,6 +58,15 @@ void ar_enable_broadcast(int fd); int ar_multiplexing_init(void); int ar_multiplexing_deinit(int fd); +/** + * Write four motor commands to an already initialized port. + * + * Writing 0 stops a motor, values from 1-512 encode the full thrust range. + * on some motor controller firmware revisions a minimum value of 10 is + * required to spin the motors. + */ +int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4); + /** * Initialize the motors. */ diff --git a/apps/ardrone_control/attitude_control.c b/apps/ardrone_control/attitude_control.c index 37edbf0e5..d31e36e33 100644 --- a/apps/ardrone_control/attitude_control.c +++ b/apps/ardrone_control/attitude_control.c @@ -54,31 +54,22 @@ #include "pid.h" #include -#define CONTROL_PID_ATTITUDE_INTERVAL 5e-3f #define MAX_MOTOR_COUNT 16 -void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status) +void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status, + bool verbose, float* roll_output, float* pitch_output, float* yaw_output, float* thrust_output) { - const unsigned int motor_count = 4; + static uint64_t last_run = 0; + const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); static int motor_skip_counter = 0; static PID_t yaw_pos_controller; static PID_t yaw_speed_controller; - static PID_t nick_controller; + static PID_t pitch_controller; static PID_t roll_controller; - const float min_thrust = 0.02f; /**< 2% minimum thrust */ - const float max_thrust = 1.0f; /**< 100% max thrust */ - const float scaling = 512.0f; /**< 100% thrust equals a value of 512 */ - - 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 */ - static uint16_t motor_pwm[MAX_MOTOR_COUNT]; - static float motor_calc[MAX_MOTOR_COUNT]; - static float pid_yawpos_lim; static float pid_yawspeed_lim; static float pid_att_lim; @@ -102,7 +93,7 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_ global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU], PID_MODE_DERIVATIV_CALC, 155); - pid_init(&nick_controller, + pid_init(&pitch_controller, global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], @@ -137,7 +128,7 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_ global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D], global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU]); - pid_set_parameters(&nick_controller, + pid_set_parameters(&pitch_controller, global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], @@ -156,13 +147,13 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_ /*Calculate Controllers*/ //control Nick - float nick_control = pid_calculate(&nick_controller, att_sp->pitch_body + global_data_parameter_storage->pm.param_values[PARAM_ATT_YOFFSET], - att->pitch, att->pitchspeed, CONTROL_PID_ATTITUDE_INTERVAL); + float pitch_control = pid_calculate(&pitch_controller, att_sp->pitch_body + global_data_parameter_storage->pm.param_values[PARAM_ATT_YOFFSET], + att->pitch, att->pitchspeed, deltaT); //control Roll float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + global_data_parameter_storage->pm.param_values[PARAM_ATT_XOFFSET], - att->roll, att->rollspeed, CONTROL_PID_ATTITUDE_INTERVAL); + att->roll, att->rollspeed, deltaT); //control Yaw Speed - float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, CONTROL_PID_ATTITUDE_INTERVAL); //attitude_setpoint_bodyframe.z is yaw speed! + float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT); //attitude_setpoint_bodyframe.z is yaw speed! /* * compensate the vertical loss of thrust @@ -185,27 +176,12 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_ zcompensation *= 1.0f / cosf(att->pitch); } - float motor_thrust; + float motor_thrust = 0.0f; // FLYING MODES - if (status->state_machine == SYSTEM_STATE_MANUAL || - status->state_machine == SYSTEM_STATE_GROUND_READY || - status->state_machine == SYSTEM_STATE_STABILIZED || - status->state_machine == SYSTEM_STATE_AUTO || - status->state_machine == SYSTEM_STATE_MISSION_ABORT || - status->state_machine == SYSTEM_STATE_EMCY_LANDING) { - motor_thrust = att_sp->thrust; - - } else if (status->state_machine == SYSTEM_STATE_EMCY_CUTOFF) { - /* immediately cut off motors */ - motor_thrust = 0.0f; + motor_thrust = att_sp->thrust; - } else { - /* limit motor throttle to zero for an unknown mode */ - motor_thrust = 0.0f; - } - - printf("mot0: %3.1f\n", motor_thrust); + //printf("mot0: %3.1f\n", motor_thrust); /* compensate thrust vector for roll / pitch contributions */ motor_thrust *= zcompensation; @@ -221,14 +197,14 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_ yaw_speed_controller.saturated = 1; } - if (nick_control > pid_att_lim) { - nick_control = pid_att_lim; - nick_controller.saturated = 1; + if (pitch_control > pid_att_lim) { + pitch_control = pid_att_lim; + pitch_controller.saturated = 1; } - if (nick_control < -pid_att_lim) { - nick_control = -pid_att_lim; - nick_controller.saturated = 1; + if (pitch_control < -pid_att_lim) { + pitch_control = -pid_att_lim; + pitch_controller.saturated = 1; } @@ -242,7 +218,26 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_ roll_controller.saturated = 1; } - printf("mot1: %3.1f\n", motor_thrust); + *roll_output = roll_control; + *pitch_output = pitch_control; + *yaw_output = yaw_rate_control; + *thrust_output = motor_thrust; +} + +void ardrone_mixing_and_output(int ardrone_write, float roll_control, float pitch_control, float yaw_control, float motor_thrust, bool verbose) { + + unsigned int motor_skip_counter = 0; + + const float min_thrust = 0.02f; /**< 2% minimum thrust */ + const float max_thrust = 1.0f; /**< 100% max thrust */ + const float scaling = 512.0f; /**< 100% thrust equals a value of 512 */ + + 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[MAX_MOTOR_COUNT] = {0}; + float motor_calc[MAX_MOTOR_COUNT] = {0}; float output_band = 0.0f; float band_factor = 0.75f; @@ -263,19 +258,23 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_ output_band = band_factor * (max_thrust - motor_thrust); } + if (verbose && motor_skip_counter % 100 == 0) { + printf("1: mot1: %3.1f band: %3.1f r: %3.1f n: %3.1f y: %3.1f\n", (double)motor_thrust, (double)output_band, (double)roll_control, (double)pitch_control, (double)yaw_control); + } + //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 + nick_control / 2 - yaw_rate_control); + motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control); // RIGHT (MOTOR 2) - motor_calc[1] = motor_thrust + (-roll_control / 2 + nick_control / 2 + yaw_rate_control); + motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control); // BACK (MOTOR 3) - motor_calc[2] = motor_thrust + (-roll_control / 2 - nick_control / 2 - yaw_rate_control); + motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control); // LEFT (MOTOR 4) - motor_calc[3] = motor_thrust + (roll_control / 2 - nick_control / 2 + yaw_rate_control); + 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 @@ -285,16 +284,20 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_ yaw_factor = 0.5f; // FRONT (MOTOR 1) - motor_calc[0] = motor_thrust + (roll_control / 2 + nick_control / 2 - yaw_rate_control * yaw_factor); + 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 + nick_control / 2 + yaw_rate_control * yaw_factor); + 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 - nick_control / 2 - yaw_rate_control * yaw_factor); + 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 - nick_control / 2 + yaw_rate_control * yaw_factor); + motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control * yaw_factor); + } + + if (verbose && motor_skip_counter % 100 == 0) { + printf("2: m1: %3.1f m2: %3.1f m3: %3.1f m4: %3.1f\n", (double)motor_calc[0], (double)motor_calc[1], (double)motor_calc[2], (double)motor_calc[3]); } for (int i = 0; i < 4; i++) { @@ -308,13 +311,21 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_ } } + if (verbose && motor_skip_counter % 100 == 0) { + printf("3: band lim: m1: %3.1f m2: %3.1f m3: %3.1f m4: %3.1f\n", (double)motor_calc[0], (double)motor_calc[1], (double)motor_calc[2], (double)motor_calc[3]); + } + /* 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; + 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); + + if (verbose && motor_skip_counter % 100 == 0) { + printf("4: scaled: m1: %d m2: %d m3: %d m4: %d\n", motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); + } /* Keep motors spinning while armed and prevent overflows */ @@ -331,12 +342,8 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_ motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512; /* send motors via UART */ - if (motor_skip_counter % 5 == 0) { - if (motor_skip_counter % 50 == 0) printf("mot: %3.1f-%i-%i-%i-%i\n", motor_thrust, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); - uint8_t buf[5] = {1, 2, 3, 4, 5}; - ar_get_motor_packet(buf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); - write(ardrone_write, buf, sizeof(buf)); - } + if (verbose && motor_skip_counter % 100 == 0) printf("5: mot: %3.1f-%i-%i-%i-%i\n\n", (double)motor_thrust, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); + ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); motor_skip_counter++; } diff --git a/apps/ardrone_control/attitude_control.h b/apps/ardrone_control/attitude_control.h index 1067d362c..56f52dabe 100644 --- a/apps/ardrone_control/attitude_control.h +++ b/apps/ardrone_control/attitude_control.h @@ -52,7 +52,10 @@ #include #include -void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, - const struct vehicle_status_s *status); +void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, + const struct vehicle_status_s *status, bool verbose, + float* roll_output, float* pitch_output, float* yaw_output, float* thrust_output); + +void ardrone_mixing_and_output(int ardrone_write, float roll_control, float pitch_control, float yaw_control, float motor_thrust, bool verbose); #endif /* ATTITUDE_CONTROL_H_ */ -- cgit v1.2.3