dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fw control: moved and renamed parameters, attitude: roll and pitch working

This commit is contained in:
Thomas Gubler 2012-10-28 15:26:49 +01:00
parent 62581fe55b
commit e5f56a1a8f
4 changed files with 142 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
apps/fixedwing_att_control/fixedwing_att_control_att.c
View File

@ -64,41 +64,48 @@
struct fw_att_control_params { struct fw_att_control_params {
float roll_p; float roll_p;
float roll_lim; float rollrate_lim;
float pitch_p; float pitch_p;
float pitch_lim; float pitch_lim;
float pitchrate_lim;
float yawrate_lim;
}; };
struct fw_att_control_params_handles { struct fw_pos_control_params_handle {
float roll_p; float roll_p;
float roll_lim; float rollrate_lim;
float pitch_p; float pitch_p;
float pitch_lim; float pitch_lim;
float pitchrate_lim;
float yawrate_lim;
}; };
/* Internal Prototypes */ /* Internal Prototypes */
static int parameters_init(struct fw_att_control_params_handles *h); static int parameters_init(struct fw_pos_control_params_handle *h);
static int parameters_update(const struct fw_att_control_params_handles *h, struct fw_att_control_params *p); static int parameters_update(const struct fw_pos_control_params_handle *h, struct fw_att_control_params *p);
static int parameters_init(struct fw_att_control_params_handles *h) static int parameters_init(struct fw_pos_control_params_handle *h)
{ {
/* PID parameters */ /* PID parameters */
h->roll_p = param_find("FW_ROLL_P"); h->roll_p = param_find("FW_ROLL_P");
h->roll_lim = param_find("FW_ROLL_LIM"); h->rollrate_lim = param_find("FW_ROLLR_LIM");
h->pitch_p = param_find("FW_PITCH_P"); h->pitch_p = param_find("FW_PITCH_P");
h->pitch_lim = param_find("FW_PITCH_LIM"); h->pitch_lim = param_find("FW_PITCH_LIM");
h->pitchrate_lim = param_find("FW_PITCHR_LIM");
h->yawrate_lim = param_find("FW_YAWR_LIM");
return OK; return OK;
} }
static int parameters_update(const struct fw_att_control_params_handles *h, struct fw_att_control_params *p) static int parameters_update(const struct fw_pos_control_params_handle *h, struct fw_att_control_params *p)
{ {
param_get(h->roll_p, &(p->roll_p)); param_get(h->roll_p, &(p->roll_p));
param_get(h->roll_lim, &(p->roll_lim)); param_get(h->rollrate_lim, &(p->rollrate_lim));
param_get(h->pitch_p, &(p->pitch_p)); param_get(h->pitch_p, &(p->pitch_p));
param_get(h->pitch_lim, &(p->pitch_lim)); param_get(h->pitchrate_lim, &(p->pitchrate_lim));
param_get(h->yawrate_lim, &(p->yawrate_lim));
return OK; return OK;
} }
@ -111,17 +118,18 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
static bool initialized = false; static bool initialized = false;
static struct fw_att_control_params p; static struct fw_att_control_params p;
static struct fw_att_control_params_handles h; static struct fw_pos_control_params_handle h;
static PID_t roll_controller; static PID_t roll_controller;
static PID_t pitch_controller; static PID_t pitch_controller;
if(!initialized) if(!initialized)
{ {
parameters_init(&h); parameters_init(&h);
parameters_update(&h, &p); parameters_update(&h, &p);
pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.roll_lim, PID_MODE_DERIVATIV_NONE); //P Controller pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitch_lim, PID_MODE_DERIVATIV_NONE); //P Controller pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
initialized = true; initialized = true;
} }
@ -129,15 +137,16 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
if (counter % 100 == 0) { if (counter % 100 == 0) {
/* update parameters from storage */ /* update parameters from storage */
parameters_update(&h, &p); parameters_update(&h, &p);
pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.roll_lim); pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.pitchrate_lim);
pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitch_lim); pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim);
} }
/* Roll (P) */ /* Roll (P) */
rates_sp->roll = pid_calculate(&roll_controller,att_sp->roll_tait_bryan, att->roll, 0, 0); rates_sp->roll = pid_calculate(&roll_controller,att_sp->roll_tait_bryan, att->roll, 0, 0);
/* Pitch (P) */ /* Pitch (P) */
//rates_sp->pitch = pid_calculate(&pitch_controller,att_sp->pitch_tait_bryan, att->pitch, 0, 0);
rates_sp->pitch = pid_calculate(&pitch_controller,att_sp->pitch_tait_bryan, att->pitch, 0, 0);
/* Yaw (from coordinated turn constraint or lateral force) */ /* Yaw (from coordinated turn constraint or lateral force) */
//TODO //TODO

