From 53dec130c49f69f10527ab55d69de46ee26c58f6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Jun 2013 09:49:46 +0200 Subject: Adapted flow estimator, position and velocity control to new state machine --- .../flow_speed_control/flow_speed_control_main.c | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) (limited to 'src/examples/flow_speed_control/flow_speed_control_main.c') diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c index 0be4b3d5a..4f60e34f2 100644 --- a/src/examples/flow_speed_control/flow_speed_control_main.c +++ b/src/examples/flow_speed_control/flow_speed_control_main.c @@ -55,7 +55,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -155,7 +156,8 @@ flow_speed_control_thread_main(int argc, char *argv[]) uint32_t counter = 0; /* structures */ - struct vehicle_status_s vstatus; + struct actuator_safety_s safety; + struct vehicle_control_mode_s control_mode; struct filtered_bottom_flow_s filtered_flow; struct vehicle_bodyframe_speed_setpoint_s speed_sp; @@ -164,7 +166,8 @@ flow_speed_control_thread_main(int argc, char *argv[]) /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint)); @@ -186,7 +189,6 @@ flow_speed_control_thread_main(int argc, char *argv[]) while (!thread_should_exit) { -#if 0 /* wait for first attitude msg to be sure all data are available */ if (sensors_ready) { @@ -227,14 +229,16 @@ flow_speed_control_thread_main(int argc, char *argv[]) { perf_begin(mc_loop_perf); - /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + /* get a local copy of the safety topic */ + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + /* get a local copy of the control mode */ + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); /* get a local copy of filtered bottom flow */ orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow); /* get a local copy of bodyframe speed setpoint */ orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp); - if (vstatus.state_machine == SYSTEM_STATE_AUTO) + if (control_mode.flag_control_velocity_enabled) { /* calc new roll/pitch */ float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p; @@ -341,7 +345,6 @@ flow_speed_control_thread_main(int argc, char *argv[]) } } } -#endif } printf("[flow speed control] ending now...\n"); @@ -352,7 +355,8 @@ flow_speed_control_thread_main(int argc, char *argv[]) close(vehicle_attitude_sub); close(vehicle_bodyframe_speed_setpoint_sub); close(filtered_bottom_flow_sub); - close(vehicle_status_sub); + close(safety_sub); + close(control_mode_sub); close(att_sp_pub); perf_print_counter(mc_loop_perf); -- cgit v1.2.3