fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 20:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

There were unintialized variables.

Browse Source 
(control mode was not updated)

Also, new flags (xy_valid etc) were considered.
This commit is contained in:
Hyon Lim (Retina)
2013-09-25 02:12:55 +09:00
parent bfe22c1140
commit 4514045fb6
3 changed files with 62 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/examples/flow_speed_control/flow_speed_control_main.c
+26 -6
View File
@@ -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(&param_handles, &params);
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;