diff options
author | tnaegeli <naegelit@student.ethz.ch> | 2012-10-04 16:01:42 +0200 |
---|---|---|
committer | tnaegeli <naegelit@student.ethz.ch> | 2012-10-04 16:01:42 +0200 |
commit | 8dfa66cb9710f1f5f8baddb6d0b542787af44f15 (patch) | |
tree | 63d99d8861187b891274172a0f95a1fd816c48e9 /apps/multirotor_att_control/multirotor_rate_control.c | |
parent | b9de72a8c9e97165c190020adf2d99849daf5f3a (diff) | |
parent | 2a06b66845542b05e3cad3d21099e33adc213227 (diff) | |
download | px4-firmware-8dfa66cb9710f1f5f8baddb6d0b542787af44f15.tar.gz px4-firmware-8dfa66cb9710f1f5f8baddb6d0b542787af44f15.tar.bz2 px4-firmware-8dfa66cb9710f1f5f8baddb6d0b542787af44f15.zip |
Merge branch 'master' of https://github.com/PX4/Firmware
Conflicts:
apps/commander/commander.c
apps/multirotor_att_control/multirotor_att_control_main.c
apps/multirotor_att_control/multirotor_rate_control.c
Diffstat (limited to 'apps/multirotor_att_control/multirotor_rate_control.c')
-rw-r--r-- | apps/multirotor_att_control/multirotor_rate_control.c | 39 |
1 files changed, 20 insertions, 19 deletions
diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 1d400f51b..372b378d1 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -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; - - //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]); - - actuators->control[0] = setpointXrate; //roll - actuators->control[1] = setpointYrate; //pitch - actuators->control[2] = setpointZrate; //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] ); + + /* control yaw rate */ + float yaw_rate_control = p.yawrate_p * deltaT * (rate_sp->yaw-rates[2] ); + + 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++; |