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

Merge branch 'master' of https://github.com/PX4/Firmware

Browse Source 
Conflicts:
	apps/commander/commander.c
	apps/multirotor_att_control/multirotor_att_control_main.c
	apps/multirotor_att_control/multirotor_rate_control.c
This commit is contained in:
tnaegeli
2012-10-04 16:01:42 +02:00
parent b9de72a8c9 2a06b66845
commit 8dfa66cb97
6 changed files with 84 additions and 135 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/multirotor_att_control/multirotor_rate_control.c
+18 -17
View File
@@ -155,32 +155,33 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
static bool initialized = false;
/* initialize the pid controllers when the function is called for the first time */
if (initialized == false) {
parameters_init(&h);
parameters_update(&h, &p);
initialized = true;
}
/* load new parameters with lower rate */
if (motor_skip_counter % 500 == 0) {
if (motor_skip_counter % 2500 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
printf("p.yawrate_p: %8.4f\n", (double)p.yawrate_p);
}
/* calculate current control outputs */
float setpointXrate;
float setpointYrate;
float setpointZrate;
float setRollRate=rate_sp->roll;
float setPitchRate=rate_sp->pitch;
float setYawRate=rate_sp->yaw;
/* control pitch (forward) output */
float pitch_control = p.attrate_p * deltaT * (rate_sp->pitch-rates[1]);
/* control roll (left/right) output */
float roll_control = p.attrate_p * deltaT * (rate_sp->roll-rates[0] );
//x-axis
setpointXrate=p.attrate_p*(setRollRate-rates[0]);
//Y-axis
setpointYrate=p.attrate_p*(setPitchRate-rates[1]);
//Z-axis
setpointZrate=p.yawrate_p*(setYawRate-rates[2]);
/* control yaw rate */
float yaw_rate_control = p.yawrate_p * deltaT * (rate_sp->yaw-rates[2] );
actuators->control[0] = setpointXrate; //roll
actuators->control[1] = setpointYrate; //pitch
actuators->control[2] = setpointZrate; //yaw
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
actuators->control[2] = yaw_rate_control;
actuators->control[3] = rate_sp->thrust;
motor_skip_counter++;