forked from Archive/PX4-Autopilot
Added RC params, fixed attitude and position control
This commit is contained in:
parent
36ed8bb97a
commit
27c5cef054
|
@ -98,7 +98,6 @@ mc_thread_main(int argc, char *argv[])
|
|||
memset(&setpoint, 0, sizeof(setpoint));
|
||||
|
||||
struct actuator_controls_s actuators;
|
||||
struct actuator_armed_s armed;
|
||||
|
||||
/* subscribe to attitude, motor setpoints and system state */
|
||||
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 };
|
||||
|
||||
/* 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;
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
/* 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.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();
|
||||
|
||||
if (motor_test_mode) {
|
||||
att_sp.roll_body = 0.0f;
|
||||
att_sp.pitch_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;
|
||||
} else {
|
||||
att_sp.thrust = manual.throttle;
|
||||
|
|
|
@ -54,21 +54,21 @@
|
|||
#include <systemlib/param/param.h>
|
||||
#include <arch/board/up_hrt.h>
|
||||
|
||||
PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f);
|
||||
PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);
|
||||
PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
|
||||
// PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f);
|
||||
// PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);
|
||||
// PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
|
||||
// PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.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_D, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.02f);
|
||||
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_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_LIM, 0.3f);
|
||||
|
||||
|
@ -76,11 +76,11 @@ PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f);
|
|||
PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f);
|
||||
|
||||
struct mc_att_control_params {
|
||||
float yaw_p;
|
||||
float yaw_i;
|
||||
float yaw_d;
|
||||
float yaw_awu;
|
||||
float yaw_lim;
|
||||
// float yaw_p;
|
||||
// float yaw_i;
|
||||
// float yaw_d;
|
||||
// float yaw_awu;
|
||||
// float yaw_lim;
|
||||
|
||||
float yawrate_p;
|
||||
float yawrate_i;
|
||||
|
@ -99,11 +99,11 @@ struct mc_att_control_params {
|
|||
};
|
||||
|
||||
struct mc_att_control_param_handles {
|
||||
param_t yaw_p;
|
||||
param_t yaw_i;
|
||||
param_t yaw_d;
|
||||
param_t yaw_awu;
|
||||
param_t yaw_lim;
|
||||
// param_t yaw_p;
|
||||
// param_t yaw_i;
|
||||
// param_t yaw_d;
|
||||
// param_t yaw_awu;
|
||||
// param_t yaw_lim;
|
||||
|
||||
param_t yawrate_p;
|
||||
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)
|
||||
{
|
||||
/* PID parameters */
|
||||
h->yaw_p = param_find("MC_YAWPOS_P");
|
||||
h->yaw_i = param_find("MC_YAWPOS_I");
|
||||
h->yaw_d = param_find("MC_YAWPOS_D");
|
||||
h->yaw_awu = param_find("MC_YAWPOS_AWU");
|
||||
h->yaw_lim = param_find("MC_YAWPOS_LIM");
|
||||
// h->yaw_p = param_find("MC_YAWPOS_P");
|
||||
// h->yaw_i = param_find("MC_YAWPOS_I");
|
||||
// h->yaw_d = param_find("MC_YAWPOS_D");
|
||||
// h->yaw_awu = param_find("MC_YAWPOS_AWU");
|
||||
// h->yaw_lim = param_find("MC_YAWPOS_LIM");
|
||||
|
||||
h->yawrate_p = param_find("MC_YAWRATE_P");
|
||||
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)
|
||||
{
|
||||
param_get(h->yaw_p, &(p->yaw_p));
|
||||
param_get(h->yaw_i, &(p->yaw_i));
|
||||
param_get(h->yaw_d, &(p->yaw_d));
|
||||
param_get(h->yaw_awu, &(p->yaw_awu));
|
||||
param_get(h->yaw_lim, &(p->yaw_lim));
|
||||
// param_get(h->yaw_p, &(p->yaw_p));
|
||||
// param_get(h->yaw_i, &(p->yaw_i));
|
||||
// param_get(h->yaw_d, &(p->yaw_d));
|
||||
// param_get(h->yaw_awu, &(p->yaw_awu));
|
||||
// param_get(h->yaw_lim, &(p->yaw_lim));
|
||||
|
||||
param_get(h->yawrate_p, &(p->yawrate_p));
|
||||
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 PID_t yaw_pos_controller;
|
||||
// static PID_t yaw_pos_controller;
|
||||
static PID_t yaw_speed_controller;
|
||||
static PID_t pitch_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_update(&h, &p);
|
||||
|
||||
pid_init(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu,
|
||||
PID_MODE_DERIVATIV_CALC, 154);
|
||||
// pid_init(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu,
|
||||
// PID_MODE_DERIVATIV_SET, 154);
|
||||
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_MODE_DERIVATIV_SET, 156);
|
||||
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 */
|
||||
parameters_update(&h, &p);
|
||||
/* 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(&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);
|
||||
|
@ -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,
|
||||
att->roll, att->rollspeed, deltaT);
|
||||
/* 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
|
||||
|
|
|
@ -60,41 +60,57 @@ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
|
|||
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
|
||||
PARAM_DEFINE_FLOAT(RC1_MAX, 2000.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_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC2_MAX, 2000);
|
||||
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_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC3_MAX, 2000);
|
||||
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_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC4_MAX, 2000);
|
||||
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_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC5_MAX, 2000);
|
||||
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_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC6_MAX, 2000);
|
||||
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_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC7_MAX, 2000);
|
||||
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_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC8_MAX, 2000);
|
||||
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
|
||||
|
||||
|
|
|
@ -64,6 +64,9 @@ struct vehicle_attitude_setpoint_s
|
|||
float roll_body; /**< body angle in NED frame */
|
||||
float pitch_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 R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */
|
||||
|
|
Loading…
Reference in New Issue