From db8d369c55d091b988a054ad39fd323e945a9bb0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Oct 2012 19:44:30 +0100 Subject: [PATCH] added a very simple altitude controller for testing --- .../fixedwing_pos_control_main.c | 51 ++++++++++++++----- 1 file changed, 38 insertions(+), 13 deletions(-) diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c index 5cab8eea78..eb4945464f 100644 --- a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -67,17 +67,21 @@ * */ PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f); +PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f); 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 heading_p; + float altitude_p; float roll_lim; float pitch_lim; }; struct fw_pos_control_param_handles { param_t heading_p; + param_t altitude_p; param_t roll_lim; param_t pitch_lim; @@ -117,6 +121,7 @@ static int parameters_init(struct fw_pos_control_param_handles *h) { /* PID parameters */ h->heading_p = param_find("FW_HEADING_P"); + h->altitude_p = param_find("FW_ALT_P"); h->roll_lim = param_find("FW_ROLL_LIM"); h->pitch_lim = param_find("FW_PITCH_LIM"); @@ -127,6 +132,7 @@ 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) { param_get(h->heading_p, &(p->heading_p)); + param_get(h->altitude_p, &(p->altitude_p)); param_get(h->roll_lim, &(p->roll_lim)); param_get(h->pitch_lim, &(p->pitch_lim)); @@ -174,6 +180,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) /* Setup of loop */ struct pollfd fds = { .fd = att_sub, .events = POLLIN }; + bool global_sp_updated_set_once = false; while(!thread_should_exit) { @@ -187,12 +194,14 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) static struct fw_pos_control_param_handles h; PID_t heading_controller; + PID_t altitude_controller; if(!initialized) { parameters_init(&h); parameters_update(&h, &p); pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f,p.roll_lim,PID_MODE_DERIVATIV_NONE); + pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f,p.pitch_lim,PID_MODE_DERIVATIV_NONE); initialized = true; } @@ -201,6 +210,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) /* update parameters from storage */ parameters_update(&h, &p); pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, p.roll_lim); + pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim); } @@ -209,6 +219,8 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) orb_check(global_pos_sub, &pos_updated); bool global_sp_updated; orb_check(global_setpoint_sub, &global_sp_updated); + if(global_sp_updated) + global_sp_updated_set_once = true; /* Load local copies */ @@ -220,25 +232,38 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) /* Control */ + /* Simple Horizontal Control */ - /* calculate bearing error */ - float target_bearing = get_bearing_to_next_waypoint(global_pos.lat / (double)1e7d, global_pos.lon / (double)1e7d, - global_setpoint.lat / (double)1e7d, global_setpoint.lon / (double)1e7d); + if(global_sp_updated_set_once) + { + /* calculate bearing error */ + float target_bearing = get_bearing_to_next_waypoint(global_pos.lat / (double)1e7d, global_pos.lon / (double)1e7d, + global_setpoint.lat / (double)1e7d, global_setpoint.lon / (double)1e7d); - /* shift error to prevent wrapping issues */ - float bearing_error = target_bearing - att.yaw; + /* shift error to prevent wrapping issues */ + float bearing_error = target_bearing - att.yaw; - if (bearing_error < M_PI_F) { - bearing_error += 2.0f * M_PI_F; + if (bearing_error < M_PI_F) { + bearing_error += 2.0f * M_PI_F; + } + + if (bearing_error > M_PI_F) { + bearing_error -= 2.0f * M_PI_F; + } + + /* calculate roll setpoint, do this artificially around zero */ + attitude_setpoint.roll_tait_bryan = pid_calculate(&heading_controller, bearing_error, 0.0f, 0.0f, 0.0f); } - if (bearing_error > M_PI_F) { - bearing_error -= 2.0f * M_PI_F; + /* Very simple Altitude Control */ + if(global_sp_updated_set_once && pos_updated) + { + + //TODO: take care of relatie vs. ab. altitude + attitude_setpoint.pitch_tait_bryan = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + } - - /* calculate roll setpoint, do this artificially around zero */ - attitude_setpoint.roll_tait_bryan = pid_calculate(&heading_controller, bearing_error, 0.0f, 0.0f, 0.0f); - + /*Publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint); counter++;