Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

Reworked ardrone / multirotor control

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-14 09:08:31 +02:00
parent a9d8a324bc
commit ab8d1b3b3b
5 changed files with 146 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/ardrone_control/ardrone_control.c
+41 -3
View File
@@ -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) {
apps/ardrone_control/ardrone_motor_control.c
+17
View File
@@ -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;
}
}
apps/ardrone_control/ardrone_motor_control.h
+12
View File
@@ -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.
*/
apps/ardrone_control/attitude_control.c
+71 -64
View File
@@ -54,31 +54,22 @@
#include "pid.h"
#include <arch/board/up_hrt.h>
#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;
motor_thrust = att_sp->thrust;
} else if (status->state_machine == SYSTEM_STATE_EMCY_CUTOFF) {
/* immediately cut off motors */
motor_thrust = 0.0f;
} 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++;
}
apps/ardrone_control/attitude_control.h
+5 -2
View File
@@ -52,7 +52,10 @@
#include <uORB/topics/ardrone_control.h>
#include <uORB/topics/vehicle_status.h>
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_ */