45
apps/fixedwing_att_control/fixedwing_att_control_main.c
View File

@ -1,7 +1,8 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2012 PX4 Development Team. All rights reserved. * Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Ivan Ovinnikov <oivan@ethz.ch> * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Doug Weibel <douglas.weibel@colorado.edu>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -57,6 +58,7 @@
#include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/debug_key_value.h>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
#include <systemlib/pid/pid.h> #include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h> #include <systemlib/geo/geo.h>
@ -70,26 +72,24 @@
* *
*/ */
// Roll control parameters // Roll control parameters
PARAM_DEFINE_FLOAT(FW_ROLLRATE_P, 0.3f); PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.3f);
PARAM_DEFINE_FLOAT(FW_ROLLRATE_I, 0.0f); PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_ROLLRATE_AWU, 0.0f); PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.0f);
PARAM_DEFINE_FLOAT(FW_ROLLRATE_LIM, 0.7f); // Roll rate limit in radians/sec PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller
PARAM_DEFINE_FLOAT(FW_ROLL_P, 0.3f); PARAM_DEFINE_FLOAT(FW_ROLL_P, 0.3f);
PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians
//Pitch control parameters //Pitch control parameters
PARAM_DEFINE_FLOAT(FW_PITCHRATE_P, 0.3f); PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.3f);
PARAM_DEFINE_FLOAT(FW_PITCHRATE_I, 0.0f); PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_PITCHRATE_AWU, 0.0f); PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.0f);
PARAM_DEFINE_FLOAT(FW_PITCHRATE_LIM, 0.35f); // Pitch rate limit in radians/sec PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller
PARAM_DEFINE_FLOAT(FW_PITCH_P, 0.3f); PARAM_DEFINE_FLOAT(FW_PITCH_P, 0.3f);
PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians
//Yaw control parameters //XXX TODO this is copy paste, asign correct values //Yaw control parameters //XXX TODO this is copy paste, asign correct values
PARAM_DEFINE_FLOAT(FW_YAWRATE_P, 0.3f); PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f);
PARAM_DEFINE_FLOAT(FW_YAWRATE_I, 0.0f); PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_YAWRATE_AWU, 0.0f); PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f);
PARAM_DEFINE_FLOAT(FW_YAWRATE_LIM, 0.35f); // Yaw rate limit in radians/sec PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec
/* Prototypes */ /* Prototypes */
/** /**
@ -134,16 +134,20 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
memset(&att_sp, 0, sizeof(att_sp)); memset(&att_sp, 0, sizeof(att_sp));
struct vehicle_rates_setpoint_s rates_sp; struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp)); memset(&rates_sp, 0, sizeof(rates_sp));
// static struct debug_key_value_s debug_output;
// memset(&debug_output, 0, sizeof(debug_output));
/* output structs */ /* output structs */
struct actuator_controls_s actuators; struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators)); memset(&actuators, 0, sizeof(actuators));
/* publish actuator controls */ /* publish actuator controls */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
actuators.control[i] = 0.0f; actuators.control[i] = 0.0f;
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
// orb_advert_t debug_pub = orb_advertise(ORB_ID(debug_key_value), &debug_output);
// debug_output.key[0] = '1';
/* subscribe */ /* subscribe */
@ -162,6 +166,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
/*Get Local Copies */ /*Get Local Copies */
/* get a local copy of attitude */ /* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
gyro[0] = att.rollspeed; gyro[0] = att.rollspeed;
gyro[1] = att.pitchspeed; gyro[1] = att.pitchspeed;
@ -170,14 +175,9 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
/* Control */ /* Control */
/* Attitude Control */ /* Attitude Control */
att_sp.roll_tait_bryan = 0.0f; //REMOVEME TODO
att_sp.pitch_tait_bryan = 0.0f;
att_sp.yaw_tait_bryan = 0.0f;
fixedwing_att_control_attitude(&att_sp, fixedwing_att_control_attitude(&att_sp,
&att, &att,
&rates_sp); &rates_sp);
rates_sp.pitch = 0.0f; //REMOVEME TODO
rates_sp.yaw = 0.0f;
/* Attitude Rate Control */ /* Attitude Rate Control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators); fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
@ -186,6 +186,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
actuators.control[3] = 0.7f; actuators.control[3] = 0.7f;
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
// debug_output.value = rates_sp.pitch;
// orb_publish(ORB_ID(debug_key_value), debug_pub, &debug_output);
} }
printf("[fixedwing_att_control] exiting, stopping all motors.\n"); printf("[fixedwing_att_control] exiting, stopping all motors.\n");
@ -198,6 +200,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
close(att_sub); close(att_sub);
close(actuator_pub); close(actuator_pub);

48
apps/fixedwing_att_control/fixedwing_att_control_rate.c
View File

@ -67,15 +67,12 @@ struct fw_rate_control_params {
float rollrate_p; float rollrate_p;
float rollrate_i; float rollrate_i;
float rollrate_awu; float rollrate_awu;
float rollrate_lim;
float pitchrate_p; float pitchrate_p;
float pitchrate_i; float pitchrate_i;
float pitchrate_awu; float pitchrate_awu;
float pitchrate_lim;
float yawrate_p; float yawrate_p;
float yawrate_i; float yawrate_i;
float yawrate_awu; float yawrate_awu;
float yawrate_lim;
}; };
@ -83,15 +80,13 @@ struct fw_rate_control_param_handles {
float rollrate_p; float rollrate_p;
float rollrate_i; float rollrate_i;
float rollrate_awu; float rollrate_awu;
float rollrate_lim;
float pitchrate_p; float pitchrate_p;
float pitchrate_i; float pitchrate_i;
float pitchrate_awu; float pitchrate_awu;
float pitchrate_lim;
float yawrate_p; float yawrate_p;
float yawrate_i; float yawrate_i;
float yawrate_awu; float yawrate_awu;
float yawrate_lim;
}; };
@ -103,18 +98,17 @@ static int parameters_update(const struct fw_rate_control_param_handles *h, stru
static int parameters_init(struct fw_rate_control_param_handles *h) static int parameters_init(struct fw_rate_control_param_handles *h)
{ {
/* PID parameters */ /* PID parameters */
h->rollrate_p = param_find("FW_ROLLRATE_P"); //TODO define rate params for fixed wing h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing
h->rollrate_i = param_find("FW_ROLLRATE_I"); h->rollrate_i = param_find("FW_ROLLR_I");
h->rollrate_awu = param_find("FW_ROLLRATE_AWU"); h->rollrate_awu = param_find("FW_ROLLR_AWU");
h->rollrate_lim = param_find("FW_ROLLRATE_LIM");
h->pitchrate_p = param_find("FW_PITCHRATE_P"); h->pitchrate_p = param_find("FW_PITCHR_P");
h->pitchrate_i = param_find("FW_PITCHRATE_I"); h->pitchrate_i = param_find("FW_PITCHR_I");
h->pitchrate_awu = param_find("FW_PITCHRATE_AWU"); h->pitchrate_awu = param_find("FW_PITCHR_AWU");
h->pitchrate_lim = param_find("FW_PITCHRATE_LIM");
h->yawrate_p = param_find("FW_YAWRATE_P"); h->yawrate_p = param_find("FW_YAWR_P");
h->yawrate_i = param_find("FW_YAWRATE_I"); h->yawrate_i = param_find("FW_YAWR_I");
h->yawrate_awu = param_find("FW_YAWRATE_AWU"); h->yawrate_awu = param_find("FW_YAWR_AWU");
h->yawrate_lim = param_find("FW_YAWRATE_LIM");
return OK; return OK;
} }
@ -124,15 +118,13 @@ static int parameters_update(const struct fw_rate_control_param_handles *h, stru
param_get(h->rollrate_p, &(p->rollrate_p)); param_get(h->rollrate_p, &(p->rollrate_p));
param_get(h->rollrate_i, &(p->rollrate_i)); param_get(h->rollrate_i, &(p->rollrate_i));
param_get(h->rollrate_awu, &(p->rollrate_awu)); param_get(h->rollrate_awu, &(p->rollrate_awu));
param_get(h->rollrate_lim, &(p->rollrate_lim));
param_get(h->pitchrate_p, &(p->pitchrate_p)); param_get(h->pitchrate_p, &(p->pitchrate_p));
param_get(h->pitchrate_i, &(p->pitchrate_i)); param_get(h->pitchrate_i, &(p->pitchrate_i));
param_get(h->pitchrate_awu, &(p->pitchrate_awu)); param_get(h->pitchrate_awu, &(p->pitchrate_awu));
param_get(h->pitchrate_lim, &(p->pitchrate_lim));
param_get(h->yawrate_p, &(p->yawrate_p)); param_get(h->yawrate_p, &(p->yawrate_p));
param_get(h->yawrate_i, &(p->yawrate_i)); param_get(h->yawrate_i, &(p->yawrate_i));
param_get(h->yawrate_awu, &(p->yawrate_awu)); param_get(h->yawrate_awu, &(p->yawrate_awu));
param_get(h->yawrate_lim, &(p->yawrate_lim));
return OK; return OK;
} }
@ -159,9 +151,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
{ {
parameters_init(&h); parameters_init(&h);
parameters_update(&h, &p); 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_NONE); // set D part to 0 because the controller layout is with a PI rate controller pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE); // 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_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE); // 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_NONE); // 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, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
initialized = true; initialized = true;
} }
@ -169,9 +161,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
if (counter % 100 == 0) { if (counter % 100 == 0) {
/* update parameters from storage */ /* update parameters from storage */
parameters_update(&h, &p); parameters_update(&h, &p);
pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, p.rollrate_lim); pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1);
pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, p.pitchrate_lim); pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1);
pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, p.yawrate_lim); pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1);
} }
@ -179,7 +171,7 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT); actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT);
//XXX TODO disabled for now //XXX TODO disabled for now
actuators->control[1] = 0;//pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT); actuators->control[1] = 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); actuators->control[2] = 0;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT);
counter++; counter++;

