/*
* Output functions
*
*/
void PID_lacet_Outputs_wrapper(const real_T *t,
const real_T *kp,
const real_T *ki,
const real_T *kd,
const real_T *rc,
const real_T *rmes,
real_T *com)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
/* This sample sets the output equal to the input
y0[0] = u0[0];
For complex signals use: y0[0].re = u0[0].re;
y0[0].im = u0[0].im;
y1[0].re = u1[0].re;
y1[0].im = u1[0].im;
*/
if (t[0]<=0.01){
t_av=0;
mon_pid = &mon_pid_t;
pid_init(mon_pid, kp[0], ki[0], kd[0], 1000, 1000, PID_MODE_DERIVATIV_SET, 0.00001);
pid_reset_integral(mon_pid);
pid_set_parameters(mon_pid, kp[0], ki[0], kd[0], 1000, 1000);
//pid_init(mon_pid, 6.8, 0, 0, 1000, 1000, PID_MODE_DERIVATIV_SET, 0.1);
//pid_set_parameters(mon_pid, 6.56, 0, 0, 4096, 4096);
toto++;
}
//com[0]=thetames[0];
dt=t[0]-t_av;
t_av=t[0];
com[0]=pid_calculate(mon_pid, rc[0] ,rmes[0], 0, dt);
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
}
void pid_cascade_control(struct pid_cascade_s *ctrl)
{
// position control
float pos_ctrl_vel;
if (ctrl->setpts.position_control_enabled) {
ctrl->position_setpoint = ctrl->setpts.position_setpt;
float position_error = ctrl->position - ctrl->setpts.position_setpt;
if (ctrl->periodic_actuator) {
position_error = periodic_error(position_error);
}
ctrl->position_error = position_error;
pos_ctrl_vel = pid_process(&ctrl->position_pid, ctrl->position_error);
ctrl->position_ctrl_out = pos_ctrl_vel;
} else {
pid_reset_integral(&ctrl->position_pid);
pos_ctrl_vel = 0;
}
// velocity control
float vel_ctrl_torque;
if (ctrl->setpts.velocity_control_enabled) {
float velocity_setpt = ctrl->setpts.velocity_setpt + pos_ctrl_vel;
velocity_setpt = filter_limit_sym(velocity_setpt, ctrl->velocity_limit);
ctrl->velocity_setpoint = velocity_setpt;
ctrl->velocity_error = ctrl->velocity - velocity_setpt;
vel_ctrl_torque = pid_process(&ctrl->velocity_pid, ctrl->velocity_error);
ctrl->velocity_ctrl_out = vel_ctrl_torque;
} else {
pid_reset_integral(&ctrl->velocity_pid);
vel_ctrl_torque = 0;
}
// torque control
float torque_setpt = vel_ctrl_torque + ctrl->setpts.feedforward_torque;
torque_setpt = filter_limit_sym(torque_setpt, ctrl->torque_limit);
float current_setpt = torque_setpt * ctrl->motor_current_constant;
current_setpt = filter_limit_sym(current_setpt, ctrl->current_limit);
ctrl->current_setpoint = current_setpt;
ctrl->current_error = ctrl->current - current_setpt;
ctrl->motor_voltage = pid_process(&ctrl->current_pid, ctrl->current_error);
}
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators, bool reset_integral)
{
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
static uint64_t last_input = 0;
if (last_input != rate_sp->timestamp) {
last_input = rate_sp->timestamp;
}
last_run = hrt_absolute_time();
static int motor_skip_counter = 0;
static PID_t pitch_rate_controller;
static PID_t roll_rate_controller;
static struct mc_rate_control_params p;
static struct mc_rate_control_param_handles h;
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;
pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
}
/* load new parameters with lower rate */
if (motor_skip_counter % 2500 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
}
/* reset integrals if needed */
if (reset_integral) {
pid_reset_integral(&pitch_rate_controller);
pid_reset_integral(&roll_rate_controller);
// TODO pid_reset_integral(&yaw_rate_controller);
}
/* control pitch (forward) output */
float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch ,
rates[1], 0.0f, deltaT);
/* control roll (left/right) output */
float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll ,
rates[0], 0.0f, deltaT);
/* control yaw rate */ //XXX use library here
float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
/* increase resilience to faulty control inputs */
if (!isfinite(yaw_rate_control)) {
yaw_rate_control = 0.0f;
warnx("rej. NaN ctrl yaw");
}
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++;
}
void *multirotor_position_control_thread_main()
{
/* welcome user */
fprintf (stdout, "Multirotor position controller started\n");
fflush(stdout);
int i;
bool_t updated;
bool_t reset_mission_sp = 0 /* false */;
bool_t global_pos_sp_valid = 0 /* false */;
bool_t reset_man_sp_z = 1 /* true */;
bool_t reset_man_sp_xy = 1 /* true */;
bool_t reset_int_z = 1 /* true */;
bool_t reset_int_z_manual = 0 /* false */;
bool_t reset_int_xy = 1 /* true */;
bool_t was_armed = 0 /* false */;
bool_t reset_auto_sp_xy = 1 /* true */;
bool_t reset_auto_sp_z = 1 /* true */;
bool_t reset_takeoff_sp = 1 /* true */;
absolute_time t_prev = 0;
const float alt_ctl_dz = 0.2f;
const float pos_ctl_dz = 0.05f;
float i_limit; /* use integral_limit_out = tilt_max / 2 */
float ref_alt = 0.0f;
absolute_time ref_alt_t = 0;
absolute_time local_ref_timestamp = 0;
PID_t xy_pos_pids[2];
PID_t xy_vel_pids[2];
PID_t z_pos_pid;
thrust_pid_t z_vel_pid;
float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
/* structures */
struct parameter_update_s ps;
struct vehicle_control_flags_s control_flags;
memset(&control_flags, 0, sizeof(control_flags));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
struct vehicle_local_position_s local_pos;
memset(&local_pos, 0, sizeof(local_pos));
struct vehicle_local_position_setpoint_s local_pos_sp;
memset(&local_pos_sp, 0, sizeof(local_pos_sp));
struct vehicle_global_position_setpoint_s global_pos_sp;
memset(&global_pos_sp, 0, sizeof(global_pos_sp));
struct vehicle_global_velocity_setpoint_s global_vel_sp;
memset(&global_vel_sp, 0, sizeof(global_vel_sp));
/* subscribe to attitude, motor setpoints and system state */
orb_subscr_t param_sub = orb_subscribe(ORB_ID(parameter_update));
if (param_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to parameter_update topic\n");
exit(-1);
}
orb_subscr_t control_flags_sub = orb_subscribe(ORB_ID(vehicle_control_flags));
if (control_flags_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to vehicle_control_flags topic\n");
exit(-1);
}
orb_subscr_t att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
if (att_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to vehicle_attitude topic\n");
exit(-1);
}
orb_subscr_t att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
if (att_sp_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to vehicle_attitude_setpoint topic\n");
exit(-1);
}
orb_subscr_t manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
if (manual_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to manual_control_setpoint topic\n");
exit(-1);
}
orb_subscr_t local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
if (local_pos_sp_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to vehicle_local_position_setpoint topic\n");
exit(-1);
}
orb_subscr_t local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
if (local_pos_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to vehicle_local_position topic\n");
exit(-1);
}
orb_subscr_t global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
if (global_pos_sp_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to vehicle_global_position_setpoint topic\n");
exit(-1);
}
/* publish setpoint */
orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint));
if (local_pos_sp_pub == -1)
{
fprintf (stderr, "Comunicator thread failed to advertise the vehicle_local_position_setpoint topic\n");
exit (-1);
}
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint));
if (global_vel_sp_pub == -1)
{
fprintf (stderr, "Comunicator thread failed to advertise the vehicle_global_velocity_setpoint topic\n");
exit (-1);
}
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint));
if (att_sp_pub == -1)
{
fprintf (stderr, "Comunicator thread failed to advertise the vehicle_attitude_setpoint topic\n");
exit (-1);
}
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
/* abort on a nonzero return value from the parameter init */
if (multirotor_position_control_params_init() != 0) {
/* parameter setup went wrong, abort */
fprintf (stderr, "Multirotor position controller aborting on startup due to an error\n");
exit(-1);
}
for (i = 0; i < 2; i++) {
pid_init(&(xy_pos_pids[i]), multirotor_position_control_parameters.xy_p, 0.0f,
multirotor_position_control_parameters.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
pid_init(&(xy_vel_pids[i]), multirotor_position_control_parameters.xy_vel_p, multirotor_position_control_parameters.xy_vel_i,
multirotor_position_control_parameters.xy_vel_d, 1.0f, multirotor_position_control_parameters.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
}
pid_init(&z_pos_pid, multirotor_position_control_parameters.z_p, 0.0f,
multirotor_position_control_parameters.z_d, 1.0f, multirotor_position_control_parameters.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f);
thrust_pid_init(&z_vel_pid, multirotor_position_control_parameters.z_vel_p, multirotor_position_control_parameters.z_vel_i,
multirotor_position_control_parameters.z_vel_d, -multirotor_position_control_parameters.thr_max, -multirotor_position_control_parameters.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
while (!_shutdown_all_systems) {
updated = orb_check (ORB_ID(parameter_update), param_sub);
if (updated) {
/* clear updated flag */
orb_copy(ORB_ID(parameter_update), param_sub, &ps);
/* update multirotor_position_control_parameters */
multirotor_position_control_params_update();
for (i = 0; i < 2; i++) {
pid_set_parameters(&(xy_pos_pids[i]), multirotor_position_control_parameters.xy_p,
0.0f, multirotor_position_control_parameters.xy_d, 1.0f, 0.0f);
if (multirotor_position_control_parameters.xy_vel_i > 0.0f) {
i_limit = multirotor_position_control_parameters.tilt_max / multirotor_position_control_parameters.xy_vel_i / 2.0f;
} else {
i_limit = 0.0f; // not used
}
pid_set_parameters(&(xy_vel_pids[i]), multirotor_position_control_parameters.xy_vel_p,
multirotor_position_control_parameters.xy_vel_i, multirotor_position_control_parameters.xy_vel_d, i_limit, multirotor_position_control_parameters.tilt_max);
}
pid_set_parameters(&z_pos_pid, multirotor_position_control_parameters.z_p, 0.0f,
multirotor_position_control_parameters.z_d, 1.0f, multirotor_position_control_parameters.z_vel_max);
thrust_pid_set_parameters(&z_vel_pid, multirotor_position_control_parameters.z_vel_p, multirotor_position_control_parameters.z_vel_i,
multirotor_position_control_parameters.z_vel_d, -multirotor_position_control_parameters.thr_max, -multirotor_position_control_parameters.thr_min);
}
updated = orb_check (ORB_ID(vehicle_control_flags), control_flags_sub);
if (updated) {
orb_copy(ORB_ID(vehicle_control_flags), control_flags_sub, &control_flags);
}
updated = orb_check (ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub);
if (updated) {
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
global_pos_sp_valid = 1 /* true */;
reset_mission_sp = 1 /* true */;
}
absolute_time t = get_absolute_time();
float dt;
if (t_prev != 0) {
dt = (t - t_prev) * 0.000001f;
} else {
dt = 0.0f;
}
if (control_flags.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */
reset_man_sp_z = 1 /* true */;
reset_man_sp_xy = 1 /* true */;
reset_auto_sp_z = 1 /* true */;
reset_auto_sp_xy = 1 /* true */;
reset_takeoff_sp = 1 /* true */;
reset_int_z = 1 /* true */;
reset_int_xy = 1 /* true */;
}
was_armed = control_flags.flag_armed;
t_prev = t;
if (control_flags.flag_control_altitude_enabled || control_flags.flag_control_velocity_enabled || control_flags.flag_control_position_enabled) {
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
float z_sp_offs_max = multirotor_position_control_parameters.z_vel_max / multirotor_position_control_parameters.z_p * 2.0f;
float xy_sp_offs_max = multirotor_position_control_parameters.xy_vel_max / multirotor_position_control_parameters.xy_p * 2.0f;
float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f };
if (control_flags.flag_control_manual_enabled) {
/* manual control */
/* check for reference point updates and correct setpoint */
if (local_pos.ref_timestamp != ref_alt_t) {
if (ref_alt_t != 0) {
/* home alt changed, don't follow large ground level changes in manual flight */
local_pos_sp.z += local_pos.ref_alt - ref_alt;
}
ref_alt_t = local_pos.ref_timestamp;
ref_alt = local_pos.ref_alt;
// TODO also correct XY setpoint
}
/* reset setpoints to current position if needed */
if (control_flags.flag_control_altitude_enabled) {
if (reset_man_sp_z) {
reset_man_sp_z = 0 /* false */;
local_pos_sp.z = local_pos.z;
//mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z);
}
/* move altitude setpoint with throttle stick */
float z_sp_ctl = scale_control(manual.thrust - 0.5f, 0.5f, alt_ctl_dz);
if (z_sp_ctl != 0.0f) {
sp_move_rate[2] = -z_sp_ctl * multirotor_position_control_parameters.z_vel_max;
local_pos_sp.z += sp_move_rate[2] * dt;
if (local_pos_sp.z > local_pos.z + z_sp_offs_max) {
local_pos_sp.z = local_pos.z + z_sp_offs_max;
} else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) {
local_pos_sp.z = local_pos.z - z_sp_offs_max;
}
}
}
if (control_flags.flag_control_position_enabled) {
if (reset_man_sp_xy) {
reset_man_sp_xy = 0 /* false */;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
pid_reset_integral(&xy_vel_pids[0]);
pid_reset_integral(&xy_vel_pids[1]);
//mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
/* move position setpoint with roll/pitch stick */
float pos_pitch_sp_ctl = scale_control(-manual.pitch / multirotor_position_control_parameters.rc_scale_pitch, 1.0f, pos_ctl_dz);
float pos_roll_sp_ctl = scale_control(manual.roll / multirotor_position_control_parameters.rc_scale_roll, 1.0f, pos_ctl_dz);
if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
/* calculate direction and increment of control in NED frame */
float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * multirotor_position_control_parameters.xy_vel_max;
sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
local_pos_sp.x += sp_move_rate[0] * dt;
local_pos_sp.y += sp_move_rate[1] * dt;
/* limit maximum setpoint from position offset and preserve direction
* fail safe, should not happen in normal operation */
float pos_vec_x = local_pos_sp.x - local_pos.x;
float pos_vec_y = local_pos_sp.y - local_pos.y;
float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max;
if (pos_vec_norm > 1.0f) {
local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm;
local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm;
}
}
}
/* copy yaw setpoint to vehicle_local_position_setpoint topic */
local_pos_sp.yaw = att_sp.yaw_body;
/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
reset_auto_sp_xy = !control_flags.flag_control_position_enabled;
reset_auto_sp_z = !control_flags.flag_control_altitude_enabled;
reset_takeoff_sp = 1 /* true */;
/* force reprojection of global setpoint after manual mode */
reset_mission_sp = 1 /* true */;
} else if (control_flags.flag_control_auto_enabled) {
/* AUTO mode, use global setpoint */
if (control_flags.auto_state == navigation_state_auto_ready) {
reset_auto_sp_xy = 1 /* true */;
reset_auto_sp_z = 1 /* true */;
} else if (control_flags.auto_state == navigation_state_auto_takeoff) {
if (reset_takeoff_sp) {
reset_takeoff_sp = 0 /* false */;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.z = - multirotor_position_control_parameters.takeoff_alt - multirotor_position_control_parameters.takeoff_gap;
att_sp.yaw_body = att.yaw;
//mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z);
}
reset_auto_sp_xy = 0 /* false */;
reset_auto_sp_z = 1 /* true */;
} else if (control_flags.auto_state == navigation_state_auto_rtl) {
// TODO
reset_auto_sp_xy = 1 /* true */;
reset_auto_sp_z = 1 /* true */;
} else if (control_flags.auto_state == navigation_state_auto_mission) {
/* init local projection using local position ref */
if (local_pos.ref_timestamp != local_ref_timestamp) {
reset_mission_sp = 1 /* true */;
local_ref_timestamp = local_pos.ref_timestamp;
double lat_home = local_pos.ref_lat * 1e-7;
double lon_home = local_pos.ref_lon * 1e-7;
map_projection_init(lat_home, lon_home);
//mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home);
}
if (reset_mission_sp) {
reset_mission_sp = 0 /* false */;
/* update global setpoint projection */
if (global_pos_sp_valid) {
/* global position setpoint valid, use it */
double sp_lat = global_pos_sp.latitude * 1e-7;
double sp_lon = global_pos_sp.longitude * 1e-7;
/* project global setpoint to local setpoint */
map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
if (global_pos_sp.altitude_is_relative) {
local_pos_sp.z = -global_pos_sp.altitude;
} else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
}
att_sp.yaw_body = global_pos_sp.yaw;
//mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
} else {
if (reset_auto_sp_xy) {
reset_auto_sp_xy = 0 /* false */;
/* local position setpoint is invalid,
* use current position as setpoint for loiter */
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.yaw = att.yaw;
}
if (reset_auto_sp_z) {
reset_auto_sp_z = 0 /* false */;
local_pos_sp.z = local_pos.z;
}
//mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
}
reset_auto_sp_xy = 1 /* true */;
reset_auto_sp_z = 1 /* true */;
}
if (control_flags.auto_state != navigation_state_auto_takeoff) {
reset_takeoff_sp = 1 /* true */;
}
if (control_flags.auto_state != navigation_state_auto_mission) {
reset_mission_sp = 1 /* true */;
}
/* copy yaw setpoint to vehicle_local_position_setpoint topic */
local_pos_sp.yaw = att_sp.yaw_body;
/* reset setpoints after AUTO mode */
reset_man_sp_xy = 1 /* true */;
reset_man_sp_z = 1 /* true */;
} else {
/* no control (failsafe), loiter or stay on ground */
if (local_pos.landed) {
/* landed: move setpoint down */
/* in air: hold altitude */
if (local_pos_sp.z < 5.0f) {
/* set altitude setpoint to 5m under ground,
* don't set it too deep to avoid unexpected landing in case of false "landed" signal */
local_pos_sp.z = 5.0f;
//mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z);
}
reset_man_sp_z = 1 /* true */;
} else {
/* in air: hold altitude */
if (reset_man_sp_z) {
reset_man_sp_z = 0 /* false */;
local_pos_sp.z = local_pos.z;
//mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z);
}
reset_auto_sp_z = 0 /* false */;
}
if (control_flags.flag_control_position_enabled) {
if (reset_man_sp_xy) {
reset_man_sp_xy = 0 /* false */;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.yaw = att.yaw;
att_sp.yaw_body = att.yaw;
//mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
reset_auto_sp_xy = 0 /* false */;
}
}
/* publish local position setpoint */
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
/* run position & altitude controllers, calculate velocity setpoint */
if (control_flags.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];
} else {
reset_man_sp_z = 1 /* true */;
global_vel_sp.vz = 0.0f;
}
if (control_flags.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];
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) / multirotor_position_control_parameters.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;
}
} else {
reset_man_sp_xy = 1 /* true */;
global_vel_sp.vx = 0.0f;
global_vel_sp.vy = 0.0f;
}
//fprintf (stderr, "global_vel_sp.vx:%.3f\tglobal_vel_sp.vy:%.3f\tglobal_vel_sp.vz:%.3f\n", global_vel_sp.vx, global_vel_sp.vy, global_vel_sp.vz);
/* 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
if (control_flags.flag_control_climb_rate_enabled || control_flags.flag_control_velocity_enabled) {
/* run velocity controllers, calculate thrust vector with attitude-thrust compensation */
if (control_flags.flag_control_climb_rate_enabled) {
if (reset_int_z) {
reset_int_z = 0 /* false */;
float i = multirotor_position_control_parameters.thr_min;
if (reset_int_z_manual) {
i = manual.thrust;
if (i < multirotor_position_control_parameters.thr_min) {
i = multirotor_position_control_parameters.thr_min;
} else if (i > multirotor_position_control_parameters.thr_max) {
i = multirotor_position_control_parameters.thr_max;
}
}
thrust_pid_set_integral(&z_vel_pid, -i);
//mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
}
thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
att_sp.thrust = -thrust_sp[2];
} else {
/* reset thrust integral when altitude control enabled */
reset_int_z = 1 /* true */;
}
if (control_flags.flag_control_velocity_enabled) {
/* calculate thrust set point in NED frame */
if (reset_int_xy) {
reset_int_xy = 0 /* false */;
pid_reset_integral(&xy_vel_pids[0]);
pid_reset_integral(&xy_vel_pids[1]);
//mavlink_log_info(mavlink_fd, "[mpc] reset pos integral");
}
thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt);
thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt);
/* 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]);
/* assuming that vertical component of thrust is g,
* horizontal component = g * tan(alpha) */
float tilt = atanf(norm(thrust_sp[0], thrust_sp[1]));
if (tilt > multirotor_position_control_parameters.tilt_max) {
tilt = multirotor_position_control_parameters.tilt_max;
}
/* convert direction to body frame */
thrust_xy_dir -= att.yaw;
/* calculate roll and pitch */
att_sp.roll_body = sinf(thrust_xy_dir) * tilt;
att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body);
} else {
reset_int_xy = 1 /* true */;
}
att_sp.timestamp = get_absolute_time();
//fprintf (stderr, "att_sp.roll:%.3f\tatt_sp.pitch:%.3f\tatt_sp.yaw:%.3f\tatt_sp.thrust:%.3f\n", att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, att_sp.thrust);
/* publish new attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
} else {
/* position controller disabled, reset setpoints */
reset_man_sp_z = 1 /* true */;
reset_man_sp_xy = 1 /* true */;
reset_int_z = 1 /* true */;
reset_int_xy = 1 /* true */;
reset_mission_sp = 1 /* true */;
reset_auto_sp_xy = 1 /* true */;
reset_auto_sp_z = 1 /* true */;
}
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
reset_int_z_manual = control_flags.flag_armed && control_flags.flag_control_manual_enabled && !control_flags.flag_control_climb_rate_enabled;
/* run at approximately 50 Hz */
usleep(20000);
}
/*
* do unsubscriptions
*/
orb_unsubscribe(ORB_ID(parameter_update), param_sub, pthread_self());
orb_unsubscribe(ORB_ID(vehicle_control_flags), control_flags_sub, pthread_self());
orb_unsubscribe(ORB_ID(vehicle_attitude), att_sub, pthread_self());
orb_unsubscribe(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, pthread_self());
orb_unsubscribe(ORB_ID(manual_control_setpoint), manual_sub, pthread_self());
orb_unsubscribe(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, pthread_self());
orb_unsubscribe(ORB_ID(vehicle_local_position), local_pos_sub, pthread_self());
orb_unsubscribe(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, pthread_self());
/*
* do unadvertises
*/
orb_unadvertise(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, pthread_self());
orb_unadvertise(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, pthread_self());
orb_unadvertise(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, pthread_self());
return 0;
}
static int multirotor_pos_control_thread_main(int argc, char *argv[])
{
/* welcome user */
warnx("started");
static int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd, "[mpc] started");
/* structures */
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
struct vehicle_local_position_s local_pos;
memset(&local_pos, 0, sizeof(local_pos));
struct vehicle_local_position_setpoint_s local_pos_sp;
memset(&local_pos_sp, 0, sizeof(local_pos_sp));
struct vehicle_global_position_setpoint_s global_pos_sp;
memset(&global_pos_sp, 0, sizeof(global_pos_sp));
struct vehicle_global_velocity_setpoint_s global_vel_sp;
memset(&global_vel_sp, 0, sizeof(global_vel_sp));
/* subscribe to attitude, motor setpoints and system state */
int param_sub = orb_subscribe(ORB_ID(parameter_update));
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
/* publish setpoint */
orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp);
orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
bool reset_mission_sp = false;
bool global_pos_sp_valid = false;
bool reset_man_sp_z = true;
bool reset_man_sp_xy = true;
bool reset_int_z = true;
bool reset_int_z_manual = false;
bool reset_int_xy = true;
bool was_armed = false;
bool reset_auto_sp_xy = true;
bool reset_auto_sp_z = true;
bool reset_takeoff_sp = true;
hrt_abstime t_prev = 0;
const float alt_ctl_dz = 0.2f;
const float pos_ctl_dz = 0.05f;
float ref_alt = 0.0f;
hrt_abstime ref_alt_t = 0;
uint64_t local_ref_timestamp = 0;
PID_t xy_pos_pids[2];
PID_t xy_vel_pids[2];
PID_t z_pos_pid;
thrust_pid_t z_vel_pid;
thread_running = true;
struct multirotor_position_control_params params;
struct multirotor_position_control_param_handles params_h;
parameters_init(¶ms_h);
parameters_update(¶ms_h, ¶ms);
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.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, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f);
thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
while (!thread_should_exit) {
bool param_updated;
orb_check(param_sub, ¶m_updated);
if (param_updated) {
/* clear updated flag */
struct parameter_update_s ps;
orb_copy(ORB_ID(parameter_update), param_sub, &ps);
/* 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, 1.0f, 0.0f);
/* use integral_limit_out = tilt_max / 2 */
float i_limit;
if (params.xy_vel_i > 0.0f) {
i_limit = params.tilt_max / params.xy_vel_i / 2.0f;
} else {
i_limit = 0.0f; // not used
}
pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
}
pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min);
}
bool updated;
orb_check(control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
}
orb_check(global_pos_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
global_pos_sp_valid = true;
reset_mission_sp = true;
}
hrt_abstime t = hrt_absolute_time();
float dt;
if (t_prev != 0) {
dt = (t - t_prev) * 0.000001f;
} else {
dt = 0.0f;
}
if (control_mode.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */
reset_man_sp_z = true;
reset_man_sp_xy = true;
reset_auto_sp_z = true;
reset_auto_sp_xy = true;
reset_takeoff_sp = true;
reset_int_z = true;
reset_int_xy = true;
}
was_armed = control_mode.flag_armed;
t_prev = t;
if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) {
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f;
float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f;
float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f };
if (control_mode.flag_control_manual_enabled) {
/* manual control */
/* check for reference point updates and correct setpoint */
if (local_pos.ref_timestamp != ref_alt_t) {
if (ref_alt_t != 0) {
/* home alt changed, don't follow large ground level changes in manual flight */
local_pos_sp.z += local_pos.ref_alt - ref_alt;
}
ref_alt_t = local_pos.ref_timestamp;
ref_alt = local_pos.ref_alt;
// TODO also correct XY setpoint
}
/* reset setpoints to current position if needed */
if (control_mode.flag_control_altitude_enabled) {
if (reset_man_sp_z) {
reset_man_sp_z = false;
local_pos_sp.z = local_pos.z;
mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z);
}
/* move altitude setpoint with throttle stick */
float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
if (z_sp_ctl != 0.0f) {
sp_move_rate[2] = -z_sp_ctl * params.z_vel_max;
local_pos_sp.z += sp_move_rate[2] * dt;
if (local_pos_sp.z > local_pos.z + z_sp_offs_max) {
local_pos_sp.z = local_pos.z + z_sp_offs_max;
} else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) {
local_pos_sp.z = local_pos.z - z_sp_offs_max;
}
}
}
if (control_mode.flag_control_position_enabled) {
if (reset_man_sp_xy) {
reset_man_sp_xy = false;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
pid_reset_integral(&xy_vel_pids[0]);
pid_reset_integral(&xy_vel_pids[1]);
mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
/* move position setpoint with roll/pitch stick */
float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz);
float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz);
if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
/* calculate direction and increment of control in NED frame */
float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max;
sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
local_pos_sp.x += sp_move_rate[0] * dt;
local_pos_sp.y += sp_move_rate[1] * dt;
/* limit maximum setpoint from position offset and preserve direction
* fail safe, should not happen in normal operation */
float pos_vec_x = local_pos_sp.x - local_pos.x;
float pos_vec_y = local_pos_sp.y - local_pos.y;
float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max;
if (pos_vec_norm > 1.0f) {
local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm;
local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm;
}
}
}
/* copy yaw setpoint to vehicle_local_position_setpoint topic */
local_pos_sp.yaw = att_sp.yaw_body;
/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
reset_auto_sp_xy = !control_mode.flag_control_position_enabled;
reset_auto_sp_z = !control_mode.flag_control_altitude_enabled;
reset_takeoff_sp = true;
/* force reprojection of global setpoint after manual mode */
reset_mission_sp = true;
} else if (control_mode.flag_control_auto_enabled) {
/* AUTO mode, use global setpoint */
if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) {
reset_auto_sp_xy = true;
reset_auto_sp_z = true;
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
if (reset_takeoff_sp) {
reset_takeoff_sp = false;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap;
att_sp.yaw_body = att.yaw;
mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z);
}
reset_auto_sp_xy = false;
reset_auto_sp_z = true;
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) {
// TODO
reset_auto_sp_xy = true;
reset_auto_sp_z = true;
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) {
/* init local projection using local position ref */
if (local_pos.ref_timestamp != local_ref_timestamp) {
reset_mission_sp = true;
local_ref_timestamp = local_pos.ref_timestamp;
double lat_home = local_pos.ref_lat * 1e-7;
double lon_home = local_pos.ref_lon * 1e-7;
map_projection_init(lat_home, lon_home);
mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home);
}
if (reset_mission_sp) {
reset_mission_sp = false;
/* update global setpoint projection */
if (global_pos_sp_valid) {
/* global position setpoint valid, use it */
double sp_lat = global_pos_sp.lat * 1e-7;
double sp_lon = global_pos_sp.lon * 1e-7;
/* project global setpoint to local setpoint */
map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
if (global_pos_sp.altitude_is_relative) {
local_pos_sp.z = -global_pos_sp.altitude;
} else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
}
/* update yaw setpoint only if value is valid */
if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) {
att_sp.yaw_body = global_pos_sp.yaw;
}
mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
} else {
if (reset_auto_sp_xy) {
reset_auto_sp_xy = false;
/* local position setpoint is invalid,
* use current position as setpoint for loiter */
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.yaw = att.yaw;
}
if (reset_auto_sp_z) {
reset_auto_sp_z = false;
local_pos_sp.z = local_pos.z;
}
mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
}
reset_auto_sp_xy = true;
reset_auto_sp_z = true;
}
if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) {
reset_takeoff_sp = true;
}
if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) {
reset_mission_sp = true;
}
/* copy yaw setpoint to vehicle_local_position_setpoint topic */
local_pos_sp.yaw = att_sp.yaw_body;
/* reset setpoints after AUTO mode */
reset_man_sp_xy = true;
reset_man_sp_z = true;
} else {
/* no control (failsafe), loiter or stay on ground */
if (local_pos.landed) {
/* landed: move setpoint down */
/* in air: hold altitude */
if (local_pos_sp.z < 5.0f) {
/* set altitude setpoint to 5m under ground,
* don't set it too deep to avoid unexpected landing in case of false "landed" signal */
local_pos_sp.z = 5.0f;
mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z);
}
reset_man_sp_z = true;
} else {
/* in air: hold altitude */
if (reset_man_sp_z) {
reset_man_sp_z = false;
local_pos_sp.z = local_pos.z;
mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z);
}
reset_auto_sp_z = false;
}
if (control_mode.flag_control_position_enabled) {
if (reset_man_sp_xy) {
reset_man_sp_xy = false;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.yaw = att.yaw;
att_sp.yaw_body = att.yaw;
mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
reset_auto_sp_xy = false;
}
}
/* publish local position setpoint */
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
/* 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];
} else {
reset_man_sp_z = true;
global_vel_sp.vz = 0.0f;
}
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];
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;
if (xy_vel_sp_norm > 1.0f) {
global_vel_sp.vx /= xy_vel_sp_norm;
global_vel_sp.vy /= xy_vel_sp_norm;
}
} else {
reset_man_sp_xy = true;
global_vel_sp.vx = 0.0f;
global_vel_sp.vy = 0.0f;
}
/* 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
if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) {
/* run velocity controllers, calculate thrust vector with attitude-thrust compensation */
float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
if (control_mode.flag_control_climb_rate_enabled) {
if (reset_int_z) {
reset_int_z = false;
float i = params.thr_min;
if (reset_int_z_manual) {
i = manual.throttle;
if (i < params.thr_min) {
i = params.thr_min;
} else if (i > params.thr_max) {
i = params.thr_max;
}
}
thrust_pid_set_integral(&z_vel_pid, -i);
mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
}
thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
att_sp.thrust = -thrust_sp[2];
} else {
/* reset thrust integral when altitude control enabled */
reset_int_z = true;
}
if (control_mode.flag_control_velocity_enabled) {
/* calculate thrust set point in NED frame */
if (reset_int_xy) {
reset_int_xy = false;
pid_reset_integral(&xy_vel_pids[0]);
pid_reset_integral(&xy_vel_pids[1]);
mavlink_log_info(mavlink_fd, "[mpc] reset pos integral");
}
thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt);
thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt);
/* 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]);
/* assuming that vertical component of thrust is g,
* horizontal component = g * tan(alpha) */
float tilt = atanf(norm(thrust_sp[0], thrust_sp[1]));
if (tilt > params.tilt_max) {
tilt = params.tilt_max;
}
/* convert direction to body frame */
thrust_xy_dir -= att.yaw;
/* calculate roll and pitch */
att_sp.roll_body = sinf(thrust_xy_dir) * tilt;
att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body);
} else {
reset_int_xy = true;
}
att_sp.timestamp = hrt_absolute_time();
/* publish new attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
} else {
/* position controller disabled, reset setpoints */
reset_man_sp_z = true;
reset_man_sp_xy = true;
reset_int_z = true;
reset_int_xy = true;
reset_mission_sp = true;
reset_auto_sp_xy = true;
reset_auto_sp_z = true;
}
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled;
/* run at approximately 50 Hz */
usleep(20000);
}
warnx("stopped");
mavlink_log_info(mavlink_fd, "[mpc] stopped");
thread_running = false;
fflush(stdout);
return 0;
}
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators, bool reset_integral)
{
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
static uint64_t last_input = 0;
if (last_input != rate_sp->timestamp) {
last_input = rate_sp->timestamp;
}
last_run = hrt_absolute_time();
static int motor_skip_counter = 0;
static PID_t pitch_rate_controller;
static PID_t roll_rate_controller;
static PID_t yaw_rate_controller;
static struct mc_rate_control_params p;
static struct mc_rate_control_param_handles h;
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;
pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
}
/* load new parameters with lower rate */
if (motor_skip_counter % 2500 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f);
}
/* reset integrals if needed */
if (reset_integral) {
pid_reset_integral(&pitch_rate_controller);
pid_reset_integral(&roll_rate_controller);
pid_reset_integral(&yaw_rate_controller);
}
/* run pitch, roll and yaw controllers */
float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
float yaw_control = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
actuators->control[2] = yaw_control;
actuators->control[3] = rate_sp->thrust;
motor_skip_counter++;
}
