diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-25 13:55:28 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-25 13:55:28 +0100 |
commit | faa672f8bb257ec0ee357ad55da3195b79aeb54b (patch) | |
tree | d20ffd7f403aab274652b016af49554128a472d1 /apps/fixedwing_pos_control/fixedwing_pos_control_main.c | |
parent | dc72d467d4abe3d18bbf02091eb4eaddb4f491d2 (diff) | |
download | px4-firmware-faa672f8bb257ec0ee357ad55da3195b79aeb54b.tar.gz px4-firmware-faa672f8bb257ec0ee357ad55da3195b79aeb54b.tar.bz2 px4-firmware-faa672f8bb257ec0ee357ad55da3195b79aeb54b.zip |
mode switching for all platforms, additional fixed wing modes
Diffstat (limited to 'apps/fixedwing_pos_control/fixedwing_pos_control_main.c')
-rw-r--r-- | apps/fixedwing_pos_control/fixedwing_pos_control_main.c | 214 |
1 files changed, 120 insertions, 94 deletions
diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c index a70b9bd30..d9acd8db9 100644 --- a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -57,9 +57,11 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/parameter_update.h> #include <systemlib/param/param.h> #include <systemlib/pid/pid.h> #include <systemlib/geo/geo.h> +#include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> /* @@ -162,16 +164,16 @@ static int parameters_update(const struct fw_pos_control_param_handles *h, struc int fixedwing_pos_control_thread_main(int argc, char *argv[]) { /* read arguments */ - bool verbose = false; + // bool verbose = false; - for (int i = 1; i < argc; i++) { - if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { - verbose = true; - } - } + // for (int i = 1; i < argc; i++) { + // if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { + // verbose = true; + // } + // } /* welcome user */ - printf("[fixedwing att_control] started\n"); + printf("[fixedwing pos control] started\n"); /* declare and safely initialize all structs */ struct vehicle_global_position_s global_pos; @@ -184,6 +186,8 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) memset(&att, 0, sizeof(att)); struct crosstrack_error_s xtrack_err; memset(&xtrack_err, 0, sizeof(xtrack_err)); + struct parameter_update_s param_update; + memset(¶m_update, 0, sizeof(param_update)); /* output structs */ struct vehicle_attitude_setpoint_s attitude_setpoint; @@ -193,129 +197,151 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) attitude_setpoint.roll_body = 0.0f; attitude_setpoint.pitch_body = 0.0f; attitude_setpoint.yaw_body = 0.0f; + attitude_setpoint.thrust = 0.0f; orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint); /* subscribe */ int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int param_sub = orb_subscribe(ORB_ID(parameter_update)); /* Setup of loop */ - struct pollfd fds = { .fd = att_sub, .events = POLLIN }; + struct pollfd fds[2] = { + { .fd = param_sub, .events = POLLIN }, + { .fd = att_sub, .events = POLLIN } + }; bool global_sp_updated_set_once = false; float psi_track = 0.0f; + int counter = 0; + + struct fw_pos_control_params p; + struct fw_pos_control_param_handles h; + + PID_t heading_controller; + PID_t altitude_controller; + + 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); + + /* error and performance monitoring */ + perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval"); + perf_counter_t fw_err_perf = perf_alloc(PC_COUNT, "fixedwing_pos_control_err"); + while(!thread_should_exit) { /* wait for a sensor update, check for exit condition every 500 ms */ - poll(&fds, 1, 500); - - static int counter = 0; - static bool initialized = false; - - static struct fw_pos_control_params p; - static struct fw_pos_control_param_handles h; + int ret = poll(fds, 2, 500); + + if (ret < 0) { + /* poll error, count it in perf */ + perf_count(fw_err_perf); + } else if (ret == 0) { + /* no return value, ignore */ + } else { + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), param_sub, &update); + + /* 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); + } - PID_t heading_controller; - PID_t altitude_controller; + /* only run controller if attitude changed */ + if (fds[1].revents & POLLIN) { - 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; - } + /* check if there is a new position or setpoint */ + bool pos_updated; + orb_check(global_pos_sub, &pos_updated); + bool global_sp_updated; + orb_check(global_setpoint_sub, &global_sp_updated); - /* load new parameters with lower rate */ - if (counter % 100 == 0) { - /* 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); + /* load local copies */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - } + if (pos_updated) { + orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + } - /* Check if there is a new position or setpoint */ - bool pos_updated; - orb_check(global_pos_sub, &pos_updated); - bool global_sp_updated; - orb_check(global_setpoint_sub, &global_sp_updated); - - /* Load local copies */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - if(pos_updated) - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); - if (global_sp_updated) - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); - - if(global_sp_updated) { - start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint) - global_sp_updated_set_once = true; - psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, - (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); - printf("psi_track: %0.4f\n", (double)psi_track); - } + if (global_sp_updated) { + orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); - /* Control */ + start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint) + global_sp_updated_set_once = true; + psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, + (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); + printf("psi_track: %0.4f\n", (double)psi_track); + } + /* simple horizontal control, execute if line between wps is known */ + if(global_sp_updated_set_once) + { + // if (counter % 100 == 0) + // printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon); + + /* calculate crosstrack error */ + // Only the case of a straight line track following handled so far + int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, + (double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d, + (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); - /* Simple Horizontal Control */ - if(global_sp_updated_set_once) - { -// if (counter % 100 == 0) -// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon); - - /* calculate crosstrack error */ - // Only the case of a straight line track following handled so far - int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, - (double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d, - (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); + if(!(distance_res != OK || xtrack_err.past_end)) { - if(!(distance_res != OK || xtrack_err.past_end)) { + float delta_psi_c = -p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards - float delta_psi_c = -p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards + if(delta_psi_c > 60.0f*M_DEG_TO_RAD_F) + delta_psi_c = 60.0f*M_DEG_TO_RAD_F; - if(delta_psi_c > 60.0f*M_DEG_TO_RAD_F) - delta_psi_c = 60.0f*M_DEG_TO_RAD_F; + if(delta_psi_c < -60.0f*M_DEG_TO_RAD_F) + delta_psi_c = -60.0f*M_DEG_TO_RAD_F; - if(delta_psi_c < -60.0f*M_DEG_TO_RAD_F) - delta_psi_c = -60.0f*M_DEG_TO_RAD_F; + float psi_c = psi_track + delta_psi_c; - float psi_c = psi_track + delta_psi_c; + float psi_e = psi_c - att.yaw; - float psi_e = psi_c - att.yaw; + /* shift error to prevent wrapping issues */ + psi_e = _wrap_pi(psi_e); - /* shift error to prevent wrapping issues */ - psi_e = _wrap_pi(psi_e); + /* calculate roll setpoint, do this artificially around zero */ + attitude_setpoint.roll_body = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); - /* calculate roll setpoint, do this artificially around zero */ - attitude_setpoint.roll_body = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); + // if (counter % 100 == 0) + // printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c); + } + else { + if (counter % 100 == 0) + printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end); + } -// if (counter % 100 == 0) -// printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c); - } - else { - if (counter % 100 == 0) - printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end); - } + // XXX SIMPLE ALTITUDE, BUT NO SPEED CONTROL + if (pos_updated) { + //TODO: take care of relative vs. ab. altitude + attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + } - } + // XXX need speed control + attitude_setpoint.thrust = 0.7f; - /* Very simple Altitude Control */ - if(global_sp_updated_set_once && pos_updated) - { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint); - //TODO: take care of relative vs. ab. altitude - attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + /* measure in what intervals the controller runs */ + perf_count(fw_interval_perf); + } else { + // XXX no setpoint, decent default needed (loiter?) + } + } } - /*Publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint); - - counter++; } printf("[fixedwing_pos_control] exiting.\n"); @@ -367,7 +393,7 @@ int fixedwing_pos_control_main(int argc, char *argv[]) deamon_task = task_spawn("fixedwing_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, - 4096, + 2048, fixedwing_pos_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; |