From 4514045fb61479c456bb2bbaea5d3fe116ca705f Mon Sep 17 00:00:00 2001 From: "Hyon Lim (Retina)" Date: Wed, 25 Sep 2013 02:12:55 +0900 Subject: There were unintialized variables. (control mode was not updated) Also, new flags (xy_valid etc) were considered. --- .../flow_speed_control/flow_speed_control_main.c | 32 ++++++++++++++++++---- 1 file changed, 26 insertions(+), 6 deletions(-) (limited to 'src/examples/flow_speed_control') 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 6af955cd7..feed0d23c 100644 --- a/src/examples/flow_speed_control/flow_speed_control_main.c +++ b/src/examples/flow_speed_control/flow_speed_control_main.c @@ -65,6 +65,7 @@ #include #include #include +#include #include "flow_speed_control_params.h" @@ -151,17 +152,23 @@ flow_speed_control_thread_main(int argc, char *argv[]) { /* welcome user */ thread_running = true; - printf("[flow speed control] starting\n"); + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd,"[fsc] started"); uint32_t counter = 0; /* structures */ struct actuator_armed_s armed; + memset(&armed, 0, sizeof(armed)); struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); struct filtered_bottom_flow_s filtered_flow; + memset(&filtered_flow, 0, sizeof(filtered_flow)); struct vehicle_bodyframe_speed_setpoint_s speed_sp; - + memset(&speed_sp, 0, sizeof(speed_sp)); struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -186,6 +193,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_speed_control_err"); static bool sensors_ready = false; + static bool status_changed = false; while (!thread_should_exit) { @@ -221,7 +229,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); parameters_update(¶m_handles, ¶ms); - printf("[flow speed control] parameters updated.\n"); + mavlink_log_info(mavlink_fd,"[fsp] parameters updated."); } /* only run controller if position/speed changed */ @@ -237,6 +245,8 @@ flow_speed_control_thread_main(int argc, char *argv[]) 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); + /* get a local copy of control mode */ + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); if (control_mode.flag_control_velocity_enabled) { @@ -244,6 +254,11 @@ flow_speed_control_thread_main(int argc, char *argv[]) float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p; float roll_body = (speed_sp.vy - filtered_flow.vy) * params.speed_p; + if(status_changed == false) + mavlink_log_info(mavlink_fd,"[fsc] flow SPEED control engaged"); + + status_changed = true; + /* limit roll and pitch corrections */ if((pitch_body <= params.limit_pitch) && (pitch_body >= -params.limit_pitch)) { @@ -299,6 +314,11 @@ flow_speed_control_thread_main(int argc, char *argv[]) } else { + if(status_changed == true) + mavlink_log_info(mavlink_fd,"[fsc] flow SPEED controller disengaged."); + + status_changed = false; + /* reset attitude setpoint */ att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; @@ -334,20 +354,20 @@ flow_speed_control_thread_main(int argc, char *argv[]) else if (ret == 0) { /* no return value, ignore */ - printf("[flow speed control] no attitude received.\n"); + mavlink_log_info(mavlink_fd,"[fsc] no attitude received."); } else { if (fds[0].revents & POLLIN) { sensors_ready = true; - printf("[flow speed control] initialized.\n"); + mavlink_log_info(mavlink_fd,"[fsp] initialized."); } } } } - printf("[flow speed control] ending now...\n"); + mavlink_log_info(mavlink_fd,"[fsc] ending now..."); thread_running = false; -- cgit v1.2.3