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

Fixed code style for multirotor_att_control app

Browse Source
This commit is contained in:
Lorenz Meier
2013-01-11 07:45:24 +01:00
parent cf563eda86
commit cded2787f0
5 changed files with 51 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/multirotor_att_control/multirotor_attitude_control.c
+11 -7
View File
@@ -158,16 +158,18 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
}
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position)
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position)
{
static uint64_t last_run = 0;
static uint64_t last_input = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
last_run = hrt_absolute_time();
if (last_input != att_sp->timestamp) {
last_input = att_sp->timestamp;
}
static int sensor_delay;
sensor_delay = hrt_absolute_time() - att->timestamp;
@@ -189,9 +191,9 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
parameters_update(&h, &p);
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
1000.0f, PID_MODE_DERIVATIV_SET);
1000.0f, PID_MODE_DERIVATIV_SET);
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
1000.0f, PID_MODE_DERIVATIV_SET);
1000.0f, PID_MODE_DERIVATIV_SET);
initialized = true;
}
@@ -207,7 +209,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
}
/* reset integral if on ground */
if(att_sp->thrust < 0.1f) {
if (att_sp->thrust < 0.1f) {
pid_reset_integral(&pitch_controller);
pid_reset_integral(&roll_controller);
}
@@ -217,13 +219,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
/* control pitch (forward) output */
rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body ,
att->pitch, att->pitchspeed, deltaT);
att->pitch, att->pitchspeed, deltaT);
/* control roll (left/right) output */
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body ,
att->roll, att->rollspeed, deltaT);
att->roll, att->rollspeed, deltaT);
if(control_yaw_position) {
if (control_yaw_position) {
/* control yaw rate */
/* positive error: rotate to right, negative error, rotate to left (NED frame) */
@@ -233,12 +235,14 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
if (yaw_error > M_PI_F) {
yaw_error -= M_TWOPI_F;
} else if (yaw_error < -M_PI_F) {
yaw_error += M_TWOPI_F;
}
rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed);
}
rates_sp->thrust = att_sp->thrust;
motor_skip_counter++;