fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 07:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

manual merge of origin/master into fw_control

Browse Source
This commit is contained in:
Thomas Gubler
2012-11-21 21:35:13 +01:00
parent 1f798efd17 e24c349d1d
commit 6fb54ec62c
59 changed files with 2088 additions and 355 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/fixedwing_pos_control/fixedwing_pos_control.h
-19
View File
@@ -42,26 +42,7 @@
#ifndef FIXEDWING_POS_CONTROL_H_
#define FIXEDWING_POS_CONTROL_H_
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#endif
struct planned_path_segments_s {
bool segment_type;
double start_lat; // Start of line or center of arc
double start_lon;
double end_lat;
double end_lon;
float radius; // Radius of arc
float arc_start_bearing; // Bearing from center to start of arc
float arc_sweep; // Angle (radians) swept out by arc around center.
// Positive for clockwise, negative for counter-clockwise
};
float _wrap180(float bearing);
apps/fixedwing_pos_control/fixedwing_pos_control_main.c
+20 -69
View File
@@ -95,6 +95,19 @@ struct fw_pos_control_param_handles {
};
struct planned_path_segments_s {
bool segment_type;
double start_lat; // Start of line or center of arc
double start_lon;
double end_lat;
double end_lon;
float radius; // Radius of arc
float arc_start_bearing; // Bearing from center to start of arc
float arc_sweep; // Angle (radians) swept out by arc around center.
// Positive for clockwise, negative for counter-clockwise
};
/* Prototypes */
/* Internal Prototypes */
static int parameters_init(struct fw_pos_control_param_handles *h);
@@ -185,9 +198,9 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
memset(&attitude_setpoint, 0, sizeof(attitude_setpoint));
/* publish attitude setpoint */
attitude_setpoint.roll_tait_bryan = 0.0f;
attitude_setpoint.pitch_tait_bryan = 0.0f;
attitude_setpoint.yaw_tait_bryan = 0.0f;
attitude_setpoint.roll_body = 0.0f;
attitude_setpoint.pitch_body = 0.0f;
attitude_setpoint.yaw_body = 0.0f;
orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint);
/* subscribe */
@@ -261,7 +274,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
global_sp_updated_set_once = true;
psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
printf("psi_track: %0.4f\n", psi_track);
printf("psi_track: %0.4f\n", (double)psi_track);
}
/* Control */
@@ -287,7 +300,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
float psi_e = psi_c - att.yaw;
/* shift error to prevent wrapping issues */
psi_e = _wrapPI(psi_e);
psi_e = _wrap_pi(psi_e);
/* calculate roll setpoint, do this artificially around zero */
//TODO: psi rate loop incomplete
@@ -301,8 +314,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
float psi_rate_e_scaled = psi_rate_e * sqrtf(pow(global_pos.vx,2) + pow(global_pos.vy,2)) / 9.81f; //* V_gr / g
attitude_setpoint.roll_tait_bryan = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
// if (counter % 100 == 0)
// printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c);
@@ -319,7 +331,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
{
//TODO: take care of relative vs. ab. altitude
attitude_setpoint.pitch_tait_bryan = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
}
/*Publish the attitude setpoint */
@@ -401,64 +413,3 @@ int fixedwing_pos_control_main(int argc, char *argv[])
usage("unrecognized command");
exit(1);
}
float _wrapPI(float bearing)
{
while (bearing > M_PI_F) {
bearing = bearing - M_TWOPI_F;
}
while (bearing <= -M_PI_F) {
bearing = bearing + M_TWOPI_F;
}
return bearing;
}
float _wrap2PI(float bearing)
{
while (bearing >= M_TWOPI_F) {
bearing = bearing - M_TWOPI_F;
}
while (bearing < 0.0f) {
bearing = bearing + M_TWOPI_F;
}
return bearing;
}
float _wrap180(float bearing)
{
while (bearing > 180.0f) {
bearing = bearing - 360.0f;
}
while (bearing <= -180.0f) {
bearing = bearing + 360.0f;
}
return bearing;
}
float _wrap360(float bearing)
{
while (bearing >= 360.0f) {
bearing = bearing - 360.0f;
}
while (bearing < 0.0f) {
bearing = bearing + 360.0f;
}
return bearing;
}