fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-06 09:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

multirotor_pos_control: cleanup, altitude hold mode implemented

Browse Source
This commit is contained in:
Anton
2013-04-08 17:08:16 +04:00
parent 4a062086ae
commit 825957cde0
6 changed files with 262 additions and 463 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/multirotor_att_control/multirotor_att_control_main.c
+95 -95
View File
@@ -242,124 +242,124 @@ mc_thread_main(int argc, char *argv[])
} else if (state.flag_control_manual_enabled) { } 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 */ /* initialize to current yaw if switching to manual or att control */
if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
state.flag_control_manual_enabled != flag_control_manual_enabled || state.flag_control_manual_enabled != flag_control_manual_enabled ||
state.flag_system_armed != flag_system_armed) { 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) {
att_sp.yaw_body = att.yaw; att_sp.yaw_body = att.yaw;
} }
/* act if stabilization is active or if the (nonsense) direct pass through mode is set */ static bool rc_loss_first_time = true;
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) { /* if the RC signal is lost, try to stay level and go slowly back down to ground */
rates_sp.yaw = manual.yaw; if (state.rc_signal_lost) {
control_yaw_position = false; /* 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 { } else {
/* att_sp.thrust = 0.0f;
* 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.
*/
/* only move setpoint if manual input is != 0 */ /* keep current yaw, do not attempt to go to north orientation,
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { * 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; rates_sp.yaw = manual.yaw;
control_yaw_position = false; control_yaw_position = false;
first_time_after_yaw_speed_control = true;
} else { } else {
if (first_time_after_yaw_speed_control) { /*
att_sp.yaw_body = att.yaw; * This mode SHOULD be the default mode, which is:
first_time_after_yaw_speed_control = false; * 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; if (motor_test_mode) {
att_sp.timestamp = hrt_absolute_time(); 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 */ /* 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 */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
} else { } else {
/* manual rate inputs, from RC control or joystick */ /* manual rate inputs, from RC control or joystick */
if (state.flag_control_rates_enabled && if (state.flag_control_rates_enabled &&
state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
rates_sp.roll = manual.roll; rates_sp.roll = manual.roll;
rates_sp.pitch = manual.pitch; rates_sp.pitch = manual.pitch;
rates_sp.yaw = manual.yaw; rates_sp.yaw = manual.yaw;
rates_sp.thrust = manual.throttle; rates_sp.thrust = manual.throttle;
rates_sp.timestamp = hrt_absolute_time(); rates_sp.timestamp = hrt_absolute_time();
}
} }
} }
} }
/** STEP 3: Identify the controller setup to run and set up the inputs correctly */ /** STEP 3: Identify the controller setup to run and set up the inputs correctly */
apps/multirotor_pos_control/multirotor_pos_control.c
+135 -68
View File
@@ -35,7 +35,7 @@
/** /**
* @file multirotor_pos_control.c * @file multirotor_pos_control.c
* *
* Skeleton for multirotor position controller * Multirotor position controller
*/ */
#include <nuttx/config.h> #include <nuttx/config.h>
@@ -52,13 +52,16 @@
#include <sys/prctl.h> #include <sys/prctl.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_attitude_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_local_position_setpoint.h>
#include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_vicon_position.h>
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
#include <mavlink/mavlink_log.h>
#include "multirotor_pos_control_params.h" #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 static void usage(const char *reason) {
usage(const char *reason)
{
if (reason) if (reason)
fprintf(stderr, "%s\n", reason); fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n"); fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
@@ -96,21 +97,20 @@ usage(const char *reason)
* The actual stack size should be set in the call * The actual stack size should be set in the call
* to task_spawn(). * to task_spawn().
*/ */
int multirotor_pos_control_main(int argc, char *argv[]) int multirotor_pos_control_main(int argc, char *argv[]) {
{
if (argc < 1) if (argc < 1)
usage("missing command"); usage("missing command");
if (!strcmp(argv[1], "start")) { if (!strcmp(argv[1], "start")) {
if (thread_running) { if (thread_running) {
printf("multirotor pos control already running\n"); printf("multirotor_pos_control already running\n");
/* this is not an error */ /* this is not an error */
exit(0); exit(0);
} }
thread_should_exit = false; thread_should_exit = false;
deamon_task = task_spawn("multirotor pos control", deamon_task = task_spawn("multirotor_pos_control",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 60, SCHED_PRIORITY_MAX - 60,
4096, 4096,
@@ -126,9 +126,9 @@ int multirotor_pos_control_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) { if (!strcmp(argv[1], "status")) {
if (thread_running) { if (thread_running) {
printf("\tmultirotor pos control app is running\n"); printf("\tmultirotor_pos_control app is running\n");
} else { } else {
printf("\tmultirotor pos control app not started\n"); printf("\tmultirotor_pos_control app not started\n");
} }
exit(0); exit(0);
} }
@@ -137,98 +137,165 @@ int multirotor_pos_control_main(int argc, char *argv[])
exit(1); exit(1);
} }
static int static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
multirotor_pos_control_thread_main(int argc, char *argv[])
{
/* welcome user */ /* 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 */ /* structures */
struct vehicle_status_s state; struct vehicle_status_s status;
memset(&status, 0, sizeof(status));
struct vehicle_attitude_s att; struct vehicle_attitude_s att;
//struct vehicle_global_position_setpoint_s global_pos_sp; memset(&att, 0, sizeof(att));
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_vicon_position_s local_pos;
struct manual_control_setpoint_s manual;
struct vehicle_attitude_setpoint_s att_sp; 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 */ /* 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 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 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_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); 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; thread_running = true;
int loopcounter = 0; struct multirotor_position_control_params params;
struct multirotor_position_control_param_handles params_h;
parameters_init(&params_h);
parameters_update(&params_h, &params);
struct multirotor_position_control_params p; int paramcheck_counter = 0;
struct multirotor_position_control_param_handles h;
parameters_init(&h);
parameters_update(&h, &p);
while (!thread_should_exit) {
while (1) {
/* get a local copy of the vehicle state */ /* 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 */ /* get a local copy of manual setpoint */
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
/* get a local copy of attitude */ /* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
/* get a local copy of local position */ /* get a local copy of local position */
orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos); orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
/* get a local copy of local position setpoint */ if (status.state_machine == SYSTEM_STATE_AUTO) {
orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); /* 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;
} }
// if (state.state_machine == SYSTEM_STATE_AUTO) { /* check parameters at 1 Hz*/
paramcheck_counter++;
// XXX IMPLEMENT POSITION CONTROL HERE if (paramcheck_counter == 50) {
bool param_updated;
orb_check(param_sub, &param_updated);
if (param_updated) {
parameters_update(&params_h, &params);
}
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 /* PID for altitude */
/* local pos is the Vicon position */ float alt_err = local_pos.z - local_pos_sp.z;
/* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */
// XXX just an example, lacks rotation around world-body transformation if (alt_err > err_linear_limit) {
att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p; alt_err = err_linear_limit;
att_sp.roll_body = 0.0f; } else if (alt_err < -err_linear_limit) {
att_sp.yaw_body = 0.0f; alt_err = -err_linear_limit;
att_sp.thrust = 0.3f; }
att_sp.timestamp = hrt_absolute_time(); /* PID for altitude rate */
float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d;
/* publish new attitude setpoint */ float thrust_ctl = thrust_ctl_pd + alt_integral;
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); if (thrust_ctl < params.thr_min) {
// } else if (state.state_machine == SYSTEM_STATE_STABILIZED) { thrust_ctl = params.thr_min;
/* set setpoint to current position */ } else if (thrust_ctl > params.thr_max) {
// XXX select pos reset channel on remote thrust_ctl = params.thr_max;
/* reset setpoint to current position (position hold) */ } else {
// if (1 == 2) { /* integrate only in linear area (with 20% gap) and not on min/max throttle */
// local_pos_sp.x = local_pos.x; if (fabs(thrust_ctl_pd) < err_linear_limit * params.alt_p * 0.8f)
// local_pos_sp.y = local_pos.y; alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt;
// local_pos_sp.z = local_pos.z; }
// local_pos_sp.yaw = att.yaw; 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 */ /* run at approximately 50 Hz */
usleep(20000); 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; thread_running = false;
apps/multirotor_pos_control/multirotor_pos_control_params.c
+20 -9
View File
@@ -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" #include "multirotor_pos_control_params.h"
/* Extended Kalman Filter covariances */
/* controller parameters */ /* 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) 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 */ /* 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; return OK;
} }
int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p) 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; return OK;
} }
apps/multirotor_pos_control/multirotor_pos_control_params.h
+12 -6
View File
@@ -42,15 +42,21 @@
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
struct multirotor_position_control_params { struct multirotor_position_control_params {
float p; float thr_min;
float i; float thr_max;
float d; float alt_p;
float alt_i;
float alt_d;
float alt_rate_max;
}; };
struct multirotor_position_control_param_handles { struct multirotor_position_control_param_handles {
param_t p; param_t thr_min;
param_t i; param_t thr_max;
param_t d; param_t alt_p;
param_t alt_i;
param_t alt_d;
param_t alt_rate_max;
}; };
/** /**
apps/multirotor_pos_control/position_control.c
-235
View File
@@ -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++;
// }
apps/multirotor_pos_control/position_control.h
-50
View File
@@ -1,50 +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.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_ */