Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-04 05:05:19 +08:00
Code Issues Actions 12 Packages Projects Releases Wiki Activity

Added RC params, fixed attitude and position control

Browse Source
This commit is contained in:
Lorenz Meier
2012-09-08 22:38:44 +02:00
parent 36ed8bb97a
commit 27c5cef054
4 changed files with 61 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/multirotor_att_control/multirotor_att_control_main.c
+8 -4
View File
@@ -98,7 +98,6 @@ mc_thread_main(int argc, char *argv[])
memset(&setpoint, 0, sizeof(setpoint)); memset(&setpoint, 0, sizeof(setpoint));
struct actuator_controls_s actuators; struct actuator_controls_s actuators;
struct actuator_armed_s armed;
/* 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 att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
@@ -113,10 +112,11 @@ mc_thread_main(int argc, char *argv[])
struct pollfd fds = { .fd = att_sub, .events = POLLIN }; struct pollfd fds = { .fd = att_sub, .events = POLLIN };
/* publish actuator controls */ /* publish actuator controls */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
actuators.control[i] = 0.0f; actuators.control[i] = 0.0f;
}
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
armed.armed = false;
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);
/* register the perf counter */ /* register the perf counter */
@@ -143,12 +143,16 @@ mc_thread_main(int argc, char *argv[])
att_sp.roll_body = -manual.roll * M_PI_F / 8.0f; att_sp.roll_body = -manual.roll * M_PI_F / 8.0f;
att_sp.pitch_body = -manual.pitch * M_PI_F / 8.0f; att_sp.pitch_body = -manual.pitch * M_PI_F / 8.0f;
att_sp.yaw_body = -manual.yaw * M_PI_F; att_sp.yaw_rate_body = -manual.yaw * M_PI_F;
att_sp.timestamp = hrt_absolute_time(); att_sp.timestamp = hrt_absolute_time();
if (motor_test_mode) { if (motor_test_mode) {
att_sp.roll_body = 0.0f; att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f; att_sp.pitch_body = 0.0f;
att_sp.yaw_body = 0.0f; att_sp.yaw_body = 0.0f;
att_sp.roll_rate_body = 0.0f;
att_sp.pitch_rate_body = 0.0f;
att_sp.yaw_rate_body = 0.0f;
att_sp.thrust = 0.1f; att_sp.thrust = 0.1f;
} else { } else {
att_sp.thrust = manual.throttle; att_sp.thrust = manual.throttle;
apps/multirotor_att_control/multirotor_attitude_control.c
+34 -34
View File
@@ -54,21 +54,21 @@
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
#include <arch/board/up_hrt.h> #include <arch/board/up_hrt.h>
PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f); // PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); // PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); // PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); // PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); // PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.1f); PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.1f); /* same on Flamewheel */
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.02f); PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.02f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.02f); PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.02f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.1f); PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.1f);
PARAM_DEFINE_FLOAT(MC_ATT_P, 0.3f); PARAM_DEFINE_FLOAT(MC_ATT_P, 0.3f); /* 0.15 F405 Flamewheel */
PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATT_D, 0.1f); PARAM_DEFINE_FLOAT(MC_ATT_D, 0.1f); /* 0.025 F405 Flamewheel */
PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f);
PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.3f); PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.3f);
@@ -76,11 +76,11 @@ PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f); PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f);
struct mc_att_control_params { struct mc_att_control_params {
float yaw_p; // float yaw_p;
float yaw_i; // float yaw_i;
float yaw_d; // float yaw_d;
float yaw_awu; // float yaw_awu;
float yaw_lim; // float yaw_lim;
float yawrate_p; float yawrate_p;
float yawrate_i; float yawrate_i;
@@ -99,11 +99,11 @@ struct mc_att_control_params {
}; };
struct mc_att_control_param_handles { struct mc_att_control_param_handles {
param_t yaw_p; // param_t yaw_p;
param_t yaw_i; // param_t yaw_i;
param_t yaw_d; // param_t yaw_d;
param_t yaw_awu; // param_t yaw_awu;
param_t yaw_lim; // param_t yaw_lim;
param_t yawrate_p; param_t yawrate_p;
param_t yawrate_i; param_t yawrate_i;
@@ -137,11 +137,11 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
static int parameters_init(struct mc_att_control_param_handles *h) static int parameters_init(struct mc_att_control_param_handles *h)
{ {
/* PID parameters */ /* PID parameters */
h->yaw_p = param_find("MC_YAWPOS_P"); // h->yaw_p = param_find("MC_YAWPOS_P");
h->yaw_i = param_find("MC_YAWPOS_I"); // h->yaw_i = param_find("MC_YAWPOS_I");
h->yaw_d = param_find("MC_YAWPOS_D"); // h->yaw_d = param_find("MC_YAWPOS_D");
h->yaw_awu = param_find("MC_YAWPOS_AWU"); // h->yaw_awu = param_find("MC_YAWPOS_AWU");
h->yaw_lim = param_find("MC_YAWPOS_LIM"); // h->yaw_lim = param_find("MC_YAWPOS_LIM");
h->yawrate_p = param_find("MC_YAWRATE_P"); h->yawrate_p = param_find("MC_YAWRATE_P");
h->yawrate_i = param_find("MC_YAWRATE_I"); h->yawrate_i = param_find("MC_YAWRATE_I");
@@ -163,11 +163,11 @@ static int parameters_init(struct mc_att_control_param_handles *h)
static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p) static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p)
{ {
param_get(h->yaw_p, &(p->yaw_p)); // param_get(h->yaw_p, &(p->yaw_p));
param_get(h->yaw_i, &(p->yaw_i)); // param_get(h->yaw_i, &(p->yaw_i));
param_get(h->yaw_d, &(p->yaw_d)); // param_get(h->yaw_d, &(p->yaw_d));
param_get(h->yaw_awu, &(p->yaw_awu)); // param_get(h->yaw_awu, &(p->yaw_awu));
param_get(h->yaw_lim, &(p->yaw_lim)); // param_get(h->yaw_lim, &(p->yaw_lim));
param_get(h->yawrate_p, &(p->yawrate_p)); param_get(h->yawrate_p, &(p->yawrate_p));
param_get(h->yawrate_i, &(p->yawrate_i)); param_get(h->yawrate_i, &(p->yawrate_i));
@@ -196,7 +196,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
static int motor_skip_counter = 0; static int motor_skip_counter = 0;
static PID_t yaw_pos_controller; // static PID_t yaw_pos_controller;
static PID_t yaw_speed_controller; static PID_t yaw_speed_controller;
static PID_t pitch_controller; static PID_t pitch_controller;
static PID_t roll_controller; static PID_t roll_controller;
@@ -211,10 +211,10 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
parameters_init(&h); parameters_init(&h);
parameters_update(&h, &p); parameters_update(&h, &p);
pid_init(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu, // pid_init(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu,
PID_MODE_DERIVATIV_CALC, 154); // PID_MODE_DERIVATIV_SET, 154);
pid_init(&yaw_speed_controller, p.yawrate_p, p.yawrate_d, p.yawrate_i, p.yawrate_awu, pid_init(&yaw_speed_controller, p.yawrate_p, p.yawrate_d, p.yawrate_i, p.yawrate_awu,
PID_MODE_DERIVATIV_CALC, 155); PID_MODE_DERIVATIV_SET, 155);
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu, pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu,
PID_MODE_DERIVATIV_SET, 156); PID_MODE_DERIVATIV_SET, 156);
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu, pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu,
@@ -228,7 +228,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
/* update parameters from storage */ /* update parameters from storage */
parameters_update(&h, &p); parameters_update(&h, &p);
/* apply parameters */ /* apply parameters */
pid_set_parameters(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu); // pid_set_parameters(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu);
pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_d, p.yawrate_i, p.yawrate_awu); pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_d, p.yawrate_i, p.yawrate_awu);
pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu); pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu);
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu); pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu);
@@ -243,7 +243,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff, float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff,
att->roll, att->rollspeed, deltaT); att->roll, att->rollspeed, deltaT);
/* control yaw rate */ /* control yaw rate */
float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT); float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_rate_body, att->yawspeed, 0.0f, deltaT);
/* /*
* compensate the vertical loss of thrust * compensate the vertical loss of thrust
apps/sensors/sensor_params.c
+16
View File
@@ -60,41 +60,57 @@ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); PARAM_DEFINE_FLOAT(RC1_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC1_DZ, 0.1f);
PARAM_DEFINE_FLOAT(RC1_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC2_MIN, 1000); PARAM_DEFINE_FLOAT(RC2_MIN, 1000);
PARAM_DEFINE_FLOAT(RC2_TRIM, 1500); PARAM_DEFINE_FLOAT(RC2_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC2_MAX, 2000); PARAM_DEFINE_FLOAT(RC2_MAX, 2000);
PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); PARAM_DEFINE_FLOAT(RC2_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC2_DZ, 0.1f);
PARAM_DEFINE_FLOAT(RC2_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC3_MIN, 1000); PARAM_DEFINE_FLOAT(RC3_MIN, 1000);
PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); PARAM_DEFINE_FLOAT(RC3_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC3_MAX, 2000); PARAM_DEFINE_FLOAT(RC3_MAX, 2000);
PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); PARAM_DEFINE_FLOAT(RC3_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC3_DZ, 0.03f);
PARAM_DEFINE_FLOAT(RC3_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC4_MIN, 1000); PARAM_DEFINE_FLOAT(RC4_MIN, 1000);
PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); PARAM_DEFINE_FLOAT(RC4_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC4_MAX, 2000); PARAM_DEFINE_FLOAT(RC4_MAX, 2000);
PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); PARAM_DEFINE_FLOAT(RC4_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC4_DZ, 0.1f);
PARAM_DEFINE_FLOAT(RC4_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC5_MIN, 1000); PARAM_DEFINE_FLOAT(RC5_MIN, 1000);
PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); PARAM_DEFINE_FLOAT(RC5_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC5_MAX, 2000); PARAM_DEFINE_FLOAT(RC5_MAX, 2000);
PARAM_DEFINE_FLOAT(RC5_REV, 1.0f); PARAM_DEFINE_FLOAT(RC5_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC5_DZ, 0.1f);
PARAM_DEFINE_FLOAT(RC5_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC6_MIN, 1000); PARAM_DEFINE_FLOAT(RC6_MIN, 1000);
PARAM_DEFINE_FLOAT(RC6_TRIM, 1500); PARAM_DEFINE_FLOAT(RC6_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC6_MAX, 2000); PARAM_DEFINE_FLOAT(RC6_MAX, 2000);
PARAM_DEFINE_FLOAT(RC6_REV, 1.0f); PARAM_DEFINE_FLOAT(RC6_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC6_DZ, 0.1f);
PARAM_DEFINE_FLOAT(RC6_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC7_MIN, 1000); PARAM_DEFINE_FLOAT(RC7_MIN, 1000);
PARAM_DEFINE_FLOAT(RC7_TRIM, 1500); PARAM_DEFINE_FLOAT(RC7_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC7_MAX, 2000); PARAM_DEFINE_FLOAT(RC7_MAX, 2000);
PARAM_DEFINE_FLOAT(RC7_REV, 1.0f); PARAM_DEFINE_FLOAT(RC7_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC7_DZ, 0.1f);
PARAM_DEFINE_FLOAT(RC7_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC8_MIN, 1000); PARAM_DEFINE_FLOAT(RC8_MIN, 1000);
PARAM_DEFINE_FLOAT(RC8_TRIM, 1500); PARAM_DEFINE_FLOAT(RC8_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC8_MAX, 2000); PARAM_DEFINE_FLOAT(RC8_MAX, 2000);
PARAM_DEFINE_FLOAT(RC8_REV, 1.0f); PARAM_DEFINE_FLOAT(RC8_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC8_DZ, 0.1f);
PARAM_DEFINE_FLOAT(RC8_EXP, 0.0f);
PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA
apps/uORB/topics/vehicle_attitude_setpoint.h
+3
View File
@@ -64,6 +64,9 @@ struct vehicle_attitude_setpoint_s
float roll_body; /**< body angle in NED frame */ float roll_body; /**< body angle in NED frame */
float pitch_body; /**< body angle in NED frame */ float pitch_body; /**< body angle in NED frame */
float yaw_body; /**< body angle in NED frame */ float yaw_body; /**< body angle in NED frame */
float roll_rate_body; /**< body angle in NED frame */
float pitch_rate_body; /**< body angle in NED frame */
float yaw_rate_body; /**< body angle in NED frame */
float body_valid; /**< Set to true if Tait-Bryan angles are valid */ float body_valid; /**< Set to true if Tait-Bryan angles are valid */
float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */