diff options
Diffstat (limited to 'src')
3 files changed, 62 insertions, 13 deletions
diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c index 621d3253f..3125ce246 100644 --- a/src/examples/flow_position_control/flow_position_control_main.c +++ b/src/examples/flow_position_control/flow_position_control_main.c @@ -67,6 +67,7 @@ #include <systemlib/perf_counter.h> #include <systemlib/err.h> #include <poll.h> +#include <mavlink/mavlink_log.h> #include "flow_position_control_params.h" @@ -153,20 +154,28 @@ flow_position_control_thread_main(int argc, char *argv[]) { /* welcome user */ thread_running = true; - printf("[flow position control] starting\n"); + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd, "[fpc] started"); uint32_t counter = 0; const float time_scale = powf(10.0f,-6.0f); /* 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 vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); struct filtered_bottom_flow_s filtered_flow; + memset(&filtered_flow, 0, sizeof(filtered_flow)); struct vehicle_local_position_s local_pos; - + memset(&local_pos, 0, sizeof(local_pos)); struct vehicle_bodyframe_speed_setpoint_s speed_sp; + memset(&speed_sp, 0, sizeof(speed_sp)); /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -216,6 +225,7 @@ flow_position_control_thread_main(int argc, char *argv[]) perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_control_err"); static bool sensors_ready = false; + static bool status_changed = false; while (!thread_should_exit) { @@ -252,7 +262,7 @@ flow_position_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); parameters_update(¶m_handles, ¶ms); - printf("[flow position control] parameters updated.\n"); + mavlink_log_info(mavlink_fd,"[fpc] parameters updated."); } /* only run controller if position/speed changed */ @@ -270,6 +280,8 @@ flow_position_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 local position */ orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos); + /* 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) { @@ -277,6 +289,11 @@ flow_position_control_thread_main(int argc, char *argv[]) float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1 float manual_yaw = manual.yaw / params.rc_scale_yaw; // -1 to 1 + if(status_changed == false) + mavlink_log_info(mavlink_fd,"[fpc] flow POSITION control engaged"); + + status_changed = true; + /* calc dt */ if(last_time == 0) { @@ -296,7 +313,7 @@ flow_position_control_thread_main(int argc, char *argv[]) { flow_sp_sumy = filtered_flow.sumy; update_flow_sp_sumy = false; - } + } /* calc new bodyframe speed setpoints */ float speed_body_x = (flow_sp_sumx - filtered_flow.sumx) * params.pos_p - filtered_flow.vx * params.pos_d; @@ -518,6 +535,11 @@ flow_position_control_thread_main(int argc, char *argv[]) else { /* in manual or stabilized state just reset speed and flow sum setpoint */ + //mavlink_log_info(mavlink_fd,"[fpc] reset speed sp, flow_sp_sumx,y (%f,%f)",filtered_flow.sumx, filtered_flow.sumy); + if(status_changed == true) + mavlink_log_info(mavlink_fd,"[fpc] flow POSITION controller disengaged."); + + status_changed = false; speed_sp.vx = 0.0f; speed_sp.vy = 0.0f; flow_sp_sumx = filtered_flow.sumx; @@ -558,20 +580,20 @@ flow_position_control_thread_main(int argc, char *argv[]) else if (ret == 0) { /* no return value, ignore */ - printf("[flow position control] no attitude received.\n"); + mavlink_log_info(mavlink_fd,"[fpc] no attitude received.\n"); } else { if (fds[0].revents & POLLIN) { sensors_ready = true; - printf("[flow position control] initialized.\n"); + mavlink_log_info(mavlink_fd,"[fpc] initialized.\n"); } } } } - printf("[flow position control] ending now...\n"); + mavlink_log_info(mavlink_fd,"[fpc] ending now...\n"); thread_running = false; diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index 5e80066a7..495c415f2 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -345,6 +345,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) local_pos.y = local_pos.y + global_speed[1] * dt; local_pos.vx = global_speed[0]; local_pos.vy = global_speed[1]; + local_pos.xy_valid = true; + local_pos.v_xy_valid = true; } else { @@ -353,6 +355,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) filtered_flow.vy = 0; local_pos.vx = 0; local_pos.vy = 0; + local_pos.xy_valid = false; + local_pos.v_xy_valid = false; } /* filtering ground distance */ @@ -361,6 +365,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* not possible to fly */ sonar_valid = false; local_pos.z = 0.0f; + local_pos.z_valid = false; } else { @@ -388,6 +393,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) { local_pos.z = -sonar_new; } + + local_pos.z_valid = true; } filtered_flow.timestamp = hrt_absolute_time(); 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 <systemlib/perf_counter.h> #include <systemlib/err.h> #include <poll.h> +#include <mavlink/mavlink_log.h> #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; |