fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 21:39:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Adapted flow estimator, position and velocity control to new state machine

Browse Source
This commit is contained in:
Julian Oes 2013-06-24 09:49:46 +02:00
parent a183f3e273
commit 53dec130c4
5 changed files with 50 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/examples/flow_position_control/flow_position_control_main.c
View File

@ -55,7 +55,8 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
@ -158,7 +159,8 @@ flow_position_control_thread_main(int argc, char *argv[])
const float time_scale = powf(10.0f,-6.0f);
/* structures */
struct vehicle_status_s vstatus;
struct actuator_safety_s safety;
struct vehicle_control_mode_s control_mode;
struct vehicle_attitude_s att;
struct manual_control_setpoint_s manual;
struct filtered_bottom_flow_s filtered_flow;
@ -169,7 +171,8 @@ flow_position_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 manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
@ -258,7 +261,7 @@ flow_position_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);
orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
/* get a local copy of manual setpoint */
orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
/* get a local copy of attitude */
@ -268,9 +271,7 @@ flow_position_control_thread_main(int argc, char *argv[])
/* get a local copy of local position */
orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos);
// XXX fix this
#if 0
if (vstatus.state_machine == SYSTEM_STATE_AUTO)
if (control_mode.flag_control_velocity_enabled)
{
float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1
float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1
@ -492,7 +493,7 @@ flow_position_control_thread_main(int argc, char *argv[])
/* store actual height for speed estimation */
last_local_pos_z = local_pos.z;
speed_sp.thrust_sp = thrust_control;
speed_sp.thrust_sp = thrust_control; //manual.throttle;
speed_sp.timestamp = hrt_absolute_time();
/* publish new speed setpoint */
@ -529,7 +530,6 @@ flow_position_control_thread_main(int argc, char *argv[])
if(isfinite(manual.throttle))
speed_sp.thrust_sp = manual.throttle;
}
#endif
/* measure in what intervals the controller runs */
perf_count(mc_interval_perf);
perf_end(mc_loop_perf);
@ -578,7 +578,8 @@ flow_position_control_thread_main(int argc, char *argv[])
close(parameter_update_sub);
close(vehicle_attitude_sub);
close(vehicle_local_position_sub);
close(vehicle_status_sub);
close(safety_sub);
close(control_mode_sub);
close(manual_control_setpoint_sub);
close(speed_sp_pub);

36
src/examples/flow_position_estimator/flow_position_estimator_main.c
View File

@ -56,7 +56,8 @@
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
@ -143,8 +144,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
printf("[flow position estimator] starting\n");
/* rotation matrix for transformation of optical flow speed vectors */
static const int8_t rotM_flow_sensor[3][3] = {{ 0, 1, 0 },
{ -1, 0, 0 },
static const int8_t rotM_flow_sensor[3][3] = {{ 0, -1, 0 },
{ 1, 0, 0 },
{ 0, 0, 1 }}; // 90deg rotated
const float time_scale = powf(10.0f,-6.0f);
static float speed[3] = {0.0f, 0.0f, 0.0f};
@ -158,8 +159,10 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
static float sonar_lp = 0.0f;
/* subscribe to vehicle status, attitude, sensors and flow*/
struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus));
struct actuator_safety_s safety;
memset(&safety, 0, sizeof(safety));
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 vehicle_attitude_setpoint_s att_sp;
@ -170,8 +173,11 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
/* subscribe to parameter changes */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
/* subscribe to vehicle status */
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* subscribe to safety topic */
int safety_sub = orb_subscribe(ORB_ID(actuator_safety));
/* subscribe to safety topic */
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
/* subscribe to attitude */
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
@ -218,8 +224,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
while (!thread_should_exit)
{
// XXX fix this
#if 0
if (sensors_ready)
{
/*This runs at the rate of the sensors */
@ -265,7 +270,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
/* got flow, updating attitude and status as well */
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp);
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
/* vehicle state estimation */
float sonar_new = flow.ground_distance_m;
@ -278,12 +284,12 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
if (!vehicle_liftoff)
{
if (vstatus.flag_system_armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f)
if (safety.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f)
vehicle_liftoff = true;
}
else
{
if (!vstatus.flag_system_armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f))
if (!safety.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f))
vehicle_liftoff = false;
}
@ -350,7 +356,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
}
/* filtering ground distance */
if (!vehicle_liftoff || !vstatus.flag_system_armed)
if (!vehicle_liftoff || !safety.armed)
{
/* not possible to fly */
sonar_valid = false;
@ -440,7 +446,6 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
}
counter++;
#endif
}
printf("[flow position estimator] exiting.\n");
@ -448,7 +453,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
close(vehicle_attitude_setpoint_sub);
close(vehicle_attitude_sub);
close(vehicle_status_sub);
close(safety_sub);
close(control_mode_sub);
close(parameter_update_sub);
close(optical_flow_sub);

22
src/examples/flow_speed_control/flow_speed_control_main.c
View File

@ -55,7 +55,8 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_bodyframe_speed_setpoint.h>
@ -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);

2
src/modules/multirotor_att_control/multirotor_att_control_main.c
View File

@ -331,7 +331,7 @@ mc_thread_main(int argc, char *argv[])
// */
//
/* only move setpoint if manual input is != 0 */
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.1f) {
rates_sp.yaw = manual.yaw;
control_yaw_position = false;
first_time_after_yaw_speed_control = true;

8
src/modules/multirotor_att_control/multirotor_attitude_control.c
View File

@ -156,8 +156,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET);
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET);
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f, PID_MODE_DERIVATIV_SET);
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f, PID_MODE_DERIVATIV_SET);
initialized = true;
}
@ -168,8 +168,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
parameters_update(&h, &p);
/* apply parameters */
pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f);
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f);
pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f);
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f);
}
/* reset integral if on ground */