diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.c b/apps/fixedwing_att_control/fixedwing_att_control_att.c index 711a1cc313..b6196702f3 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_att.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_att.c @@ -64,41 +64,48 @@ struct fw_att_control_params { float roll_p; - float roll_lim; + float rollrate_lim; float pitch_p; float pitch_lim; + float pitchrate_lim; + float yawrate_lim; }; -struct fw_att_control_params_handles { +struct fw_pos_control_params_handle { float roll_p; - float roll_lim; + float rollrate_lim; float pitch_p; float pitch_lim; + float pitchrate_lim; + float yawrate_lim; }; /* Internal Prototypes */ -static int parameters_init(struct fw_att_control_params_handles *h); -static int parameters_update(const struct fw_att_control_params_handles *h, struct fw_att_control_params *p); +static int parameters_init(struct fw_pos_control_params_handle *h); +static int parameters_update(const struct fw_pos_control_params_handle *h, struct fw_att_control_params *p); -static int parameters_init(struct fw_att_control_params_handles *h) +static int parameters_init(struct fw_pos_control_params_handle *h) { /* PID parameters */ h->roll_p = param_find("FW_ROLL_P"); - h->roll_lim = param_find("FW_ROLL_LIM"); + h->rollrate_lim = param_find("FW_ROLLR_LIM"); h->pitch_p = param_find("FW_PITCH_P"); h->pitch_lim = param_find("FW_PITCH_LIM"); + h->pitchrate_lim = param_find("FW_PITCHR_LIM"); + h->yawrate_lim = param_find("FW_YAWR_LIM"); return OK; } -static int parameters_update(const struct fw_att_control_params_handles *h, struct fw_att_control_params *p) +static int parameters_update(const struct fw_pos_control_params_handle *h, struct fw_att_control_params *p) { param_get(h->roll_p, &(p->roll_p)); - param_get(h->roll_lim, &(p->roll_lim)); + param_get(h->rollrate_lim, &(p->rollrate_lim)); param_get(h->pitch_p, &(p->pitch_p)); - param_get(h->pitch_lim, &(p->pitch_lim)); + param_get(h->pitchrate_lim, &(p->pitchrate_lim)); + param_get(h->yawrate_lim, &(p->yawrate_lim)); return OK; } @@ -111,17 +118,18 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att static bool initialized = false; static struct fw_att_control_params p; - static struct fw_att_control_params_handles h; + static struct fw_pos_control_params_handle h; static PID_t roll_controller; static PID_t pitch_controller; + if(!initialized) { parameters_init(&h); parameters_update(&h, &p); - pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.roll_lim, PID_MODE_DERIVATIV_NONE); //P Controller - pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitch_lim, PID_MODE_DERIVATIV_NONE); //P Controller + pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller + pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller initialized = true; } @@ -129,15 +137,16 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att if (counter % 100 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.roll_lim); - pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitch_lim); + pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.pitchrate_lim); + pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim); } /* Roll (P) */ rates_sp->roll = pid_calculate(&roll_controller,att_sp->roll_tait_bryan, att->roll, 0, 0); /* Pitch (P) */ - //rates_sp->pitch = pid_calculate(&pitch_controller,att_sp->pitch_tait_bryan, att->pitch, 0, 0); + + rates_sp->pitch = pid_calculate(&pitch_controller,att_sp->pitch_tait_bryan, att->pitch, 0, 0); /* Yaw (from coordinated turn constraint or lateral force) */ //TODO diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index 06890ed8b5..82ef0403be 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -1,7 +1,8 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Ivan Ovinnikov + * Author: @author Thomas Gubler + * @author Doug Weibel * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -57,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -70,26 +72,24 @@ * */ // Roll control parameters -PARAM_DEFINE_FLOAT(FW_ROLLRATE_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_ROLLRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_ROLLRATE_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_ROLLRATE_LIM, 0.7f); // Roll rate limit in radians/sec +PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.0f); +PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller PARAM_DEFINE_FLOAT(FW_ROLL_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians //Pitch control parameters -PARAM_DEFINE_FLOAT(FW_PITCHRATE_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_PITCHRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_PITCHRATE_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_PITCHRATE_LIM, 0.35f); // Pitch rate limit in radians/sec +PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.0f); +PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller PARAM_DEFINE_FLOAT(FW_PITCH_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians //Yaw control parameters //XXX TODO this is copy paste, asign correct values -PARAM_DEFINE_FLOAT(FW_YAWRATE_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_YAWRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_YAWRATE_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_YAWRATE_LIM, 0.35f); // Yaw rate limit in radians/sec +PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f); +PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec /* Prototypes */ /** @@ -134,16 +134,20 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) memset(&att_sp, 0, sizeof(att_sp)); struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); +// static struct debug_key_value_s debug_output; +// memset(&debug_output, 0, sizeof(debug_output)); /* output structs */ struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); + /* publish actuator controls */ 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); - +// orb_advert_t debug_pub = orb_advertise(ORB_ID(debug_key_value), &debug_output); +// debug_output.key[0] = '1'; /* subscribe */ @@ -162,6 +166,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) /*Get Local Copies */ /* get a local copy of attitude */ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); gyro[0] = att.rollspeed; gyro[1] = att.pitchspeed; @@ -170,14 +175,9 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) /* Control */ /* Attitude Control */ - att_sp.roll_tait_bryan = 0.0f; //REMOVEME TODO - att_sp.pitch_tait_bryan = 0.0f; - att_sp.yaw_tait_bryan = 0.0f; fixedwing_att_control_attitude(&att_sp, &att, &rates_sp); - rates_sp.pitch = 0.0f; //REMOVEME TODO - rates_sp.yaw = 0.0f; /* Attitude Rate Control */ fixedwing_att_control_rates(&rates_sp, gyro, &actuators); @@ -186,6 +186,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) actuators.control[3] = 0.7f; orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); +// debug_output.value = rates_sp.pitch; +// orb_publish(ORB_ID(debug_key_value), debug_pub, &debug_output); } printf("[fixedwing_att_control] exiting, stopping all motors.\n"); @@ -198,6 +200,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + close(att_sub); close(actuator_pub); diff --git a/apps/fixedwing_att_control/fixedwing_att_control_rate.c b/apps/fixedwing_att_control/fixedwing_att_control_rate.c index 4b7e2c2bae..ec3039dd60 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_rate.c @@ -67,15 +67,12 @@ struct fw_rate_control_params { float rollrate_p; float rollrate_i; float rollrate_awu; - float rollrate_lim; float pitchrate_p; float pitchrate_i; float pitchrate_awu; - float pitchrate_lim; float yawrate_p; float yawrate_i; float yawrate_awu; - float yawrate_lim; }; @@ -83,15 +80,13 @@ struct fw_rate_control_param_handles { float rollrate_p; float rollrate_i; float rollrate_awu; - float rollrate_lim; float pitchrate_p; float pitchrate_i; float pitchrate_awu; - float pitchrate_lim; float yawrate_p; float yawrate_i; float yawrate_awu; - float yawrate_lim; + }; @@ -103,18 +98,17 @@ static int parameters_update(const struct fw_rate_control_param_handles *h, stru static int parameters_init(struct fw_rate_control_param_handles *h) { /* PID parameters */ - h->rollrate_p = param_find("FW_ROLLRATE_P"); //TODO define rate params for fixed wing - h->rollrate_i = param_find("FW_ROLLRATE_I"); - h->rollrate_awu = param_find("FW_ROLLRATE_AWU"); - h->rollrate_lim = param_find("FW_ROLLRATE_LIM"); - h->pitchrate_p = param_find("FW_PITCHRATE_P"); - h->pitchrate_i = param_find("FW_PITCHRATE_I"); - h->pitchrate_awu = param_find("FW_PITCHRATE_AWU"); - h->pitchrate_lim = param_find("FW_PITCHRATE_LIM"); - h->yawrate_p = param_find("FW_YAWRATE_P"); - h->yawrate_i = param_find("FW_YAWRATE_I"); - h->yawrate_awu = param_find("FW_YAWRATE_AWU"); - h->yawrate_lim = param_find("FW_YAWRATE_LIM"); + h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing + h->rollrate_i = param_find("FW_ROLLR_I"); + h->rollrate_awu = param_find("FW_ROLLR_AWU"); + + h->pitchrate_p = param_find("FW_PITCHR_P"); + h->pitchrate_i = param_find("FW_PITCHR_I"); + h->pitchrate_awu = param_find("FW_PITCHR_AWU"); + + h->yawrate_p = param_find("FW_YAWR_P"); + h->yawrate_i = param_find("FW_YAWR_I"); + h->yawrate_awu = param_find("FW_YAWR_AWU"); return OK; } @@ -124,15 +118,13 @@ static int parameters_update(const struct fw_rate_control_param_handles *h, stru param_get(h->rollrate_p, &(p->rollrate_p)); param_get(h->rollrate_i, &(p->rollrate_i)); param_get(h->rollrate_awu, &(p->rollrate_awu)); - param_get(h->rollrate_lim, &(p->rollrate_lim)); param_get(h->pitchrate_p, &(p->pitchrate_p)); param_get(h->pitchrate_i, &(p->pitchrate_i)); param_get(h->pitchrate_awu, &(p->pitchrate_awu)); - param_get(h->pitchrate_lim, &(p->pitchrate_lim)); param_get(h->yawrate_p, &(p->yawrate_p)); param_get(h->yawrate_i, &(p->yawrate_i)); param_get(h->yawrate_awu, &(p->yawrate_awu)); - param_get(h->yawrate_lim, &(p->yawrate_lim)); + return OK; } @@ -159,9 +151,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, { parameters_init(&h); parameters_update(&h, &p); - pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller - pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher - pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, p.yawrate_lim, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher + pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller + pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher + pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher initialized = true; } @@ -169,9 +161,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (counter % 100 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, p.rollrate_lim); - pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, p.pitchrate_lim); - pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, p.yawrate_lim); + pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1); + pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1); + pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1); } @@ -179,7 +171,7 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT); //XXX TODO disabled for now - actuators->control[1] = 0;//pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT); + actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT); actuators->control[2] = 0;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT); counter++; diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c index c08017b8de..9efe9cb340 100644 --- a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -1,7 +1,8 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Ivan Ovinnikov + * Author: @author Thomas Gubler + * @author Doug Weibel * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -60,7 +61,29 @@ #include #include +/* + * Controller parameters, accessible via MAVLink + * + */ +PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians +PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians + +struct fw_pos_control_params { + float roll_lim; + float pitch_lim; +}; + +struct fw_pos_control_param_handles { + float roll_lim; + float pitch_lim; +}; + + /* Prototypes */ +/* Internal Prototypes */ +static int parameters_init(struct fw_pos_control_param_handles *h); +static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p); + /** * Deamon management function. */ @@ -81,6 +104,29 @@ static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ + +/** + * Parameter management + */ +static int parameters_init(struct fw_pos_control_param_handles *h) +{ + /* PID parameters */ + h->roll_lim = param_find("FW_ROLL_LIM"); + h->pitch_lim = param_find("FW_PITCH_LIM"); + + + return OK; +} + +static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p) +{ + param_get(h->roll_lim, &(p->roll_lim)); + param_get(h->pitch_lim, &(p->pitch_lim)); + + return OK; +} + + /* Main Thread */ int fixedwing_pos_control_thread_main(int argc, char *argv[]) { @@ -106,7 +152,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) /* publish attitude setpoint */ attitude_setpoint.roll_tait_bryan = 0.0f; - attitude_setpoint.pitch_tait_bryan = 0.0f; + attitude_setpoint.pitch_tait_bryan = 0.2f; //TODO: for testing attitude_setpoint.yaw_tait_bryan = 0.0f; orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint); @@ -124,10 +170,34 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) // poll(&fds, 1, 500); sleep(500); //TODO removeme, this is for testing only - /* Control */ + + static int counter = 0; + static bool initialized = false; + + static struct fw_pos_control_params p; + static struct fw_pos_control_param_handles h; + + if(!initialized) + { + parameters_init(&h); + parameters_update(&h, &p); + initialized = true; + } + + /* load new parameters with lower rate */ + if (counter % 100 == 0) { + /* update parameters from storage */ + parameters_update(&h, &p); + + } + + /* Control */ + //TODO: control here orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint); + + counter++; } printf("[fixedwing_pos_control] exiting.\n");