diff options
Diffstat (limited to 'src/modules/multirotor_pos_control/multirotor_pos_control.c')
| -rw-r--r-- | src/modules/multirotor_pos_control/multirotor_pos_control.c | 626 |
1 files changed, 539 insertions, 87 deletions
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index f39d11438..3d23d0c09 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,534 @@ 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(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 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 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; - 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); + + 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; } - // 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_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); - loopcounter++; - } - printf("[multirotor pos control] ending now...\n"); + warnx("stopped"); + mavlink_log_info(mavlink_fd, "[mpc] stopped"); thread_running = false; |