diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index b49186ee92..6252178d11 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -235,7 +235,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) for (int i = 0; i < 2; i++) { pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); - pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); @@ -259,7 +259,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) for (int i = 0; i < 2; i++) { pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); - pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max); + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max); } pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); @@ -416,12 +416,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } /* run position & altitude controllers, calculate velocity setpoint */ - global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz, dt); + 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]; if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { /* 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, dt); - global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy, dt); + 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]; + 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]; /* limit horizontal speed */ float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; @@ -439,9 +439,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* publish new velocity setpoint */ orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); - /* run velocity controllers, calculate thrust vector */ + /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt); + thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { /* calculate velocity set point in NED frame */ @@ -452,33 +452,24 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ /* limit horizontal part of thrust */ float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); - float thrust_xy_norm = norm(thrust_sp[0], thrust_sp[1]); + /* assuming that vertical component of thrust is g, + * horizontal component = g * tan(alpha) */ + float tilt = atanf(norm(thrust_sp[0], thrust_sp[1])); - if (thrust_xy_norm > params.slope_max) { - thrust_xy_norm = params.slope_max; + if (tilt > params.tilt_max) { + tilt = params.tilt_max; } - /* use approximation: slope ~ sin(slope) = force */ /* convert direction to body frame */ thrust_xy_dir -= att.yaw; if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { /* calculate roll and pitch */ - att_sp.roll_body = sinf(thrust_xy_dir) * thrust_xy_norm; - att_sp.pitch_body = -cosf(thrust_xy_dir) * thrust_xy_norm / cosf(att_sp.roll_body); // reverse pitch + att_sp.roll_body = sinf(thrust_xy_dir) * tilt; + att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body); } - /* attitude-thrust compensation */ - float att_comp; - - if (att.R[2][2] > 0.8f) - att_comp = 1.0f / att.R[2][2]; - else if (att.R[2][2] > 0.0f) - att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * att.R[2][2] + 1.0f; - else - att_comp = 1.0f; - - att_sp.thrust = -thrust_sp[2] * att_comp; + att_sp.thrust = -thrust_sp[2]; att_sp.timestamp = hrt_absolute_time(); if (status.flag_control_manual_enabled) { 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 f8a982c6c0..1094fd23b7 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -56,7 +56,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 10.0f); -PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.5f); +PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); int parameters_init(struct multirotor_position_control_param_handles *h) { @@ -74,7 +74,7 @@ int parameters_init(struct multirotor_position_control_param_handles *h) 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->slope_max = param_find("MPC_SLOPE_MAX"); + h->tilt_max = param_find("MPC_TILT_MAX"); h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); h->rc_scale_roll = param_find("RC_SCALE_ROLL"); @@ -99,7 +99,7 @@ int parameters_update(const struct multirotor_position_control_param_handles *h, 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->slope_max, &(p->slope_max)); + param_get(h->tilt_max, &(p->tilt_max)); param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); param_get(h->rc_scale_roll, &(p->rc_scale_roll)); 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 e9b1f5c390..14a3de2e59 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -56,7 +56,7 @@ struct multirotor_position_control_params { float xy_vel_i; float xy_vel_d; float xy_vel_max; - float slope_max; + float tilt_max; float rc_scale_pitch; float rc_scale_roll; @@ -78,7 +78,7 @@ struct multirotor_position_control_param_handles { param_t xy_vel_i; param_t xy_vel_d; param_t xy_vel_max; - param_t slope_max; + param_t tilt_max; param_t rc_scale_pitch; param_t rc_scale_roll; diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c index 89efe1334e..51dcd7df24 100644 --- a/src/modules/multirotor_pos_control/thrust_pid.c +++ b/src/modules/multirotor_pos_control/thrust_pid.c @@ -1,11 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Anton Babushkin - * Julian Oes + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,13 +35,9 @@ /** * @file thrust_pid.c * - * Implementation of generic PID control interface. + * Implementation of thrust control PID. * - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann * @author Anton Babushkin - * @author Julian Oes */ #include "thrust_pid.h" @@ -108,16 +100,18 @@ __EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, fl return ret; } -__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt) +__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22) { /* Alternative integral component calculation - error = setpoint - actual_position - integral = integral + (Ki*error*dt) - derivative = (error - previous_error)/dt - output = (Kp*error) + integral + (Kd*derivative) - previous_error = error - wait(dt) - goto start + * + * start: + * error = setpoint - current_value + * integral = integral + (Ki * error * dt) + * derivative = (error - previous_error) / dt + * previous_error = error + * output = (Kp * error) + integral + (Kd * derivative) + * wait(dt) + * goto start */ if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) { @@ -147,21 +141,32 @@ __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, floa d = 0.0f; } - // Calculate the error integral and check for saturation + /* calculate the error integral */ i = pid->integral + (pid->ki * error * dt); - float output = (error * pid->kp) + i + (d * pid->kd); + /* attitude-thrust compensation + * r22 is (2, 2) componet of rotation matrix for current attitude */ + float att_comp; + + if (r22 > 0.8f) + att_comp = 1.0f / r22; + else if (r22 > 0.0f) + att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f; + else + att_comp = 1.0f; + + /* calculate output */ + float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp; + + /* check for saturation */ if (output < pid->limit_min || output > pid->limit_max) { - i = pid->integral; // If saturated then do not update integral value - // recalculate output with old integral - output = (error * pid->kp) + i + (d * pid->kd); + /* saturated, recalculate output with old integral */ + output = (error * pid->kp) + pid->integral + (d * pid->kd); } else { - if (!isfinite(i)) { - i = 0.0f; + if (isfinite(i)) { + pid->integral = i; } - - pid->integral = i; } if (isfinite(output)) { diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h index 65ee33c51c..551c032fe2 100644 --- a/src/modules/multirotor_pos_control/thrust_pid.h +++ b/src/modules/multirotor_pos_control/thrust_pid.h @@ -68,7 +68,7 @@ typedef struct { __EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min); __EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max); -__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt); +__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22); __END_DECLS