76
apps/fixedwing_pos_control/fixedwing_pos_control_main.c
View File

@ -1,7 +1,8 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2012 PX4 Development Team. All rights reserved. * Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Ivan Ovinnikov <oivan@ethz.ch> * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Doug Weibel <douglas.weibel@colorado.edu>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -60,7 +61,29 @@
#include <systemlib/geo/geo.h> #include <systemlib/geo/geo.h>
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
/*
* Controller parameters, accessible via MAVLink
*
*/
PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians
PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians
struct fw_pos_control_params {
float roll_lim;
float pitch_lim;
};
struct fw_pos_control_param_handles {
float roll_lim;
float pitch_lim;
};
/* Prototypes */ /* Prototypes */
/* Internal Prototypes */
static int parameters_init(struct fw_pos_control_param_handles *h);
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p);
/** /**
* Deamon management function. * Deamon management function.
*/ */
@ -81,6 +104,29 @@ static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */ static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */ static int deamon_task; /**< Handle of deamon task / thread */
/**
* Parameter management
*/
static int parameters_init(struct fw_pos_control_param_handles *h)
{
/* PID parameters */
h->roll_lim = param_find("FW_ROLL_LIM");
h->pitch_lim = param_find("FW_PITCH_LIM");
return OK;
}
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p)
{
param_get(h->roll_lim, &(p->roll_lim));
param_get(h->pitch_lim, &(p->pitch_lim));
return OK;
}
/* Main Thread */ /* Main Thread */
int fixedwing_pos_control_thread_main(int argc, char *argv[]) int fixedwing_pos_control_thread_main(int argc, char *argv[])
{ {
@ -106,7 +152,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
/* publish attitude setpoint */ /* publish attitude setpoint */
attitude_setpoint.roll_tait_bryan = 0.0f; attitude_setpoint.roll_tait_bryan = 0.0f;
attitude_setpoint.pitch_tait_bryan = 0.0f; attitude_setpoint.pitch_tait_bryan = 0.2f; //TODO: for testing
attitude_setpoint.yaw_tait_bryan = 0.0f; attitude_setpoint.yaw_tait_bryan = 0.0f;
orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint); orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint);
@ -124,10 +170,34 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
// poll(&fds, 1, 500); // poll(&fds, 1, 500);
sleep(500); //TODO removeme, this is for testing only sleep(500); //TODO removeme, this is for testing only
/* Control */
static int counter = 0;
static bool initialized = false;
static struct fw_pos_control_params p;
static struct fw_pos_control_param_handles h;
if(!initialized)
{
parameters_init(&h);
parameters_update(&h, &p);
initialized = true;
}
/* load new parameters with lower rate */
if (counter % 100 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
}
/* Control */
//TODO: control here
orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint); orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint);
counter++;
} }
printf("[fixedwing_pos_control] exiting.\n"); printf("[fixedwing_pos_control] exiting.\n");