diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-08 22:38:44 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-08 22:38:44 +0200 |
commit | 27c5cef054b8db1ad14ea6ba403bbfe811d99d9a (patch) | |
tree | baa7a9f77381faa50a4f5d36a4d8689bbf0fc347 /apps | |
parent | 36ed8bb97a5b3638f8d3678a7c49744d21ea2e19 (diff) | |
download | px4-firmware-27c5cef054b8db1ad14ea6ba403bbfe811d99d9a.tar.gz px4-firmware-27c5cef054b8db1ad14ea6ba403bbfe811d99d9a.tar.bz2 px4-firmware-27c5cef054b8db1ad14ea6ba403bbfe811d99d9a.zip |
Added RC params, fixed attitude and position control
Diffstat (limited to 'apps')
-rw-r--r-- | apps/multirotor_att_control/multirotor_att_control_main.c | 12 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_attitude_control.c | 68 | ||||
-rw-r--r-- | apps/sensors/sensor_params.c | 16 | ||||
-rw-r--r-- | apps/uORB/topics/vehicle_attitude_setpoint.h | 3 |
4 files changed, 61 insertions, 38 deletions
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 350308fa3..b08723452 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -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; diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 312942acb..38fe13bfe 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -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 diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index 221711c7f..6e36002dd 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -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 diff --git a/apps/uORB/topics/vehicle_attitude_setpoint.h b/apps/uORB/topics/vehicle_attitude_setpoint.h index 65ba98e9d..0c33ebff8 100644 --- a/apps/uORB/topics/vehicle_attitude_setpoint.h +++ b/apps/uORB/topics/vehicle_attitude_setpoint.h @@ -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 */ |