diff options
author | Doug Weibel <deweibel@gmail.com> | 2012-10-13 16:38:18 -0600 |
---|---|---|
committer | Doug Weibel <deweibel@gmail.com> | 2012-10-13 16:38:18 -0600 |
commit | 0a0215338a4bd6550805072d036c6cb396e7497a (patch) | |
tree | 498a3bbcb806aba3f890073c9e03410acdfbc733 /apps/multirotor_att_control | |
parent | 77e6375920df67344e895e669dd2a641a7b87b6e (diff) | |
parent | 4dbf7befe369ba00a73945a0193f0a061c271dc3 (diff) | |
download | px4-firmware-0a0215338a4bd6550805072d036c6cb396e7497a.tar.gz px4-firmware-0a0215338a4bd6550805072d036c6cb396e7497a.tar.bz2 px4-firmware-0a0215338a4bd6550805072d036c6cb396e7497a.zip |
Merge branch 'master' of https://github.com/PX4/Firmware
Diffstat (limited to 'apps/multirotor_att_control')
-rw-r--r-- | apps/multirotor_att_control/multirotor_att_control_main.c | 66 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_rate_control.c | 88 |
2 files changed, 55 insertions, 99 deletions
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 18efe1ae2..7d5821d3f 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -165,8 +165,30 @@ mc_thread_main(int argc, char *argv[]) /** STEP 1: Define which input is the dominating control input */ - - if (state.flag_control_manual_enabled) { + if (state.flag_control_offboard_enabled) { + /* offboard inputs */ + if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { + rates_sp.roll = offboard_sp.p1; + rates_sp.pitch = offboard_sp.p2; + rates_sp.yaw = offboard_sp.p3; + rates_sp.thrust = offboard_sp.p4; + printf("thrust_rate=%8.4f\n",offboard_sp.p4); + rates_sp.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); + } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { + att_sp.roll_body = offboard_sp.p1; + att_sp.pitch_body = offboard_sp.p2; + att_sp.yaw_body = offboard_sp.p3; + att_sp.thrust = offboard_sp.p4; + printf("thrust_att=%8.4f\n",offboard_sp.p4); + att_sp.timestamp = hrt_absolute_time(); + /* STEP 2: publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + + /* decide wether we want rate or position input */ + } + else if (state.flag_control_manual_enabled) { /* manual inputs, from RC control or joystick */ if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) { @@ -188,7 +210,7 @@ mc_thread_main(int argc, char *argv[]) } /* STEP 2: publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - + if (motor_test_mode) { att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; @@ -199,38 +221,19 @@ mc_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } - } else if (state.flag_control_offboard_enabled) { - /* offboard inputs */ - if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { - rates_sp.roll = offboard_sp.p1; - rates_sp.pitch = offboard_sp.p2; - rates_sp.yaw = offboard_sp.p3; - rates_sp.thrust = offboard_sp.p4; - rates_sp.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); - } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { - att_sp.roll_body = offboard_sp.p1; - att_sp.pitch_body = offboard_sp.p2; - att_sp.yaw_body = offboard_sp.p3; - att_sp.thrust = offboard_sp.p4; - att_sp.timestamp = hrt_absolute_time(); - /* STEP 2: publish the result to the vehicle actuators */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } - - /* decide wether we want rate or position input */ } /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ + /* run attitude controller */ - if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) { - multirotor_control_attitude(&att_sp, &att, NULL, &actuators); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) { - multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); - } + if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) { + multirotor_control_attitude(&att_sp, &att, NULL, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) { + multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); + } if (state.flag_control_rates_enabled) { @@ -253,7 +256,6 @@ mc_thread_main(int argc, char *argv[]) orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); } - perf_end(mc_loop_perf); } @@ -321,7 +323,7 @@ int multirotor_att_control_main(int argc, char *argv[]) mc_task = task_spawn("multirotor_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 15, - 4096, + 6000, mc_thread_main, NULL); exit(0); diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index d332660f6..7532dffa2 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -56,18 +56,21 @@ // PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.1f); PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.2f); /* 0.15 F405 Flamewheel */ -PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.0f); +//PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); +//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f); PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 8.0f); /**< roughly < 500 deg/s limit */ struct mc_rate_control_params { float yawrate_p; + float yawrate_d; float yawrate_i; float yawrate_awu; float yawrate_lim; float attrate_p; + float attrate_d; float attrate_i; float attrate_awu; float attrate_lim; @@ -79,11 +82,13 @@ struct mc_rate_control_param_handles { param_t yawrate_p; param_t yawrate_i; + param_t yawrate_d; param_t yawrate_awu; param_t yawrate_lim; param_t attrate_p; param_t attrate_i; + param_t attrate_d; param_t attrate_awu; param_t attrate_lim; }; @@ -106,11 +111,13 @@ static int parameters_init(struct mc_rate_control_param_handles *h) /* PID parameters */ h->yawrate_p = param_find("MC_YAWRATE_P"); h->yawrate_i = param_find("MC_YAWRATE_I"); + h->yawrate_d = param_find("MC_YAWRATE_D"); h->yawrate_awu = param_find("MC_YAWRATE_AWU"); h->yawrate_lim = param_find("MC_YAWRATE_LIM"); h->attrate_p = param_find("MC_ATTRATE_P"); h->attrate_i = param_find("MC_ATTRATE_I"); + h->attrate_d = param_find("MC_ATTRATE_D"); h->attrate_awu = param_find("MC_ATTRATE_AWU"); h->attrate_lim = param_find("MC_ATTRATE_LIM"); @@ -121,11 +128,13 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru { param_get(h->yawrate_p, &(p->yawrate_p)); param_get(h->yawrate_i, &(p->yawrate_i)); + param_get(h->yawrate_d, &(p->yawrate_d)); param_get(h->yawrate_awu, &(p->yawrate_awu)); param_get(h->yawrate_lim, &(p->yawrate_lim)); param_get(h->attrate_p, &(p->attrate_p)); param_get(h->attrate_i, &(p->attrate_i)); + param_get(h->attrate_d, &(p->attrate_d)); param_get(h->attrate_awu, &(p->attrate_awu)); param_get(h->attrate_lim, &(p->attrate_lim)); @@ -135,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(); @@ -157,83 +168,26 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - printf("p.yawrate_p: %8.4f", (double)p.yawrate_p); + printf("p.yawrate_p: %8.4f\n", (double)p.yawrate_p); } /* calculate current control outputs */ /* control pitch (forward) output */ - float pitch_control = 5 * deltaT * (rates[1] - rate_sp->pitch); + + 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 * (rates[0] - rate_sp->roll); + 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 * (rates[2] - rate_sp->yaw); - - /* - * compensate the vertical loss of thrust - * when thrust plane has an angle. - * start with a factor of 1.0 (no change) - */ - float zcompensation = 1.0f; - - // if (fabsf(att->roll) > 0.3f) { - // zcompensation *= 1.04675160154f; - - // } else { - // zcompensation *= 1.0f / cosf(att->roll); - // } - - // if (fabsf(att->pitch) > 0.3f) { - // zcompensation *= 1.04675160154f; - - // } else { - // zcompensation *= 1.0f / cosf(att->pitch); - // } - - float motor_thrust = 0.0f; - - motor_thrust = rate_sp->thrust; - - /* compensate thrust vector for roll / pitch contributions */ - motor_thrust *= zcompensation; - - // /* limit yaw rate output */ - // if (yaw_rate_control > p.yawrate_lim) { - // yaw_rate_control = p.yawrate_lim; - // yaw_speed_controller.saturated = 1; - // } - - // if (yaw_rate_control < -p.yawrate_lim) { - // yaw_rate_control = -p.yawrate_lim; - // yaw_speed_controller.saturated = 1; - // } - - // if (pitch_control > p.attrate_lim) { - // pitch_control = p.attrate_lim; - // pitch_controller.saturated = 1; - // } - - // if (pitch_control < -p.attrate_lim) { - // pitch_control = -p.attrate_lim; - // pitch_controller.saturated = 1; - // } - - - // if (roll_control > p.attrate_lim) { - // roll_control = p.attrate_lim; - // roll_controller.saturated = 1; - // } - - // if (roll_control < -p.attrate_lim) { - // roll_control = -p.attrate_lim; - // roll_controller.saturated = 1; - // } + 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] = motor_thrust; + actuators->control[3] = rate_sp->thrust; motor_skip_counter++; } |