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

WIP on HIL

Browse Source
This commit is contained in:
Lorenz Meier
2012-09-22 20:55:44 +02:00
parent a9b21886f3
commit 8b951ec417
6 changed files with 78 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/fixedwing_control/fixedwing_control.c
+7 -2
View File
@@ -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;
}
apps/mavlink/mavlink.c
+24 -23
View File
@@ -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 --- */
apps/multirotor_att_control/multirotor_attitude_control.c
+3 -3
View File
@@ -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;
}
apps/multirotor_att_control/multirotor_rate_control.c
+3 -3
View File
@@ -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;
}
apps/systemlib/pid/pid.c
+38 -18
View File
@@ -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;
}
return (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
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 pid->last_output;
}
apps/systemlib/pid/pid.h
+3 -2
View File
@@ -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);