diff options
Diffstat (limited to 'src/modules/multirotor_pos_control')
-rw-r--r-- | src/modules/multirotor_pos_control/module.mk | 3 | ||||
-rw-r--r-- | src/modules/multirotor_pos_control/multirotor_pos_control.c | 602 | ||||
-rw-r--r-- | src/modules/multirotor_pos_control/multirotor_pos_control_params.c | 70 | ||||
-rw-r--r-- | src/modules/multirotor_pos_control/multirotor_pos_control_params.h | 55 | ||||
-rw-r--r-- | src/modules/multirotor_pos_control/position_control.c | 235 | ||||
-rw-r--r-- | src/modules/multirotor_pos_control/thrust_pid.c | 189 | ||||
-rw-r--r-- | src/modules/multirotor_pos_control/thrust_pid.h (renamed from src/modules/multirotor_pos_control/position_control.h) | 52 |
7 files changed, 846 insertions, 360 deletions
diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/multirotor_pos_control/module.mk index d04847745..bc4b48fb4 100644 --- a/src/modules/multirotor_pos_control/module.mk +++ b/src/modules/multirotor_pos_control/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = multirotor_pos_control SRCS = multirotor_pos_control.c \ - multirotor_pos_control_params.c + multirotor_pos_control_params.c \ + thrust_pid.c diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index f39d11438..8227f76c5 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,13 +35,14 @@ /** * @file multirotor_pos_control.c * - * Skeleton for multirotor position controller + * Multirotor position controller */ #include <nuttx/config.h> #include <stdio.h> #include <stdlib.h> #include <string.h> +#include <math.h> #include <stdbool.h> #include <unistd.h> #include <fcntl.h> @@ -52,15 +53,22 @@ #include <sys/prctl.h> #include <drivers/drv_hrt.h> #include <uORB/uORB.h> +#include <uORB/topics/parameter_update.h> #include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_local_position_setpoint.h> -#include <uORB/topics/vehicle_vicon_position.h> +#include <uORB/topics/vehicle_global_position_setpoint.h> +#include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <systemlib/systemlib.h> +#include <systemlib/pid/pid.h> +#include <mavlink/mavlink_log.h> #include "multirotor_pos_control_params.h" +#include "thrust_pid.h" static bool thread_should_exit = false; /**< Deamon exit flag */ @@ -79,12 +87,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]); */ static void usage(const char *reason); -static void -usage(const char *reason) +static float scale_control(float ctl, float end, float dz); + +static float norm(float x, float y); + +static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n"); + + fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n"); exit(1); } @@ -92,9 +104,9 @@ usage(const char *reason) * The deamon app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. - * + * * The actual stack size should be set in the call - * to task_spawn_cmd(). + * to task_spawn(). */ int multirotor_pos_control_main(int argc, char *argv[]) { @@ -104,32 +116,36 @@ int multirotor_pos_control_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("multirotor pos control already running\n"); + warnx("already running"); /* this is not an error */ exit(0); } + warnx("start"); thread_should_exit = false; - deamon_task = task_spawn_cmd("multirotor pos control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 60, - 4096, - multirotor_pos_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + deamon_task = task_spawn_cmd("multirotor_pos_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 60, + 4096, + multirotor_pos_control_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } if (!strcmp(argv[1], "stop")) { + warnx("stop"); thread_should_exit = true; exit(0); } if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tmultirotor pos control app is running\n"); + warnx("app is running"); + } else { - printf("\tmultirotor pos control app not started\n"); + warnx("app not started"); } + exit(0); } @@ -137,98 +153,510 @@ int multirotor_pos_control_main(int argc, char *argv[]) exit(1); } -static int -multirotor_pos_control_thread_main(int argc, char *argv[]) +static float scale_control(float ctl, float end, float dz) +{ + if (ctl > dz) { + return (ctl - dz) / (end - dz); + + } else if (ctl < -dz) { + return (ctl + dz) / (end - dz); + + } else { + return 0.0f; + } +} + +static float norm(float x, float y) +{ + return sqrtf(x * x + y * y); +} + +static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* welcome user */ - printf("[multirotor pos control] Control started, taking over position control\n"); + warnx("started"); + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd, "[mpc] started"); /* structures */ - struct vehicle_status_s state; + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_attitude_s att; - //struct vehicle_global_position_setpoint_s global_pos_sp; - struct vehicle_local_position_setpoint_s local_pos_sp; - struct vehicle_vicon_position_s local_pos; - struct manual_control_setpoint_s manual; + 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(local_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 state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); - //int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_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 attitude 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 global_pos_sp_reproject = false; + bool global_pos_sp_valid = false; + bool local_pos_sp_valid = false; + bool reset_sp_z = true; + bool reset_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_integral = true; + bool reset_auto_pos = true; + + hrt_abstime t_prev = 0; + const float alt_ctl_dz = 0.2f; + const float pos_ctl_dz = 0.05f; + const float takeoff_alt_default = 10.0f; + 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; - int loopcounter = 0; - - struct multirotor_position_control_params p; - struct multirotor_position_control_param_handles h; - parameters_init(&h); - parameters_update(&h, &p); - - - while (1) { - /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); - /* get a local copy of manual setpoint */ - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - /* get a local copy of local position */ - orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos); - /* get a local copy of local position setpoint */ - orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); - - if (loopcounter == 500) { - parameters_update(&h, &p); - loopcounter = 0; + 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); + + int paramcheck_counter = 0; + + while (!thread_should_exit) { + /* check parameters at 1 Hz */ + if (++paramcheck_counter >= 50) { + paramcheck_counter = 0; + bool param_updated; + orb_check(param_sub, ¶m_updated); + + if (param_updated) { + 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.0) { + i_limit = params.tilt_max / params.xy_vel_i / 2.0; + + } else { + i_limit = 1.0f; // not used really + } + + 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; + global_pos_sp_reproject = true; } - // if (state.state_machine == SYSTEM_STATE_AUTO) { - - // XXX IMPLEMENT POSITION CONTROL HERE - - float dT = 1.0f / 50.0f; - - float x_setpoint = 0.0f; - - // XXX enable switching between Vicon and local position estimate - /* local pos is the Vicon position */ - - // XXX just an example, lacks rotation around world-body transformation - att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p; - att_sp.roll_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.3f; - att_sp.timestamp = hrt_absolute_time(); - - /* publish new attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - // } else if (state.state_machine == SYSTEM_STATE_STABILIZED) { - /* set setpoint to current position */ - // XXX select pos reset channel on remote - /* reset setpoint to current position (position hold) */ - // if (1 == 2) { - // local_pos_sp.x = local_pos.x; - // local_pos_sp.y = local_pos.y; - // local_pos_sp.z = local_pos.z; - // local_pos_sp.yaw = att.yaw; - // } - // } + 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_sp_z = true; + reset_sp_xy = 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_sp_z) { + reset_sp_z = false; + local_pos_sp.z = local_pos.z; + mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", -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_sp_xy) { + reset_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", local_pos_sp.x, 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; + } + } + } + + local_pos_sp.yaw = att_sp.yaw_body; + + /* local position setpoint is valid and can be used for loiter after position controlled mode */ + local_pos_sp_valid = control_mode.flag_control_position_enabled; + + reset_auto_pos = true; + + /* force reprojection of global setpoint after manual mode */ + global_pos_sp_reproject = 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_pos = true; + + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (reset_auto_pos) { + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.z = -takeoff_alt_default; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; + att_sp.yaw_body = att.yaw; + reset_auto_pos = false; + mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, -local_pos_sp.z); + } + + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { + // TODO + reset_auto_pos = 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) { + global_pos_sp_reproject = 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", lat_home, lon_home); + } + + if (global_pos_sp_reproject) { + /* update global setpoint projection */ + global_pos_sp_reproject = false; + + 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; + } + + local_pos_sp.yaw = global_pos_sp.yaw; + att_sp.yaw_body = global_pos_sp.yaw; + + mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); + + } else { + if (!local_pos_sp_valid) { + /* 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.z = local_pos.z; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; + } + + mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); + } + } + + reset_auto_pos = true; + } + + if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) { + global_pos_sp_reproject = true; + } + + /* reset setpoints after AUTO mode */ + reset_sp_xy = true; + reset_sp_z = true; + + } else { + /* no control, 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", -local_pos_sp.z); + } + + reset_sp_z = true; + + } else { + /* in air: hold altitude */ + if (reset_sp_z) { + reset_sp_z = false; + local_pos_sp.z = local_pos.z; + mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", -local_pos_sp.z); + } + } + + if (control_mode.flag_control_position_enabled) { + if (reset_sp_xy) { + reset_sp_xy = false; + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; + att_sp.yaw_body = att.yaw; + mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", local_pos_sp.x, local_pos_sp.y); + } + } + } + + /* 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_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_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", 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_sp_z = true; + reset_sp_xy = true; + reset_int_z = true; + reset_int_xy = true; + global_pos_sp_reproject = true; + reset_auto_pos = 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); - loopcounter++; - } - printf("[multirotor pos control] ending now...\n"); + warnx("stopped"); + mavlink_log_info(mavlink_fd, "[mpc] stopped"); thread_running = false; 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 6b73dc405..9c1ef2edb 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -1,8 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli <naegelit@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,29 +33,76 @@ ****************************************************************************/ /* - * @file multirotor_position_control_params.c - * - * Parameters for EKF filter + * @file multirotor_pos_control_params.c + * + * Parameters for multirotor_pos_control */ #include "multirotor_pos_control_params.h" -/* Extended Kalman Filter covariances */ - /* controller parameters */ -PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f); +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.1f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); +PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f); +PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f); +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_TILT_MAX, 0.5f); int parameters_init(struct multirotor_position_control_param_handles *h) { - /* PID parameters */ - h->p = param_find("MC_POS_P"); + 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->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->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"); + h->rc_scale_yaw = param_find("RC_SCALE_YAW"); return OK; } int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p) { - param_get(h->p, &(p->p)); + 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->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->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)); + param_get(h->rc_scale_yaw, &(p->rc_scale_yaw)); return OK; } 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 274f6c22a..3ec85a364 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -1,8 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli <naegelit@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,23 +33,55 @@ ****************************************************************************/ /* - * @file multirotor_position_control_params.h - * - * Parameters for position controller + * @file multirotor_pos_control_params.h + * + * Parameters for multirotor_pos_control */ #include <systemlib/param/param.h> struct multirotor_position_control_params { - float p; - float i; - float d; + 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 xy_p; + float xy_d; + float xy_vel_p; + float xy_vel_i; + float xy_vel_d; + float xy_vel_max; + float tilt_max; + + float rc_scale_pitch; + float rc_scale_roll; + float rc_scale_yaw; }; struct multirotor_position_control_param_handles { - param_t p; - param_t i; - param_t d; + 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 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 tilt_max; + + param_t rc_scale_pitch; + param_t rc_scale_roll; + param_t rc_scale_yaw; }; /** diff --git a/src/modules/multirotor_pos_control/position_control.c b/src/modules/multirotor_pos_control/position_control.c deleted file mode 100644 index 9c53a5bf6..000000000 --- a/src/modules/multirotor_pos_control/position_control.c +++ /dev/null @@ -1,235 +0,0 @@ -// /**************************************************************************** -// * -// * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. -// * Author: @author Lorenz Meier <lm@inf.ethz.ch> -// * @author Laurens Mackay <mackayl@student.ethz.ch> -// * @author Tobias Naegeli <naegelit@student.ethz.ch> -// * @author Martin Rutschmann <rutmarti@student.ethz.ch> -// * -// * Redistribution and use in source and binary forms, with or without -// * modification, are permitted provided that the following conditions -// * are met: -// * -// * 1. Redistributions of source code must retain the above copyright -// * notice, this list of conditions and the following disclaimer. -// * 2. Redistributions in binary form must reproduce the above copyright -// * notice, this list of conditions and the following disclaimer in -// * the documentation and/or other materials provided with the -// * distribution. -// * 3. Neither the name PX4 nor the names of its contributors may be -// * used to endorse or promote products derived from this software -// * without specific prior written permission. -// * -// * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -// * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -// * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -// * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -// * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -// * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -// * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// * POSSIBILITY OF SUCH DAMAGE. -// * -// ****************************************************************************/ - -// /** -// * @file multirotor_position_control.c -// * Implementation of the position control for a multirotor VTOL -// */ - -// #include <stdio.h> -// #include <stdlib.h> -// #include <stdio.h> -// #include <stdint.h> -// #include <math.h> -// #include <stdbool.h> -// #include <float.h> -// #include <systemlib/pid/pid.h> - -// #include "multirotor_position_control.h" - -// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, -// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, -// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp) -// { -// static PID_t distance_controller; - -// static int read_ret; -// static global_data_position_t position_estimated; - -// static uint16_t counter; - -// static bool initialized; -// static uint16_t pm_counter; - -// static float lat_next; -// static float lon_next; - -// static float pitch_current; - -// static float thrust_total; - - -// if (initialized == false) { - -// pid_init(&distance_controller, -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU], -// PID_MODE_DERIVATIV_CALC, 150);//150 - -// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; -// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; - -// thrust_total = 0.0f; - -// /* Position initialization */ -// /* Wait for new position estimate */ -// do { -// read_ret = read_lock_position(&position_estimated); -// } while (read_ret != 0); - -// lat_next = position_estimated.lat; -// lon_next = position_estimated.lon; - -// /* attitude initialization */ -// global_data_lock(&global_data_attitude->access_conf); -// pitch_current = global_data_attitude->pitch; -// global_data_unlock(&global_data_attitude->access_conf); - -// initialized = true; -// } - -// /* load new parameters with 10Hz */ -// if (counter % 50 == 0) { -// if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) { -// /* check whether new parameters are available */ -// if (global_data_parameter_storage->counter > pm_counter) { -// pid_set_parameters(&distance_controller, -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]); - -// // -// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; -// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; - -// pm_counter = global_data_parameter_storage->counter; -// printf("Position controller changed pid parameters\n"); -// } -// } - -// global_data_unlock(&global_data_parameter_storage->access_conf); -// } - - -// /* Wait for new position estimate */ -// do { -// read_ret = read_lock_position(&position_estimated); -// } while (read_ret != 0); - -// /* Get next waypoint */ //TODO: add local copy - -// if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) { -// lat_next = global_data_position_setpoint->x; -// lon_next = global_data_position_setpoint->y; -// global_data_unlock(&global_data_position_setpoint->access_conf); -// } - -// /* Get distance to waypoint */ -// float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next); -// // if(counter % 5 == 0) -// // printf("distance_to_waypoint: %.4f\n", distance_to_waypoint); - -// /* Get bearing to waypoint (direction on earth surface to next waypoint) */ -// float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next); - -// if (counter % 5 == 0) -// printf("bearing: %.4f\n", bearing); - -// /* Calculate speed in direction of bearing (needed for controller) */ -// float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy); -// // if(counter % 5 == 0) -// // printf("speed_norm: %.4f\n", speed_norm); -// float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller - -// /* Control Thrust in bearing direction */ -// float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint, -// CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else - -// // if(counter % 5 == 0) -// // printf("horizontal thrust: %.4f\n", horizontal_thrust); - -// /* Get total thrust (from remote for now) */ -// if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { -// thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum? -// global_data_unlock(&global_data_rc_channels->access_conf); -// } - -// const float max_gas = 500.0f; -// thrust_total *= max_gas / 20000.0f; //TODO: check this -// thrust_total += max_gas / 2.0f; - - -// if (horizontal_thrust > thrust_total) { -// horizontal_thrust = thrust_total; - -// } else if (horizontal_thrust < -thrust_total) { -// horizontal_thrust = -thrust_total; -// } - - - -// //TODO: maybe we want to add a speed controller later... - -// /* Calclulate thrust in east and north direction */ -// float thrust_north = cosf(bearing) * horizontal_thrust; -// float thrust_east = sinf(bearing) * horizontal_thrust; - -// if (counter % 10 == 0) { -// printf("thrust north: %.4f\n", thrust_north); -// printf("thrust east: %.4f\n", thrust_east); -// fflush(stdout); -// } - -// /* Get current attitude */ -// if (0 == global_data_trylock(&global_data_attitude->access_conf)) { -// pitch_current = global_data_attitude->pitch; -// global_data_unlock(&global_data_attitude->access_conf); -// } - -// /* Get desired pitch & roll */ -// float pitch_desired = 0.0f; -// float roll_desired = 0.0f; - -// if (thrust_total != 0) { -// float pitch_fraction = -thrust_north / thrust_total; -// float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total); - -// if (roll_fraction < -1) { -// roll_fraction = -1; - -// } else if (roll_fraction > 1) { -// roll_fraction = 1; -// } - -// // if(counter % 5 == 0) -// // { -// // printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction); -// // fflush(stdout); -// // } - -// pitch_desired = asinf(pitch_fraction); -// roll_desired = asinf(roll_fraction); -// } - -// att_sp.roll = roll_desired; -// att_sp.pitch = pitch_desired; - -// counter++; -// } diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c new file mode 100644 index 000000000..b985630ae --- /dev/null +++ b/src/modules/multirotor_pos_control/thrust_pid.c @@ -0,0 +1,189 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file thrust_pid.c + * + * Implementation of thrust control PID. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include "thrust_pid.h" +#include <math.h> + +__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) +{ + pid->kp = kp; + pid->ki = ki; + pid->kd = kd; + pid->limit_min = limit_min; + pid->limit_max = limit_max; + pid->mode = mode; + pid->dt_min = dt_min; + pid->last_output = 0.0f; + pid->sp = 0.0f; + pid->error_previous = 0.0f; + pid->integral = 0.0f; +} + +__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max) +{ + int ret = 0; + + if (isfinite(kp)) { + pid->kp = kp; + + } else { + ret = 1; + } + + if (isfinite(ki)) { + pid->ki = ki; + + } else { + ret = 1; + } + + if (isfinite(kd)) { + pid->kd = kd; + + } else { + ret = 1; + } + + if (isfinite(limit_min)) { + pid->limit_min = limit_min; + + } else { + ret = 1; + } + + if (isfinite(limit_max)) { + pid->limit_max = limit_max; + + } else { + ret = 1; + } + + return ret; +} + +__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22) +{ + /* Alternative integral component calculation + * + * 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)) { + return pid->last_output; + } + + float i, d; + pid->sp = sp; + + // Calculated current error value + float error = pid->sp - val; + + // Calculate or measured current error derivative + if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) { + d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = error; + + } else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) { + d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = -val; + + } else { + d = 0.0f; + } + + if (!isfinite(d)) { + d = 0.0f; + } + + /* calculate the error integral */ + i = pid->integral + (pid->ki * error * dt); + + /* 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) { + /* saturated, recalculate output with old integral */ + output = (error * pid->kp) + pid->integral + (d * pid->kd); + + } else { + if (isfinite(i)) { + pid->integral = i; + } + } + + if (isfinite(output)) { + if (output > pid->limit_max) { + output = pid->limit_max; + + } else if (output < pid->limit_min) { + output = pid->limit_min; + } + + pid->last_output = output; + } + + return pid->last_output; +} + +__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i) +{ + pid->integral = i; +} diff --git a/src/modules/multirotor_pos_control/position_control.h b/src/modules/multirotor_pos_control/thrust_pid.h index 2144ebc34..5e169c1ba 100644 --- a/src/modules/multirotor_pos_control/position_control.h +++ b/src/modules/multirotor_pos_control/thrust_pid.h @@ -1,10 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * @author Laurens Mackay <mackayl@student.ethz.ch> - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,15 +33,44 @@ ****************************************************************************/ /** - * @file multirotor_position_control.h - * Definition of the position control for a multirotor VTOL + * @file thrust_pid.h + * + * Definition of thrust control PID interface. + * + * @author Anton Babushkin <anton.babushkin@me.com> */ -// #ifndef POSITION_CONTROL_H_ -// #define POSITION_CONTROL_H_ +#ifndef THRUST_PID_H_ +#define THRUST_PID_H_ + +#include <stdint.h> + +__BEGIN_DECLS + +/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error */ +#define THRUST_PID_MODE_DERIVATIV_CALC 0 +/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored */ +#define THRUST_PID_MODE_DERIVATIV_CALC_NO_SP 1 + +typedef struct { + float kp; + float ki; + float kd; + float sp; + float integral; + float error_previous; + float last_output; + float limit_min; + float limit_max; + float dt_min; + uint8_t mode; +} thrust_pid_t; + +__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, float r22); +__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i); -// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, -// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, -// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp); +__END_DECLS -// #endif /* POSITION_CONTROL_H_ */ +#endif /* THRUST_PID_H_ */ |