forked from Archive/PX4-Autopilot
fw control: moved and renamed parameters, attitude: roll and pitch working
This commit is contained in:
Thomas Gubler
parent
62581fe55b
commit
e5f56a1a8f
|
|
@ -64,41 +64,48 @@
|
|||
|
||||
struct fw_att_control_params {
|
||||
float roll_p;
|
||||
float roll_lim;
|
||||
float rollrate_lim;
|
||||
float pitch_p;
|
||||
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_lim;
|
||||
float rollrate_lim;
|
||||
float pitch_p;
|
||||
float pitch_lim;
|
||||
float pitchrate_lim;
|
||||
float yawrate_lim;
|
||||
};
|
||||
|
||||
|
||||
|
||||
/* Internal Prototypes */
|
||||
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_pos_control_params_handle *h);
|
||||
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 */
|
||||
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_lim = param_find("FW_PITCH_LIM");
|
||||
h->pitchrate_lim = param_find("FW_PITCHR_LIM");
|
||||
h->yawrate_lim = param_find("FW_YAWR_LIM");
|
||||
|
||||
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_lim, &(p->roll_lim));
|
||||
param_get(h->rollrate_lim, &(p->rollrate_lim));
|
||||
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;
|
||||
}
|
||||
|
|
@ -111,17 +118,18 @@ 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_handles h;
|
||||
static struct fw_pos_control_params_handle h;
|
||||
|
||||
static PID_t roll_controller;
|
||||
static PID_t pitch_controller;
|
||||
|
||||
|
||||
if(!initialized)
|
||||
{
|
||||
parameters_init(&h);
|
||||
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(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitch_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.pitchrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
|
||||
initialized = true;
|
||||
}
|
||||
|
||||
|
|
@ -129,15 +137,16 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
|
|||
if (counter % 100 == 0) {
|
||||
/* update parameters from storage */
|
||||
parameters_update(&h, &p);
|
||||
pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.roll_lim);
|
||||
pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitch_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.pitchrate_lim);
|
||||
}
|
||||
|
||||
/* Roll (P) */
|
||||
rates_sp->roll = pid_calculate(&roll_controller,att_sp->roll_tait_bryan, att->roll, 0, 0);
|
||||
|
||||
/* 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) */
|
||||
//TODO
|
||||
|
|
|
|||
|
|
@ -1,7 +1,8 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -57,6 +58,7 @@
|
|||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/pid/pid.h>
|
||||
#include <systemlib/geo/geo.h>
|
||||
|
|
@ -70,26 +72,24 @@
|
|||
*
|
||||
*/
|
||||
// Roll control parameters
|
||||
PARAM_DEFINE_FLOAT(FW_ROLLRATE_P, 0.3f);
|
||||
PARAM_DEFINE_FLOAT(FW_ROLLRATE_I, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_ROLLRATE_AWU, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_ROLLRATE_LIM, 0.7f); // Roll rate limit in radians/sec
|
||||
PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.3f);
|
||||
PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.0f);
|
||||
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_LIM, 0.7f); // Roll angle limit in radians
|
||||
|
||||
//Pitch control parameters
|
||||
PARAM_DEFINE_FLOAT(FW_PITCHRATE_P, 0.3f);
|
||||
PARAM_DEFINE_FLOAT(FW_PITCHRATE_I, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_PITCHRATE_AWU, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_PITCHRATE_LIM, 0.35f); // Pitch rate limit in radians/sec
|
||||
PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.3f);
|
||||
PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.0f);
|
||||
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_LIM, 0.35f); // Pitch angle limit in radians
|
||||
|
||||
//Yaw control parameters //XXX TODO this is copy paste, asign correct values
|
||||
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_YAWR_P, 0.3f);
|
||||
PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec
|
||||
|
||||
/* Prototypes */
|
||||
/**
|
||||
|
|
@ -134,16 +134,20 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
|
|||
memset(&att_sp, 0, sizeof(att_sp));
|
||||
struct vehicle_rates_setpoint_s 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 */
|
||||
struct actuator_controls_s actuators;
|
||||
memset(&actuators, 0, sizeof(actuators));
|
||||
|
||||
|
||||
/* publish actuator controls */
|
||||
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
|
||||
actuators.control[i] = 0.0f;
|
||||
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 */
|
||||
|
|
@ -162,6 +166,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
|
|||
/*Get Local Copies */
|
||||
/* get a local copy of attitude */
|
||||
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[1] = att.pitchspeed;
|
||||
|
|
@ -170,14 +175,9 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
|
|||
/* 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,
|
||||
&att,
|
||||
&rates_sp);
|
||||
rates_sp.pitch = 0.0f; //REMOVEME TODO
|
||||
rates_sp.yaw = 0.0f;
|
||||
|
||||
/* Attitude Rate Control */
|
||||
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;
|
||||
|
||||
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");
|
||||
|
|
@ -198,6 +200,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
|
|||
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
|
||||
|
||||
|
||||
|
||||
close(att_sub);
|
||||
close(actuator_pub);
|
||||
|
||||
|
|
|
|||
|
|
@ -67,15 +67,12 @@ struct fw_rate_control_params {
|
|||
float rollrate_p;
|
||||
float rollrate_i;
|
||||
float rollrate_awu;
|
||||
float rollrate_lim;
|
||||
float pitchrate_p;
|
||||
float pitchrate_i;
|
||||
float pitchrate_awu;
|
||||
float pitchrate_lim;
|
||||
float yawrate_p;
|
||||
float yawrate_i;
|
||||
float yawrate_awu;
|
||||
float yawrate_lim;
|
||||
|
||||
};
|
||||
|
||||
|
|
@ -83,15 +80,13 @@ struct fw_rate_control_param_handles {
|
|||
float rollrate_p;
|
||||
float rollrate_i;
|
||||
float rollrate_awu;
|
||||
float rollrate_lim;
|
||||
float pitchrate_p;
|
||||
float pitchrate_i;
|
||||
float pitchrate_awu;
|
||||
float pitchrate_lim;
|
||||
float yawrate_p;
|
||||
float yawrate_i;
|
||||
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)
|
||||
{
|
||||
/* PID parameters */
|
||||
h->rollrate_p = param_find("FW_ROLLRATE_P"); //TODO define rate params for fixed wing
|
||||
h->rollrate_i = param_find("FW_ROLLRATE_I");
|
||||
h->rollrate_awu = param_find("FW_ROLLRATE_AWU");
|
||||
h->rollrate_lim = param_find("FW_ROLLRATE_LIM");
|
||||
h->pitchrate_p = param_find("FW_PITCHRATE_P");
|
||||
h->pitchrate_i = param_find("FW_PITCHRATE_I");
|
||||
h->pitchrate_awu = param_find("FW_PITCHRATE_AWU");
|
||||
h->pitchrate_lim = param_find("FW_PITCHRATE_LIM");
|
||||
h->yawrate_p = param_find("FW_YAWRATE_P");
|
||||
h->yawrate_i = param_find("FW_YAWRATE_I");
|
||||
h->yawrate_awu = param_find("FW_YAWRATE_AWU");
|
||||
h->yawrate_lim = param_find("FW_YAWRATE_LIM");
|
||||
h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing
|
||||
h->rollrate_i = param_find("FW_ROLLR_I");
|
||||
h->rollrate_awu = param_find("FW_ROLLR_AWU");
|
||||
|
||||
h->pitchrate_p = param_find("FW_PITCHR_P");
|
||||
h->pitchrate_i = param_find("FW_PITCHR_I");
|
||||
h->pitchrate_awu = param_find("FW_PITCHR_AWU");
|
||||
|
||||
h->yawrate_p = param_find("FW_YAWR_P");
|
||||
h->yawrate_i = param_find("FW_YAWR_I");
|
||||
h->yawrate_awu = param_find("FW_YAWR_AWU");
|
||||
|
||||
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_i, &(p->rollrate_i));
|
||||
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_i, &(p->pitchrate_i));
|
||||
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_i, &(p->yawrate_i));
|
||||
param_get(h->yawrate_awu, &(p->yawrate_awu));
|
||||
param_get(h->yawrate_lim, &(p->yawrate_lim));
|
||||
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
|
@ -159,9 +151,9 @@ 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_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(&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(&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, 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, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
|
||||
initialized = true;
|
||||
}
|
||||
|
||||
|
|
@ -169,9 +161,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
|||
if (counter % 100 == 0) {
|
||||
/* update parameters from storage */
|
||||
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(&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);
|
||||
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, 1);
|
||||
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);
|
||||
|
||||
//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);
|
||||
|
||||
counter++;
|
||||
|
|
|
|||
|
|
@ -1,7 +1,8 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -60,7 +61,29 @@
|
|||
#include <systemlib/geo/geo.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 */
|
||||
/* 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.
|
||||
*/
|
||||
|
|
@ -81,6 +104,29 @@ static bool thread_should_exit = false; /**< Deamon exit flag */
|
|||
static bool thread_running = false; /**< Deamon status flag */
|
||||
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 */
|
||||
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 */
|
||||
|
||||
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;
|
||||
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);
|
||||
sleep(500); //TODO removeme, this is for testing only
|
||||
|
||||
|
||||
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);
|
||||
|
||||
counter++;
|
||||
}
|
||||
|
||||
printf("[fixedwing_pos_control] exiting.\n");
|
||||
|
|
|
|||
Loading…
Reference in New Issue