diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-22 20:55:44 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-22 20:55:44 +0200 |
commit | 8b951ec417454353d61d19b3379e52b6da5dd6b6 (patch) | |
tree | 954ecb29a7184ab7f306ec2cccc6f857fd7f58dd | |
parent | a9b21886f32d0a8ef7cad8dfe7efbc3276f4fd58 (diff) | |
download | px4-firmware-8b951ec417454353d61d19b3379e52b6da5dd6b6.tar.gz px4-firmware-8b951ec417454353d61d19b3379e52b6da5dd6b6.tar.bz2 px4-firmware-8b951ec417454353d61d19b3379e52b6da5dd6b6.zip |
WIP on HIL
-rw-r--r-- | apps/fixedwing_control/fixedwing_control.c | 9 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 47 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_attitude_control.c | 6 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_rate_control.c | 6 | ||||
-rw-r--r-- | apps/systemlib/pid/pid.c | 56 | ||||
-rw-r--r-- | apps/systemlib/pid/pid.h | 5 |
6 files changed, 78 insertions, 51 deletions
diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index 1323f2566..bdf797673 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -223,11 +223,11 @@ int fixedwing_control_thread_main(int argc, char *argv[]) PID_t roll_pos_controller; pid_init(&roll_pos_controller, p.roll_pos_p, p.roll_pos_i, 0.0f, p.roll_pos_awu, - PID_MODE_DERIVATIV_SET, 155); + PID_MODE_DERIVATIV_SET); PID_t heading_controller; pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, - PID_MODE_DERIVATIV_SET, 155); + PID_MODE_DERIVATIV_SET); while(!thread_should_exit) { @@ -245,6 +245,8 @@ int fixedwing_control_thread_main(int argc, char *argv[]) // FIXME SUBSCRIBE if (loopcounter % 20 == 0) { + att_parameters_update(&h, &p); + pos_parameters_update(&hpos, &ppos); pid_set_parameters(&roll_pos_controller, p.roll_pos_p, p.roll_pos_i, 0.0f, p.roll_pos_awu); pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f); } @@ -420,6 +422,9 @@ static int att_parameters_init(struct fw_att_control_param_handles *h) static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p) { param_get(h->roll_pos_p, &(p->roll_pos_p)); + param_get(h->roll_pos_i, &(p->roll_pos_i)); + param_get(h->roll_pos_lim, &(p->roll_pos_lim)); + param_get(h->roll_pos_awu, &(p->roll_pos_awu)); return OK; } diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index ce0a238dd..1f745baac 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -936,29 +936,6 @@ static void *uorb_receiveloop(void *arg) orb_copy(ORB_ID(vehicle_attitude_setpoint), subs->spa_sub, &buf.att_sp); mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, buf.att_sp.timestamp/1000, buf.att_sp.roll_body, buf.att_sp.pitch_body, buf.att_sp.yaw_body, buf.att_sp.thrust); - - /* Only send in HIL mode */ - if (mavlink_hil_enabled) { - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); - - /* HIL message as per MAVLink spec */ - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - buf.att_sp.roll_body, /* this may be replaced by ctrl0 later */ - buf.att_sp.pitch_body, - buf.att_sp.yaw_body, - buf.att_sp.thrust, - 0, - 0, - 0, - 0, - mavlink_mode, - 0); - } } /* --- ACTUATOR OUTPUTS 0 --- */ @@ -975,6 +952,7 @@ static void *uorb_receiveloop(void *arg) buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]); + // if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { // mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), // 1 /* port 1 */, @@ -1093,6 +1071,29 @@ static void *uorb_receiveloop(void *arg) mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl1 ", buf.actuators.control[1]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl2 ", buf.actuators.control[2]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl3 ", buf.actuators.control[3]); + + /* Only send in HIL mode */ + if (mavlink_hil_enabled) { + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_mode = 0; + get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); + + /* HIL message as per MAVLink spec */ + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + buf.actuators.control[0], + buf.actuators.control[1], + buf.actuators.control[2], + buf.actuators.control[3], + 0, + 0, + 0, + 0, + mavlink_mode, + 0); + } } /* --- DEBUG KEY/VALUE --- */ diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index d7413913b..6e2a54438 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -214,11 +214,11 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s // 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_i, p.yawrate_d, p.yawrate_awu, - PID_MODE_DERIVATIV_SET, 155); + PID_MODE_DERIVATIV_SET); 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); pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu, - PID_MODE_DERIVATIV_SET, 157); + PID_MODE_DERIVATIV_SET); initialized = true; } diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index fefd1f767..b4eb3469b 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -157,11 +157,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); pid_init(&yaw_speed_controller, p.yawrate_p, 0, p.yawrate_i, p.yawrate_awu, - PID_MODE_DERIVATIV_SET, 155); + PID_MODE_DERIVATIV_SET); pid_init(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu, - PID_MODE_DERIVATIV_SET, 156); + PID_MODE_DERIVATIV_SET); pid_init(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu, - PID_MODE_DERIVATIV_SET, 157); + PID_MODE_DERIVATIV_SET); initialized = true; } diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index 807373c15..fbf3edc33 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -40,18 +40,19 @@ */ #include "pid.h" +#include <math.h> __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - uint8_t mode, uint8_t plot_i) + uint8_t mode) { pid->kp = kp; pid->ki = ki; pid->kd = kd; pid->intmax = intmax; pid->mode = mode; - pid->plot_i = plot_i; pid->count = 0; pid->saturated = 0; + pid->last_output = 0; pid->sp = 0; pid->error_previous = 0; @@ -63,11 +64,7 @@ __EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float pid->ki = ki; pid->kd = kd; pid->intmax = intmax; - // pid->mode = mode; - - // pid->sp = 0; - // pid->error_previous = 0; - // pid->integral = 0; + // pid->limit = limit; } //void pid_set(PID_t *pid, float sp) @@ -95,6 +92,11 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo goto start */ + if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) + { + return pid->last_output; + } + float i, d; pid->sp = sp; float error = pid->sp - val; @@ -111,7 +113,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo } // Anti-Windup. Needed if we don't use the saturation above. - if (pid->intmax != 0.0) { + if (pid->intmax != 0.0f) { if (i > pid->intmax) { pid->integral = pid->intmax; @@ -122,14 +124,6 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo } else { pid->integral = i; } - - //Send Controller integrals - // Disabled because of new possibilities with debug_vect. - // Now sent in Main Loop at 5 Hz. 26.06.2010 Laurens - // if (pid->plot_i && (pid->count++ % 16 == 0)&&(global_data.param[PARAM_SEND_SLOT_DEBUG_2] == 1)) - // { - // mavlink_msg_debug_send(MAVLINK_COMM_1, pid->plot_i, pid->integral); - // } } if (pid->mode == PID_MODE_DERIVATIV_CALC) { @@ -142,7 +136,33 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = 0; } - pid->error_previous = error; + if (pid->kd == 0) { + d = 0; + } + + if (pid->ki == 0) { + i = 0; + } + + if (pid->kp == 0) { + p = 0; + } else { + p = error; + } + + if (isfinite(error)) { + pid->error_previous = error; + } + + float val = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); + + if (isfinite(val)) { + last_output = val; + } + + if (!isfinite(pid->integral)) { + pid->integral = 0; + } - return (error * pid->kp) + (i * pid->ki) + (d * pid->kd); + return pid->last_output; } diff --git a/apps/systemlib/pid/pid.h b/apps/systemlib/pid/pid.h index 83bf09b59..d4bbcaf31 100644 --- a/apps/systemlib/pid/pid.h +++ b/apps/systemlib/pid/pid.h @@ -58,13 +58,14 @@ typedef struct { float sp; float integral; float error_previous; + float last_output; + float limit; uint8_t mode; - uint8_t plot_i; uint8_t count; uint8_t saturated; } PID_t; -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode, uint8_t plot_i); +__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode); __EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax); //void pid_set(PID_t *pid, float sp); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); |