fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 20:33:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Massive improvements in state machine, still tracing wrong throttle scaling in manual input path

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-13 18:53:37 +02:00
parent 56b3b46f75
commit 9014577aff
10 changed files with 268 additions and 167 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/ardrone_control/Makefile
+1 -1
View File
@@ -37,7 +37,7 @@
APPNAME = ardrone_control
PRIORITY = SCHED_PRIORITY_MAX - 15
STACKSIZE = 2048
STACKSIZE = 3096
# explicit list of sources - not everything is built currently
CSRCS = ardrone_control.c ardrone_motor_control.c ardrone_control_helper.c rate_control.c attitude_control.c pid.c
apps/ardrone_control/ardrone_control.c
+7 -2
View File
@@ -187,6 +187,9 @@ int ardrone_control_main(int argc, char *argv[])
while (1) {
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
if (state.state_machine == SYSTEM_STATE_MANUAL ||
state.state_machine == SYSTEM_STATE_GROUND_READY ||
state.state_machine == SYSTEM_STATE_STABILIZED ||
@@ -205,8 +208,6 @@ int ardrone_control_main(int argc, char *argv[])
/* hardcore, last-resort safety checking */
//if (status->rc_signal_lost) {
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
/* get a local copy of manual setpoint */
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
/* get a local copy of attitude */
@@ -217,8 +218,12 @@ 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;
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");
}
}
apps/ardrone_control/attitude_control.c
+15 -16
View File
@@ -83,7 +83,7 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
static float pid_yawspeed_lim;
static float pid_att_lim;
static bool initialized;
static bool initialized = false;
/* initialize the pid controllers when the function is called for the first time */
if (initialized == false) {
@@ -117,8 +117,8 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
PID_MODE_DERIVATIV_SET, 157);
pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM];
pid_yawspeed_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
pid_att_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
pid_yawspeed_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
pid_att_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
initialized = true;
}
@@ -150,8 +150,8 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]);
pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM];
pid_yawspeed_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
pid_att_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
pid_yawspeed_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
pid_att_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
}
/*Calculate Controllers*/
@@ -188,17 +188,13 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
float motor_thrust;
// FLYING MODES
if (status->state_machine == SYSTEM_STATE_MANUAL) {
motor_thrust = att_sp->thrust;
} else if (status->state_machine == SYSTEM_STATE_GROUND_READY ||
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) {
motor_thrust = att_sp->thrust; //TODO
} else if (status->state_machine == SYSTEM_STATE_EMCY_LANDING) {
motor_thrust = att_sp->thrust; //TODO
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 */
@@ -209,6 +205,8 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
motor_thrust = 0.0f;
}
printf("mot0: %3.1f\n", motor_thrust);
/* compensate thrust vector for roll / pitch contributions */
motor_thrust *= zcompensation;
@@ -244,6 +242,8 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
roll_controller.saturated = 1;
}
printf("mot1: %3.1f\n", motor_thrust);
float output_band = 0.0f;
float band_factor = 0.75f;
const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */
@@ -332,12 +332,11 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
/* send motors via UART */
if (motor_skip_counter % 5 == 0) {
if (motor_skip_counter % 50) printf("mot: %1.3f-%i-%i-%i-%i\n", motor_thrust, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
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));
}
motor_skip_counter++;
}