diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index d94c0a69c9..09710f0fcb 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -242,124 +242,124 @@ mc_thread_main(int argc, char *argv[]) } else if (state.flag_control_manual_enabled) { - if (state.flag_control_attitude_enabled) { + if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS) { + /* direct manual input */ + if (state.flag_control_attitude_enabled) { - /* initialize to current yaw if switching to manual or att control */ - if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || - state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_system_armed != flag_system_armed) { - att_sp.yaw_body = att.yaw; - } - - static bool rc_loss_first_time = true; - - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (state.rc_signal_lost) { - /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - param_get(failsafe_throttle_handle, &failsafe_throttle); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - - /* - * Only go to failsafe throttle if last known throttle was - * high enough to create some lift to make hovering state likely. - * - * This is to prevent that someone landing, but not disarming his - * multicopter (throttle = 0) does not make it jump up in the air - * if shutting down his remote. - */ - if (isfinite(manual.throttle) && manual.throttle > 0.2f) { - att_sp.thrust = failsafe_throttle; - - } else { - att_sp.thrust = 0.0f; - } - - /* keep current yaw, do not attempt to go to north orientation, - * since if the pilot regains RC control, he will be lost regarding - * the current orientation. - */ - if (rc_loss_first_time) - att_sp.yaw_body = att.yaw; - - rc_loss_first_time = false; - - } else { - rc_loss_first_time = true; - - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; - - /* set attitude if arming */ - if (!flag_control_attitude_enabled && state.flag_system_armed) { + /* initialize to current yaw if switching to manual or att control */ + if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || + state.flag_control_manual_enabled != flag_control_manual_enabled || + state.flag_system_armed != flag_system_armed) { att_sp.yaw_body = att.yaw; } - /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ - if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + static bool rc_loss_first_time = true; - if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; + /* if the RC signal is lost, try to stay level and go slowly back down to ground */ + if (state.rc_signal_lost) { + /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ + param_get(failsafe_throttle_handle, &failsafe_throttle); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + + /* + * Only go to failsafe throttle if last known throttle was + * high enough to create some lift to make hovering state likely. + * + * This is to prevent that someone landing, but not disarming his + * multicopter (throttle = 0) does not make it jump up in the air + * if shutting down his remote. + */ + if (isfinite(manual.throttle) && manual.throttle > 0.2f) { + att_sp.thrust = failsafe_throttle; } else { - /* - * This mode SHOULD be the default mode, which is: - * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS - * - * However, we fall back to this setting for all other (nonsense) - * settings as well. - */ + att_sp.thrust = 0.0f; + } - /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + /* keep current yaw, do not attempt to go to north orientation, + * since if the pilot regains RC control, he will be lost regarding + * the current orientation. + */ + if (rc_loss_first_time) + att_sp.yaw_body = att.yaw; + + rc_loss_first_time = false; + + } else { + rc_loss_first_time = true; + + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; + + /* set attitude if arming */ + if (!flag_control_attitude_enabled && state.flag_system_armed) { + att_sp.yaw_body = att.yaw; + } + + /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ + if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + + if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { rates_sp.yaw = manual.yaw; control_yaw_position = false; - first_time_after_yaw_speed_control = true; } else { - if (first_time_after_yaw_speed_control) { - att_sp.yaw_body = att.yaw; - first_time_after_yaw_speed_control = false; - } + /* + * This mode SHOULD be the default mode, which is: + * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS + * + * However, we fall back to this setting for all other (nonsense) + * settings as well. + */ - control_yaw_position = true; + /* only move setpoint if manual input is != 0 */ + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + first_time_after_yaw_speed_control = true; + + } else { + if (first_time_after_yaw_speed_control) { + att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; + } + + control_yaw_position = true; + } } } + + att_sp.thrust = manual.throttle; + att_sp.timestamp = hrt_absolute_time(); } - att_sp.thrust = manual.throttle; - att_sp.timestamp = hrt_absolute_time(); - } + if (motor_test_mode) { + printf("testmode"); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.1f; + att_sp.timestamp = hrt_absolute_time(); + } - /* STEP 2: publish the controller output */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - - if (motor_test_mode) { - printf("testmode"); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.1f; - att_sp.timestamp = hrt_absolute_time(); - /* STEP 2: publish the result to the vehicle actuators */ + /* STEP 2: publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } - } else { - /* manual rate inputs, from RC control or joystick */ - if (state.flag_control_rates_enabled && - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { - rates_sp.roll = manual.roll; + } else { + /* manual rate inputs, from RC control or joystick */ + if (state.flag_control_rates_enabled && + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { + rates_sp.roll = manual.roll; - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); + rates_sp.pitch = manual.pitch; + rates_sp.yaw = manual.yaw; + rates_sp.thrust = manual.throttle; + rates_sp.timestamp = hrt_absolute_time(); + } } } - } /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index 7b8d83aa8a..d2b6685aca 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -35,7 +35,7 @@ /** * @file multirotor_pos_control.c * - * Skeleton for multirotor position controller + * Multirotor position controller */ #include @@ -52,13 +52,16 @@ #include #include #include +#include #include #include #include #include +#include #include #include #include +#include #include "multirotor_pos_control_params.h" @@ -79,9 +82,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]); */ static void usage(const char *reason); -static void -usage(const char *reason) -{ +static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); fprintf(stderr, "usage: deamon {start|stop|status} [-p ]\n\n"); @@ -96,21 +97,20 @@ usage(const char *reason) * The actual stack size should be set in the call * to task_spawn(). */ -int multirotor_pos_control_main(int argc, char *argv[]) -{ +int multirotor_pos_control_main(int argc, char *argv[]) { if (argc < 1) usage("missing command"); if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("multirotor pos control already running\n"); + printf("multirotor_pos_control already running\n"); /* this is not an error */ exit(0); } thread_should_exit = false; - deamon_task = task_spawn("multirotor pos control", + deamon_task = task_spawn("multirotor_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 60, 4096, @@ -126,9 +126,9 @@ int multirotor_pos_control_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tmultirotor pos control app is running\n"); + printf("\tmultirotor_pos_control app is running\n"); } else { - printf("\tmultirotor pos control app not started\n"); + printf("\tmultirotor_pos_control app not started\n"); } exit(0); } @@ -137,98 +137,165 @@ int multirotor_pos_control_main(int argc, char *argv[]) exit(1); } -static int -multirotor_pos_control_thread_main(int argc, char *argv[]) -{ +static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* welcome user */ - printf("[multirotor pos control] Control started, taking over position control\n"); + printf("[multirotor_pos_control] started\n"); + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd, "[multirotor_pos_control] started"); /* structures */ - struct vehicle_status_s state; + struct vehicle_status_s status; + memset(&status, 0, sizeof(status)); 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)); /* subscribe to attitude, motor setpoints and system state */ - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int param_sub = orb_subscribe(ORB_ID(parameter_update)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); 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)); - /* 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 att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + bool reset_sp_alt = true; + bool reset_sp_pos = true; + hrt_abstime t_prev = 0; + float alt_integral = 0.0f; + float alt_ctl_dz = 0.2f; + thread_running = true; - int 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); - struct multirotor_position_control_params p; - struct multirotor_position_control_param_handles h; - parameters_init(&h); - parameters_update(&h, &p); + int paramcheck_counter = 0; - - while (1) { + while (!thread_should_exit) { /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); + orb_copy(ORB_ID(vehicle_status), state_sub, &status); /* 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; + orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); + if (status.state_machine == SYSTEM_STATE_AUTO) { + /* get a local copy of local position setpoint */ + orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); } - // if (state.state_machine == SYSTEM_STATE_AUTO) { - - // XXX IMPLEMENT POSITION CONTROL HERE + /* check parameters at 1 Hz*/ + paramcheck_counter++; + if (paramcheck_counter == 50) { + bool param_updated; + orb_check(param_sub, ¶m_updated); + if (param_updated) { + parameters_update(¶ms_h, ¶ms); + } + paramcheck_counter = 0; + } - float dT = 1.0f / 50.0f; + if (status.state_machine == SYSTEM_STATE_MANUAL) { + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE || status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { + hrt_abstime t = hrt_absolute_time(); + if (reset_sp_alt) { + reset_sp_alt = false; + local_pos_sp.z = local_pos.z; + alt_integral = manual.throttle; + char str[80]; + sprintf(str, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); + mavlink_log_info(mavlink_fd, str); + } - float x_setpoint = 0.0f; + float dt; + if (t_prev != 0) { + dt = (t - t_prev) * 0.000001f; + } else { + dt = 0.0f; + } + float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; + /* move altitude setpoint by manual controls */ + float alt_ctl = manual.throttle - 0.5f; + if (fabs(alt_ctl) < alt_ctl_dz) { + alt_ctl = 0.0f; + } else { + if (alt_ctl > 0.0f) + alt_ctl = (alt_ctl - alt_ctl_dz) / (0.5f - alt_ctl_dz); + else + alt_ctl = (alt_ctl + alt_ctl_dz) / (0.5f - alt_ctl_dz); + local_pos_sp.z -= alt_ctl * params.alt_rate_max * dt; + if (local_pos_sp.z > local_pos.z + err_linear_limit) + local_pos_sp.z = local_pos.z + err_linear_limit; + else if (local_pos_sp.z < local_pos.z - err_linear_limit) + local_pos_sp.z = local_pos.z - err_linear_limit; + } - // 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; - // } - // } + /* PID for altitude */ + float alt_err = local_pos.z - local_pos_sp.z; + /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ + if (alt_err > err_linear_limit) { + alt_err = err_linear_limit; + } else if (alt_err < -err_linear_limit) { + alt_err = -err_linear_limit; + } + /* PID for altitude rate */ + float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d; + float thrust_ctl = thrust_ctl_pd + alt_integral; + if (thrust_ctl < params.thr_min) { + thrust_ctl = params.thr_min; + } else if (thrust_ctl > params.thr_max) { + thrust_ctl = params.thr_max; + } else { + /* integrate only in linear area (with 20% gap) and not on min/max throttle */ + if (fabs(thrust_ctl_pd) < err_linear_limit * params.alt_p * 0.8f) + alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; + } + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { + // TODO add position controller + att_sp.pitch_body = manual.pitch; + att_sp.roll_body = manual.roll; + att_sp.yaw_body = manual.yaw; + } else { + att_sp.pitch_body = manual.pitch; + att_sp.roll_body = manual.roll; + att_sp.yaw_body = manual.yaw; + } + att_sp.thrust = thrust_ctl; + att_sp.timestamp = hrt_absolute_time(); + /* publish local position setpoint */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + /* publish new attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + t_prev = t; + } else { + reset_sp_alt = true; + } + } else { + reset_sp_alt = true; + } /* run at approximately 50 Hz */ usleep(20000); - loopcounter++; } - printf("[multirotor pos control] ending now...\n"); + printf("[multirotor_pos_control] exiting\n"); + mavlink_log_info(mavlink_fd, "[multirotor_pos_control] exiting"); thread_running = false; diff --git a/apps/multirotor_pos_control/multirotor_pos_control_params.c b/apps/multirotor_pos_control/multirotor_pos_control_params.c index 6b73dc4052..90dd820f43 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control_params.c +++ b/apps/multirotor_pos_control/multirotor_pos_control_params.c @@ -34,29 +34,40 @@ ****************************************************************************/ /* - * @file multirotor_position_control_params.c + * @file multirotor_pos_control_params.c * - * Parameters for EKF filter + * 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.3f); +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f); +PARAM_DEFINE_FLOAT(MPC_ALT_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_ALT_I, 0.01f); +PARAM_DEFINE_FLOAT(MPC_ALT_D, 0.2f); +PARAM_DEFINE_FLOAT(MPC_ALT_RATE_MAX, 3.0f); int parameters_init(struct multirotor_position_control_param_handles *h) { + h->thr_min = param_find("MPC_THR_MIN"); + h->thr_max = param_find("MPC_THR_MAX"); /* PID parameters */ - h->p = param_find("MC_POS_P"); - + h->alt_p = param_find("MPC_ALT_P"); + h->alt_i = param_find("MPC_ALT_I"); + h->alt_d = param_find("MPC_ALT_D"); + h->alt_rate_max = param_find("MPC_ALT_RATE_MAX"); 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->alt_p, &(p->alt_p)); + param_get(h->alt_i, &(p->alt_i)); + param_get(h->alt_d, &(p->alt_d)); + param_get(h->alt_rate_max, &(p->alt_rate_max)); return OK; } diff --git a/apps/multirotor_pos_control/multirotor_pos_control_params.h b/apps/multirotor_pos_control/multirotor_pos_control_params.h index 274f6c22a9..849055cd16 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control_params.h +++ b/apps/multirotor_pos_control/multirotor_pos_control_params.h @@ -42,15 +42,21 @@ #include struct multirotor_position_control_params { - float p; - float i; - float d; + float thr_min; + float thr_max; + float alt_p; + float alt_i; + float alt_d; + float alt_rate_max; }; 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 alt_p; + param_t alt_i; + param_t alt_d; + param_t alt_rate_max; }; /** diff --git a/apps/multirotor_pos_control/position_control.c b/apps/multirotor_pos_control/position_control.c deleted file mode 100644 index 9c53a5bf6c..0000000000 --- a/apps/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 -// * @author Laurens Mackay -// * @author Tobias Naegeli -// * @author Martin Rutschmann -// * -// * 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 -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// #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/apps/multirotor_pos_control/position_control.h b/apps/multirotor_pos_control/position_control.h deleted file mode 100644 index 2144ebc343..0000000000 --- a/apps/multirotor_pos_control/position_control.h +++ /dev/null @@ -1,50 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * - * 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.h - * Definition of the position control for a multirotor VTOL - */ - -// #ifndef POSITION_CONTROL_H_ -// #define 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); - -// #endif /* POSITION_CONTROL_H_ */