diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-22 22:41:46 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-22 22:41:46 +0400 |
commit | 4c6f6ed12cc90a7a0365fc22b56c59dfa3d55028 (patch) | |
tree | c5cefb1e859eac52174cd03f8fb59af33a4f14c1 /src/modules/multirotor_pos_control | |
parent | fae6c694239194bbdbc7c872bb1a1d11a8894474 (diff) | |
download | px4-firmware-4c6f6ed12cc90a7a0365fc22b56c59dfa3d55028.tar.gz px4-firmware-4c6f6ed12cc90a7a0365fc22b56c59dfa3d55028.tar.bz2 px4-firmware-4c6f6ed12cc90a7a0365fc22b56c59dfa3d55028.zip |
multirotor_pos_control: remove unused parameters, use ellipsoid for velocity setpoint limiting
Diffstat (limited to 'src/modules/multirotor_pos_control')
3 files changed, 20 insertions, 49 deletions
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 74ae5947f..378d3fda7 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -173,6 +173,11 @@ static float norm(float x, float y) return sqrtf(x * x + y * y); } +static float norm3(float x, float y, float z) +{ + return sqrtf(x * x + y * y + z * z); +} + static void cross3(float a[3], float b[3], float res[3]) { res[0] = a[1] * b[2] - a[2] * b[1]; @@ -298,8 +303,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) hrt_abstime ref_alt_t = 0; uint64_t local_ref_timestamp = 0; - PID_t xy_pos_pids[2]; - PID_t z_pos_pid; float thrust_int[3] = { 0.0f, 0.0f, 0.0f }; thread_running = true; @@ -309,12 +312,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) parameters_init(¶ms_h); parameters_update(¶ms_h, ¶ms); - for (int i = 0; i < 2; i++) { - pid_init(&(xy_pos_pids[i]), PID_MODE_DERIVATIV_SET, 0.02f); - } - - pid_init(&z_pos_pid, PID_MODE_DERIVATIV_SET, 0.02f); - bool param_updated = true; while (!thread_should_exit) { @@ -329,12 +326,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* update params */ parameters_update(¶ms_h, ¶ms); - - for (int i = 0; i < 2; i++) { - pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 0.0f, 0.0f); - } - - pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); } bool updated; @@ -362,6 +353,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { dt = 0.0f; } + t_prev = t; if (control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ @@ -376,8 +368,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) was_armed = control_mode.flag_armed; - t_prev = t; - if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_position_enabled || control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); @@ -612,7 +602,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* run position & altitude controllers, calculate velocity setpoint */ if (control_mode.flag_control_altitude_enabled) { - global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2] * params.z_ff; + global_vel_sp.vz = (local_pos_sp.z - local_pos.z) * params.z_p + sp_move_rate[2] * params.z_ff; } else { reset_man_sp_z = true; @@ -621,18 +611,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (control_mode.flag_control_position_enabled) { /* calculate velocity set point in NED frame */ - global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0] * params.xy_ff; - global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1] * params.xy_ff; - - if (!control_mode.flag_control_manual_enabled) { - /* limit horizontal speed only in AUTO mode */ - float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; - - if (xy_vel_sp_norm > 1.0f) { - global_vel_sp.vx /= xy_vel_sp_norm; - global_vel_sp.vy /= xy_vel_sp_norm; - } - } + global_vel_sp.vx = (local_pos_sp.x - local_pos.x) * params.xy_p + sp_move_rate[0] * params.xy_ff; + global_vel_sp.vy = (local_pos_sp.y - local_pos.y) * params.xy_p + sp_move_rate[1] * params.xy_ff; } else { reset_man_sp_xy = true; @@ -640,6 +620,17 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) global_vel_sp.vy = 0.0f; } + if (!control_mode.flag_control_manual_enabled) { + /* limit 3D speed only in AUTO mode */ + float vel_sp_norm = norm3(global_vel_sp.vx / params.xy_vel_max, global_vel_sp.vy / params.xy_vel_max, global_vel_sp.vz / params.z_vel_max); + + if (vel_sp_norm > 1.0f) { + global_vel_sp.vx /= vel_sp_norm; + global_vel_sp.vy /= vel_sp_norm; + global_vel_sp.vz /= vel_sp_norm; + } + } + /* publish new velocity setpoint */ orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); // TODO subscribe to velocity setpoint if altitude/position control disabled diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 1c798b007..bebcdb3f5 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -44,17 +44,13 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f); PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f); PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); -PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.2f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.1f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f); PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f); -PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.05f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); @@ -66,17 +62,13 @@ int parameters_init(struct multirotor_position_control_param_handles *h) h->thr_min = param_find("MPC_THR_MIN"); h->thr_max = param_find("MPC_THR_MAX"); h->z_p = param_find("MPC_Z_P"); - h->z_d = param_find("MPC_Z_D"); h->z_vel_p = param_find("MPC_Z_VEL_P"); h->z_vel_i = param_find("MPC_Z_VEL_I"); - h->z_vel_d = param_find("MPC_Z_VEL_D"); h->z_vel_max = param_find("MPC_Z_VEL_MAX"); h->z_ff = param_find("MPC_Z_FF"); h->xy_p = param_find("MPC_XY_P"); - h->xy_d = param_find("MPC_XY_D"); h->xy_vel_p = param_find("MPC_XY_VEL_P"); h->xy_vel_i = param_find("MPC_XY_VEL_I"); - h->xy_vel_d = param_find("MPC_XY_VEL_D"); h->xy_vel_max = param_find("MPC_XY_VEL_MAX"); h->xy_ff = param_find("MPC_XY_FF"); h->tilt_max = param_find("MPC_TILT_MAX"); @@ -95,17 +87,13 @@ int parameters_update(const struct multirotor_position_control_param_handles *h, param_get(h->thr_min, &(p->thr_min)); param_get(h->thr_max, &(p->thr_max)); param_get(h->z_p, &(p->z_p)); - param_get(h->z_d, &(p->z_d)); param_get(h->z_vel_p, &(p->z_vel_p)); param_get(h->z_vel_i, &(p->z_vel_i)); - param_get(h->z_vel_d, &(p->z_vel_d)); param_get(h->z_vel_max, &(p->z_vel_max)); param_get(h->z_ff, &(p->z_ff)); param_get(h->xy_p, &(p->xy_p)); - param_get(h->xy_d, &(p->xy_d)); param_get(h->xy_vel_p, &(p->xy_vel_p)); param_get(h->xy_vel_i, &(p->xy_vel_i)); - param_get(h->xy_vel_d, &(p->xy_vel_d)); param_get(h->xy_vel_max, &(p->xy_vel_max)); param_get(h->xy_ff, &(p->xy_ff)); param_get(h->tilt_max, &(p->tilt_max)); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 804716d11..37a925dcc 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -46,17 +46,13 @@ struct multirotor_position_control_params { float thr_min; float thr_max; float z_p; - float z_d; float z_vel_p; float z_vel_i; - float z_vel_d; float z_vel_max; float z_ff; float xy_p; - float xy_d; float xy_vel_p; float xy_vel_i; - float xy_vel_d; float xy_vel_max; float xy_ff; float tilt_max; @@ -72,17 +68,13 @@ struct multirotor_position_control_param_handles { param_t thr_min; param_t thr_max; param_t z_p; - param_t z_d; param_t z_vel_p; param_t z_vel_i; - param_t z_vel_d; param_t z_vel_max; param_t z_ff; param_t xy_p; - param_t xy_d; param_t xy_vel_p; param_t xy_vel_i; - param_t xy_vel_d; param_t xy_vel_max; param_t xy_ff; param_t tilt_max; |