Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-05 14:17:20 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

[work in progess] some copy paste for pitch and yaw, but not enabled yet

Browse Source
This commit is contained in:
Thomas Gubler
2012-10-22 19:12:27 +02:00
parent 69185643c0
commit b9d03c7c27
4 changed files with 45 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/fixedwing_att_control/fixedwing_att_control_att.c
+35 -17
View File
@@ -35,7 +35,7 @@
* @file fixedwing_att_control_rate.c
* Implementation of a fixed wing attitude controller.
*/
#include <fixedwing_att_control_rate.h>
#include <fixedwing_att_control_att.h>
#include <nuttx/config.h>
#include <stdio.h>
@@ -63,30 +63,42 @@
struct fw_att_control_params {
param_t roll_p;
float roll_p;
float roll_lim;
float pitch_p;
float pitch_lim;
};
struct fw_att_control_params_handles {
float roll_p;
float roll_lim;
float pitch_p;
float pitch_lim;
};
/* Internal Prototypes */
static int parameters_init(struct fw_att_control_params *h);
static int parameters_update(const struct fw_att_control_params *h, struct fw_att_control_params *p);
static int parameters_init(struct fw_att_control_params_handles *h);
static int parameters_update(const struct fw_att_control_params_handles *h, struct fw_att_control_params *p);
static int parameters_init(struct fw_att_control_params *h)
static int parameters_init(struct fw_att_control_params_handles *h)
{
/* PID parameters */
h->roll_p = param_find("FW_ROLL_POS_P"); //TODO define rate params for fixed wing
// if(h->attrate_i == PARAM_INVALID)
// printf("FATAL MC_ATTRATE_I does not exist\n");
h->roll_p = param_find("FW_ROLL_P");
h->roll_lim = param_find("FW_ROLL_LIM");
h->pitch_p = param_find("FW_PITCH_P");
h->pitch_lim = param_find("FW_PITCH_LIM");
return OK;
}
static int parameters_update(const struct fw_att_control_params *h, struct fw_att_control_params *p)
static int parameters_update(const struct fw_att_control_params_handles *h, struct fw_att_control_params *p)
{
param_get(h->roll_p, &(p->roll_p));
param_get(h->roll_lim, &(p->roll_lim));
param_get(h->pitch_p, &(p->pitch_p));
param_get(h->pitch_lim, &(p->pitch_lim));
return OK;
}
@@ -99,11 +111,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
static bool initialized = false;
static struct fw_att_control_params p;
static struct fw_att_control_params h;
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
static struct fw_att_control_params_handles h;
if(!initialized)
{
@@ -120,7 +128,17 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
/* Roll (P) */
float roll_error = att_sp->roll_tait_bryan - att->roll;
rates_sp->roll = p.roll_p * roll_error;
//TODO convert to body frame
rates_sp->roll = p.roll_p * roll_error; //TODO enabled for testing only
/* Pitch (P) */
float pitch_error = att_sp->pitch_tait_bryan - att->pitch;
//TODO convert to body frame
/* Yaw (from coordinated turn constraint) */
//TODO
//TODO Limits
counter++;
apps/fixedwing_att_control/fixedwing_att_control_main.c
-2
View File
@@ -90,8 +90,6 @@ PARAM_DEFINE_FLOAT(FW_YAWRATE_P, 0.3f);
PARAM_DEFINE_FLOAT(FW_YAWRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_YAWRATE_AWU, 0.0f);
PARAM_DEFINE_FLOAT(FW_YAWRATE_LIM, 0.35f); // Yaw rate limit in radians/sec
PARAM_DEFINE_FLOAT(FW_YAW_P, 0.3f);
PARAM_DEFINE_FLOAT(FW_YAW_LIM, 0.35f); // Yaw angle limit in radians
/* Prototypes */
/**
apps/fixedwing_att_control/fixedwing_att_control_rate.c
+9 -3
View File
@@ -152,6 +152,8 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
static struct fw_rate_control_param_handles h;
static PID_t roll_rate_controller;
static PID_t pitch_rate_controller;
static PID_t yaw_rate_controller;
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@@ -162,6 +164,8 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, p.rollrate_lim, PID_MODE_DERIVATIV_SET); // set D part to 0 because the controller layout is with a PI rate controller
pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, p.pitchrate_lim, PID_MODE_DERIVATIV_SET); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, p.yawrate_lim, PID_MODE_DERIVATIV_SET); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
initialized = true;
}
@@ -169,15 +173,17 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
if (counter % 2500 == 0) {
/* update parameters from storage */
pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, p.rollrate_lim);
pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, p.pitchrate_lim);
pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, p.yawrate_lim);
parameters_update(&h, &p);
}
/* Roll Rate (PI) */
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT);
actuators->control[1] = 0;
actuators->control[2] = 0;
//XXX TODO disabled for now
actuators->control[1] = 0;//pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT);
actuators->control[2] = 0;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT);
counter++;
apps/fixedwing_pos_control/fixedwing_pos_control_main.c
+1 -1
View File
@@ -112,7 +112,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
// /* subscribe to attitude (for attitude rate) and rate septoint */
// /* subscribe */
//
/* Setup of loop */