fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 02:01:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

working offboard

Browse Source
This commit is contained in:
tnaegeli
2012-10-09 16:31:04 +02:00
parent f292b03772
commit 613e12fcac
7 changed files with 79 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/multirotor_att_control/multirotor_rate_control.c
+8 -3
View File
@@ -144,6 +144,8 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators)
{
static float roll_control_last=0;
static float pitch_control_last=0;
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
@@ -172,10 +174,13 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
/* calculate current control outputs */
/* 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] );
float pitch_control = p.attrate_p * deltaT *(rate_sp->pitch-rates[1])-p.attrate_d*(pitch_control_last);
pitch_control_last=pitch_control;
/* control roll (left/right) output */
float roll_control = p.attrate_p * deltaT * (rate_sp->roll-rates[0])-p.attrate_d*(roll_control_last);
roll_control_last=roll_control;
/* control yaw rate */
float yaw_rate_control = p.yawrate_p * deltaT * (rate_sp->yaw-rates[2] );