diff options
Diffstat (limited to 'apps/fixedwing_pos_control')
| -rw-r--r-- | apps/fixedwing_pos_control/fixedwing_pos_control_main.c | 249 |
1 files changed, 140 insertions, 109 deletions
diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c index 640edba99..45736aa2f 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> /* @@ -170,16 +172,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; @@ -192,6 +194,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; @@ -201,143 +205,170 @@ 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 heading_rate_controller; + PID_t offtrack_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, 10000.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit + pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 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); + pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f*M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value + + /* 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; - - PID_t heading_controller; - PID_t heading_rate_controller; - PID_t offtrack_controller; - PID_t altitude_controller; - - static uint64_t last_run = 0; - const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); - - if(!initialized) - { - parameters_init(&h); - parameters_update(&h, &p); - pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit - pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 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); - pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f*M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value - initialized = true; - } + 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, 10000.0f); //arbitrary high limit + pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim); + pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim); + pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f*M_DEG_TO_RAD); //TODO: remove hardcoded value + } - /* 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, 10000.0f); //arbitrary high limit - pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim); - pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim); - pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f*M_DEG_TO_RAD); //TODO: remove hardcoded value + /* only run controller if attitude changed */ + if (fds[1].revents & POLLIN) { - } + /* 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); - /* 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); - } + /* load local copies */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - /* Control */ + static uint64_t last_run = 0; + const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); - /* 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 (pos_updated) { + orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + } - float delta_psi_c = -pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards + if (global_sp_updated) { + orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); - float psi_c = psi_track + delta_psi_c; + 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); + } - float psi_e = psi_c - att.yaw; + /* 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); - /* shift error to prevent wrapping issues */ - psi_e = _wrap_pi(psi_e); + /* 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); - /* calculate roll setpoint, do this artificially around zero */ - //TODO: psi rate loop incomplete - float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); - float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following - float psi_rate_c = delta_psi_rate_c + psi_rate_track; + if(!(distance_res != OK || xtrack_err.past_end)) { - //TODO limit turn rate + float delta_psi_c = -pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards - float psi_rate_e = psi_rate_c - att.yawspeed; + float psi_c = psi_track + delta_psi_c; - float psi_rate_e_scaled = psi_rate_e * sqrtf(pow(global_pos.vx,2) + pow(global_pos.vy,2)) / 9.81f; //* V_gr / g + float psi_e = psi_c - att.yaw; - attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT); + /* shift error to prevent wrapping issues */ + psi_e = _wrap_pi(psi_e); -// 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); - } + /* calculate roll setpoint, do this artificially around zero */ + //TODO: psi rate loop incomplete + float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); + float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following + float psi_rate_c = delta_psi_rate_c + psi_rate_track; - } + //TODO limit turn rate - /* Very simple Altitude Control */ - if(global_sp_updated_set_once && pos_updated) - { + float psi_rate_e = psi_rate_c - att.yawspeed; - //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); + float psi_rate_e_scaled = psi_rate_e * sqrtf(pow(global_pos.vx,2) + pow(global_pos.vy,2)) / 9.81f; //* V_gr / g - } - /*Publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint); + attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT); + + // 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); + } + + /* Very simple Altitude 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); - counter++; + } + + // XXX need speed control + attitude_setpoint.thrust = 0.7f; + + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint); + + /* measure in what intervals the controller runs */ + perf_count(fw_interval_perf); + + } else { + // XXX no setpoint, decent default needed (loiter?) + } + } + } } printf("[fixedwing_pos_control] exiting.\n"); @@ -389,7 +420,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; |