mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Fixedwing controller code style
This commit is contained in:
Lorenz Meier
parent
9ab20b11b6
commit
dc100f2020
@ -112,9 +112,9 @@ static int parameters_update(const struct fw_pos_control_param_handles *h, struc
|
||||
}
|
||||
|
||||
int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
|
||||
const struct vehicle_attitude_s *att,
|
||||
const float speed_body[],
|
||||
struct vehicle_rates_setpoint_s *rates_sp)
|
||||
const struct vehicle_attitude_s *att,
|
||||
const float speed_body[],
|
||||
struct vehicle_rates_setpoint_s *rates_sp)
|
||||
{
|
||||
static int counter = 0;
|
||||
static bool initialized = false;
|
||||
@ -126,8 +126,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
|
||||
static PID_t pitch_controller;
|
||||
|
||||
|
||||
if(!initialized)
|
||||
{
|
||||
if (!initialized) {
|
||||
parameters_init(&h);
|
||||
parameters_update(&h, &p);
|
||||
pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
|
||||
@ -153,8 +152,8 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
|
||||
float pitch_sp_rollcompensation = p.pitch_roll_compensation_p * fabsf(sinf(att_sp->roll_body));
|
||||
/* set pitch plus feedforward roll compensation */
|
||||
rates_sp->pitch = pid_calculate(&pitch_controller,
|
||||
att_sp->pitch_body + pitch_sp_rollcompensation,
|
||||
att->pitch, 0, 0);
|
||||
att_sp->pitch_body + pitch_sp_rollcompensation,
|
||||
att->pitch, 0, 0);
|
||||
|
||||
/* Yaw (from coordinated turn constraint or lateral force) */
|
||||
rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll))
|
||||
|
||||
@ -44,8 +44,8 @@
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
|
||||
int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
|
||||
const struct vehicle_attitude_s *att,
|
||||
const float speed_body[],
|
||||
struct vehicle_rates_setpoint_s *rates_sp);
|
||||
const struct vehicle_attitude_s *att,
|
||||
const float speed_body[],
|
||||
struct vehicle_rates_setpoint_s *rates_sp);
|
||||
|
||||
#endif /* FIXEDWING_ATT_CONTROL_ATT_H_ */
|
||||
|
||||
@ -93,100 +93,146 @@ static int deamon_task; /**< Handle of deamon task / thread */
|
||||
int fixedwing_att_control_thread_main(int argc, char *argv[])
|
||||
{
|
||||
/* read arguments */
|
||||
bool verbose = false;
|
||||
bool verbose = false;
|
||||
|
||||
for (int i = 1; i < argc; i++) {
|
||||
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
|
||||
verbose = true;
|
||||
for (int i = 1; i < argc; i++) {
|
||||
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
|
||||
verbose = true;
|
||||
}
|
||||
}
|
||||
|
||||
/* welcome user */
|
||||
printf("[fixedwing att control] started\n");
|
||||
|
||||
/* declare and safely initialize all structs */
|
||||
struct vehicle_attitude_s att;
|
||||
memset(&att, 0, sizeof(att));
|
||||
struct vehicle_attitude_setpoint_s att_sp;
|
||||
memset(&att_sp, 0, sizeof(att_sp));
|
||||
struct vehicle_rates_setpoint_s rates_sp;
|
||||
memset(&rates_sp, 0, sizeof(rates_sp));
|
||||
struct vehicle_global_position_s global_pos;
|
||||
memset(&global_pos, 0, sizeof(global_pos));
|
||||
struct manual_control_setpoint_s manual_sp;
|
||||
memset(&manual_sp, 0, sizeof(manual_sp));
|
||||
struct vehicle_status_s vstatus;
|
||||
memset(&vstatus, 0, sizeof(vstatus));
|
||||
|
||||
/* 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 rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
|
||||
|
||||
/* subscribe */
|
||||
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
|
||||
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
|
||||
/* Setup of loop */
|
||||
float gyro[3] = {0.0f, 0.0f, 0.0f};
|
||||
float speed_body[3] = {0.0f, 0.0f, 0.0f};
|
||||
struct pollfd fds = { .fd = att_sub, .events = POLLIN };
|
||||
|
||||
while (!thread_should_exit) {
|
||||
/* wait for a sensor update, check for exit condition every 500 ms */
|
||||
poll(&fds, 1, 500);
|
||||
|
||||
/* Check if there is a new position measurement or attitude setpoint */
|
||||
bool pos_updated;
|
||||
orb_check(global_pos_sub, &pos_updated);
|
||||
bool att_sp_updated;
|
||||
orb_check(att_sp_sub, &att_sp_updated);
|
||||
|
||||
/* get a local copy of attitude */
|
||||
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
|
||||
|
||||
if (att_sp_updated)
|
||||
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
|
||||
|
||||
if (pos_updated) {
|
||||
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
|
||||
|
||||
if (att.R_valid) {
|
||||
speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz;
|
||||
speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz;
|
||||
speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz;
|
||||
|
||||
} else {
|
||||
speed_body[0] = 0;
|
||||
speed_body[1] = 0;
|
||||
speed_body[2] = 0;
|
||||
|
||||
printf("FW ATT CONTROL: Did not get a valid R\n");
|
||||
}
|
||||
}
|
||||
|
||||
/* welcome user */
|
||||
printf("[fixedwing att control] started\n");
|
||||
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
|
||||
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
|
||||
|
||||
/* declare and safely initialize all structs */
|
||||
struct vehicle_attitude_s att;
|
||||
memset(&att, 0, sizeof(att));
|
||||
struct vehicle_attitude_setpoint_s att_sp;
|
||||
memset(&att_sp, 0, sizeof(att_sp));
|
||||
struct vehicle_rates_setpoint_s rates_sp;
|
||||
memset(&rates_sp, 0, sizeof(rates_sp));
|
||||
struct vehicle_global_position_s global_pos;
|
||||
memset(&global_pos, 0, sizeof(global_pos));
|
||||
struct manual_control_setpoint_s manual_sp;
|
||||
memset(&manual_sp, 0, sizeof(manual_sp));
|
||||
struct vehicle_status_s vstatus;
|
||||
memset(&vstatus, 0, sizeof(vstatus));
|
||||
gyro[0] = att.rollspeed;
|
||||
gyro[1] = att.pitchspeed;
|
||||
gyro[2] = att.yawspeed;
|
||||
|
||||
/* output structs */
|
||||
struct actuator_controls_s actuators;
|
||||
memset(&actuators, 0, sizeof(actuators));
|
||||
/* control */
|
||||
|
||||
if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
|
||||
vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
|
||||
/* attitude control */
|
||||
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
|
||||
|
||||
/* 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 rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
|
||||
/* angular rate control */
|
||||
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
|
||||
|
||||
/* subscribe */
|
||||
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
|
||||
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
/* pass through throttle */
|
||||
actuators.control[3] = att_sp.thrust;
|
||||
|
||||
/* Setup of loop */
|
||||
float gyro[3] = {0.0f, 0.0f, 0.0f};
|
||||
float speed_body[3] = {0.0f, 0.0f, 0.0f};
|
||||
struct pollfd fds = { .fd = att_sub, .events = POLLIN };
|
||||
/* set flaps to zero */
|
||||
actuators.control[4] = 0.0f;
|
||||
|
||||
while(!thread_should_exit)
|
||||
{
|
||||
/* wait for a sensor update, check for exit condition every 500 ms */
|
||||
poll(&fds, 1, 500);
|
||||
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
|
||||
if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
|
||||
|
||||
/* Check if there is a new position measurement or attitude setpoint */
|
||||
bool pos_updated;
|
||||
orb_check(global_pos_sub, &pos_updated);
|
||||
bool att_sp_updated;
|
||||
orb_check(att_sp_sub, &att_sp_updated);
|
||||
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
|
||||
if (vstatus.rc_signal_lost) {
|
||||
|
||||
/* get a local copy of attitude */
|
||||
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
|
||||
if(att_sp_updated)
|
||||
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
|
||||
if(pos_updated)
|
||||
{
|
||||
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
|
||||
if(att.R_valid)
|
||||
{
|
||||
speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz;
|
||||
speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz;
|
||||
speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz;
|
||||
/* put plane into loiter */
|
||||
att_sp.roll_body = 0.3f;
|
||||
att_sp.pitch_body = 0.0f;
|
||||
|
||||
/* limit throttle to 60 % of last value if sane */
|
||||
if (isfinite(manual_sp.throttle) &&
|
||||
(manual_sp.throttle >= 0.0f) &&
|
||||
(manual_sp.throttle <= 1.0f)) {
|
||||
att_sp.thrust = 0.6f * manual_sp.throttle;
|
||||
|
||||
} else {
|
||||
att_sp.thrust = 0.0f;
|
||||
}
|
||||
|
||||
att_sp.yaw_body = 0;
|
||||
|
||||
// XXX disable yaw control, loiter
|
||||
|
||||
} else {
|
||||
|
||||
att_sp.roll_body = manual_sp.roll;
|
||||
att_sp.pitch_body = manual_sp.pitch;
|
||||
att_sp.yaw_body = 0;
|
||||
att_sp.thrust = manual_sp.throttle;
|
||||
}
|
||||
else
|
||||
{
|
||||
speed_body[0] = 0;
|
||||
speed_body[1] = 0;
|
||||
speed_body[2] = 0;
|
||||
|
||||
printf("FW ATT CONTROL: Did not get a valid R\n");
|
||||
}
|
||||
}
|
||||
att_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
|
||||
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
|
||||
|
||||
gyro[0] = att.rollspeed;
|
||||
gyro[1] = att.pitchspeed;
|
||||
gyro[2] = att.yawspeed;
|
||||
|
||||
/* control */
|
||||
|
||||
if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
|
||||
vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
|
||||
/* attitude control */
|
||||
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
|
||||
|
||||
@ -196,104 +242,62 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
|
||||
/* pass through throttle */
|
||||
actuators.control[3] = att_sp.thrust;
|
||||
|
||||
/* set flaps to zero */
|
||||
actuators.control[4] = 0.0f;
|
||||
/* pass through flaps */
|
||||
if (isfinite(manual_sp.flaps)) {
|
||||
actuators.control[4] = manual_sp.flaps;
|
||||
|
||||
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
|
||||
if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
|
||||
|
||||
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
|
||||
if (vstatus.rc_signal_lost) {
|
||||
|
||||
/* put plane into loiter */
|
||||
att_sp.roll_body = 0.3f;
|
||||
att_sp.pitch_body = 0.0f;
|
||||
|
||||
/* limit throttle to 60 % of last value if sane */
|
||||
if (isfinite(manual_sp.throttle) &&
|
||||
(manual_sp.throttle >= 0.0f) &&
|
||||
(manual_sp.throttle <= 1.0f)) {
|
||||
att_sp.thrust = 0.6f * manual_sp.throttle;
|
||||
} else {
|
||||
att_sp.thrust = 0.0f;
|
||||
}
|
||||
att_sp.yaw_body = 0;
|
||||
|
||||
// XXX disable yaw control, loiter
|
||||
|
||||
} else {
|
||||
|
||||
att_sp.roll_body = manual_sp.roll;
|
||||
att_sp.pitch_body = manual_sp.pitch;
|
||||
att_sp.yaw_body = 0;
|
||||
att_sp.thrust = manual_sp.throttle;
|
||||
}
|
||||
|
||||
att_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
/* attitude control */
|
||||
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
|
||||
|
||||
/* angular rate control */
|
||||
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
|
||||
|
||||
/* pass through throttle */
|
||||
actuators.control[3] = att_sp.thrust;
|
||||
|
||||
/* pass through flaps */
|
||||
if (isfinite(manual_sp.flaps)) {
|
||||
actuators.control[4] = manual_sp.flaps;
|
||||
} else {
|
||||
actuators.control[4] = 0.0f;
|
||||
}
|
||||
|
||||
} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
|
||||
/* directly pass through values */
|
||||
actuators.control[0] = manual_sp.roll;
|
||||
/* positive pitch means negative actuator -> pull up */
|
||||
actuators.control[1] = manual_sp.pitch;
|
||||
actuators.control[2] = manual_sp.yaw;
|
||||
actuators.control[3] = manual_sp.throttle;
|
||||
if (isfinite(manual_sp.flaps)) {
|
||||
actuators.control[4] = manual_sp.flaps;
|
||||
} else {
|
||||
actuators.control[4] = 0.0f;
|
||||
}
|
||||
} else {
|
||||
actuators.control[4] = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
/* publish rates */
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
|
||||
} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
|
||||
/* directly pass through values */
|
||||
actuators.control[0] = manual_sp.roll;
|
||||
/* positive pitch means negative actuator -> pull up */
|
||||
actuators.control[1] = manual_sp.pitch;
|
||||
actuators.control[2] = manual_sp.yaw;
|
||||
actuators.control[3] = manual_sp.throttle;
|
||||
|
||||
/* sanity check and publish actuator outputs */
|
||||
if (isfinite(actuators.control[0]) &&
|
||||
isfinite(actuators.control[1]) &&
|
||||
isfinite(actuators.control[2]) &&
|
||||
isfinite(actuators.control[3]))
|
||||
{
|
||||
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
|
||||
if (isfinite(manual_sp.flaps)) {
|
||||
actuators.control[4] = manual_sp.flaps;
|
||||
|
||||
} else {
|
||||
actuators.control[4] = 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
printf("[fixedwing_att_control] exiting, stopping all motors.\n");
|
||||
thread_running = false;
|
||||
/* publish rates */
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
|
||||
|
||||
/* kill all outputs */
|
||||
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
|
||||
actuators.control[i] = 0.0f;
|
||||
/* sanity check and publish actuator outputs */
|
||||
if (isfinite(actuators.control[0]) &&
|
||||
isfinite(actuators.control[1]) &&
|
||||
isfinite(actuators.control[2]) &&
|
||||
isfinite(actuators.control[3])) {
|
||||
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
|
||||
}
|
||||
}
|
||||
|
||||
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
|
||||
printf("[fixedwing_att_control] exiting, stopping all motors.\n");
|
||||
thread_running = false;
|
||||
|
||||
/* kill all outputs */
|
||||
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
|
||||
actuators.control[i] = 0.0f;
|
||||
|
||||
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
|
||||
|
||||
|
||||
|
||||
close(att_sub);
|
||||
close(actuator_pub);
|
||||
close(rates_pub);
|
||||
close(att_sub);
|
||||
close(actuator_pub);
|
||||
close(rates_pub);
|
||||
|
||||
fflush(stdout);
|
||||
exit(0);
|
||||
fflush(stdout);
|
||||
exit(0);
|
||||
|
||||
return 0;
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
@ -304,6 +308,7 @@ usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
|
||||
fprintf(stderr, "usage: fixedwing_att_control {start|stop|status}\n\n");
|
||||
exit(1);
|
||||
}
|
||||
@ -348,9 +353,11 @@ int fixedwing_att_control_main(int argc, char *argv[])
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
printf("\tfixedwing_att_control is running\n");
|
||||
|
||||
} else {
|
||||
printf("\tfixedwing_att_control not started\n");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
||||
@ -157,8 +157,8 @@ static int parameters_update(const struct fw_rate_control_param_handles *h, stru
|
||||
}
|
||||
|
||||
int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
||||
const float rates[],
|
||||
struct actuator_controls_s *actuators)
|
||||
const float rates[],
|
||||
struct actuator_controls_s *actuators)
|
||||
{
|
||||
static int counter = 0;
|
||||
static bool initialized = false;
|
||||
@ -174,8 +174,7 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
||||
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
|
||||
last_run = hrt_absolute_time();
|
||||
|
||||
if(!initialized)
|
||||
{
|
||||
if (!initialized) {
|
||||
parameters_init(&h);
|
||||
parameters_update(&h, &p);
|
||||
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
|
||||
|
||||
@ -42,7 +42,7 @@
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
|
||||
int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
||||
const float rates[],
|
||||
struct actuator_controls_s *actuators);
|
||||
const float rates[],
|
||||
struct actuator_controls_s *actuators);
|
||||
|
||||
#endif /* FIXEDWING_ATT_CONTROL_RATE_H_ */
|
||||
|
||||
@ -108,8 +108,8 @@ struct planned_path_segments_s {
|
||||
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 arc_sweep; // Angle (radians) swept out by arc around center.
|
||||
// Positive for clockwise, negative for counter-clockwise
|
||||
};
|
||||
|
||||
|
||||
@ -176,241 +176,241 @@ static int parameters_update(const struct fw_pos_control_param_handles *h, struc
|
||||
int fixedwing_pos_control_thread_main(int argc, char *argv[])
|
||||
{
|
||||
/* read arguments */
|
||||
bool verbose = false;
|
||||
bool verbose = false;
|
||||
|
||||
for (int i = 1; i < argc; i++) {
|
||||
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
|
||||
verbose = true;
|
||||
}
|
||||
for (int i = 1; i < argc; i++) {
|
||||
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
|
||||
verbose = true;
|
||||
}
|
||||
}
|
||||
|
||||
/* welcome user */
|
||||
printf("[fixedwing pos control] started\n");
|
||||
/* welcome user */
|
||||
printf("[fixedwing pos control] started\n");
|
||||
|
||||
/* declare and safely initialize all structs */
|
||||
struct vehicle_global_position_s global_pos;
|
||||
memset(&global_pos, 0, sizeof(global_pos));
|
||||
struct vehicle_global_position_s start_pos; // Temporary variable, replace with
|
||||
memset(&start_pos, 0, sizeof(start_pos)); // previous waypoint when available
|
||||
struct vehicle_global_position_setpoint_s global_setpoint;
|
||||
memset(&global_setpoint, 0, sizeof(global_setpoint));
|
||||
struct vehicle_attitude_s att;
|
||||
memset(&att, 0, sizeof(att));
|
||||
struct crosstrack_error_s xtrack_err;
|
||||
memset(&xtrack_err, 0, sizeof(xtrack_err));
|
||||
struct parameter_update_s param_update;
|
||||
memset(¶m_update, 0, sizeof(param_update));
|
||||
|
||||
/* output structs */
|
||||
struct vehicle_attitude_setpoint_s attitude_setpoint;
|
||||
memset(&attitude_setpoint, 0, sizeof(attitude_setpoint));
|
||||
/* declare and safely initialize all structs */
|
||||
struct vehicle_global_position_s global_pos;
|
||||
memset(&global_pos, 0, sizeof(global_pos));
|
||||
struct vehicle_global_position_s start_pos; // Temporary variable, replace with
|
||||
memset(&start_pos, 0, sizeof(start_pos)); // previous waypoint when available
|
||||
struct vehicle_global_position_setpoint_s global_setpoint;
|
||||
memset(&global_setpoint, 0, sizeof(global_setpoint));
|
||||
struct vehicle_attitude_s att;
|
||||
memset(&att, 0, sizeof(att));
|
||||
struct crosstrack_error_s xtrack_err;
|
||||
memset(&xtrack_err, 0, sizeof(xtrack_err));
|
||||
struct parameter_update_s param_update;
|
||||
memset(¶m_update, 0, sizeof(param_update));
|
||||
|
||||
/* publish attitude setpoint */
|
||||
attitude_setpoint.roll_body = 0.0f;
|
||||
attitude_setpoint.pitch_body = 0.0f;
|
||||
attitude_setpoint.yaw_body = 0.0f;
|
||||
attitude_setpoint.thrust = 0.0f;
|
||||
orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint);
|
||||
/* output structs */
|
||||
struct vehicle_attitude_setpoint_s attitude_setpoint;
|
||||
memset(&attitude_setpoint, 0, sizeof(attitude_setpoint));
|
||||
|
||||
/* subscribe */
|
||||
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
|
||||
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
int param_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
/* publish attitude setpoint */
|
||||
attitude_setpoint.roll_body = 0.0f;
|
||||
attitude_setpoint.pitch_body = 0.0f;
|
||||
attitude_setpoint.yaw_body = 0.0f;
|
||||
attitude_setpoint.thrust = 0.0f;
|
||||
orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint);
|
||||
|
||||
/* Setup of loop */
|
||||
struct pollfd fds[2] = {
|
||||
{ .fd = param_sub, .events = POLLIN },
|
||||
{ .fd = att_sub, .events = POLLIN }
|
||||
};
|
||||
bool global_sp_updated_set_once = false;
|
||||
/* subscribe */
|
||||
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
|
||||
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
int param_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
|
||||
float psi_track = 0.0f;
|
||||
/* Setup of loop */
|
||||
struct pollfd fds[2] = {
|
||||
{ .fd = param_sub, .events = POLLIN },
|
||||
{ .fd = att_sub, .events = POLLIN }
|
||||
};
|
||||
bool global_sp_updated_set_once = false;
|
||||
|
||||
int counter = 0;
|
||||
float psi_track = 0.0f;
|
||||
|
||||
struct fw_pos_control_params p;
|
||||
struct fw_pos_control_param_handles h;
|
||||
int counter = 0;
|
||||
|
||||
PID_t heading_controller;
|
||||
PID_t heading_rate_controller;
|
||||
PID_t offtrack_controller;
|
||||
PID_t altitude_controller;
|
||||
struct fw_pos_control_params p;
|
||||
struct fw_pos_control_param_handles h;
|
||||
|
||||
parameters_init(&h);
|
||||
parameters_update(&h, &p);
|
||||
pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit
|
||||
pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE);
|
||||
pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE);
|
||||
pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f*M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value
|
||||
PID_t heading_controller;
|
||||
PID_t heading_rate_controller;
|
||||
PID_t offtrack_controller;
|
||||
PID_t altitude_controller;
|
||||
|
||||
/* error and performance monitoring */
|
||||
perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval");
|
||||
perf_counter_t fw_err_perf = perf_alloc(PC_COUNT, "fixedwing_pos_control_err");
|
||||
parameters_init(&h);
|
||||
parameters_update(&h, &p);
|
||||
pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit
|
||||
pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE);
|
||||
pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE);
|
||||
pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value
|
||||
|
||||
while(!thread_should_exit)
|
||||
{
|
||||
/* wait for a sensor update, check for exit condition every 500 ms */
|
||||
int ret = poll(fds, 2, 500);
|
||||
/* error and performance monitoring */
|
||||
perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval");
|
||||
perf_counter_t fw_err_perf = perf_alloc(PC_COUNT, "fixedwing_pos_control_err");
|
||||
|
||||
if (ret < 0) {
|
||||
/* poll error, count it in perf */
|
||||
perf_count(fw_err_perf);
|
||||
} else if (ret == 0) {
|
||||
/* no return value, ignore */
|
||||
} else {
|
||||
while (!thread_should_exit) {
|
||||
/* wait for a sensor update, check for exit condition every 500 ms */
|
||||
int ret = poll(fds, 2, 500);
|
||||
|
||||
/* only update parameters if they changed */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
/* read from param to clear updated flag */
|
||||
struct parameter_update_s update;
|
||||
orb_copy(ORB_ID(parameter_update), param_sub, &update);
|
||||
if (ret < 0) {
|
||||
/* poll error, count it in perf */
|
||||
perf_count(fw_err_perf);
|
||||
|
||||
/* update parameters from storage */
|
||||
parameters_update(&h, &p);
|
||||
pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit
|
||||
pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim);
|
||||
pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
|
||||
pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f*M_DEG_TO_RAD); //TODO: remove hardcoded value
|
||||
} else if (ret == 0) {
|
||||
/* no return value, ignore */
|
||||
} else {
|
||||
|
||||
/* only update parameters if they changed */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
/* read from param to clear updated flag */
|
||||
struct parameter_update_s update;
|
||||
orb_copy(ORB_ID(parameter_update), param_sub, &update);
|
||||
|
||||
/* update parameters from storage */
|
||||
parameters_update(&h, &p);
|
||||
pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit
|
||||
pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim);
|
||||
pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
|
||||
pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value
|
||||
}
|
||||
|
||||
/* only run controller if attitude changed */
|
||||
if (fds[1].revents & POLLIN) {
|
||||
|
||||
|
||||
static uint64_t last_run = 0;
|
||||
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
|
||||
last_run = hrt_absolute_time();
|
||||
|
||||
/* check if there is a new position or setpoint */
|
||||
bool pos_updated;
|
||||
orb_check(global_pos_sub, &pos_updated);
|
||||
bool global_sp_updated;
|
||||
orb_check(global_setpoint_sub, &global_sp_updated);
|
||||
|
||||
/* load local copies */
|
||||
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
|
||||
|
||||
if (pos_updated) {
|
||||
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
|
||||
}
|
||||
|
||||
/* only run controller if attitude changed */
|
||||
if (fds[1].revents & POLLIN) {
|
||||
if (global_sp_updated) {
|
||||
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
|
||||
start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
|
||||
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("next wp direction: %0.4f\n", (double)psi_track);
|
||||
}
|
||||
|
||||
static uint64_t last_run = 0;
|
||||
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
|
||||
last_run = hrt_absolute_time();
|
||||
/* Simple Horizontal Control */
|
||||
if (global_sp_updated_set_once) {
|
||||
// if (counter % 100 == 0)
|
||||
// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
|
||||
|
||||
/* check if there is a new position or setpoint */
|
||||
bool pos_updated;
|
||||
orb_check(global_pos_sub, &pos_updated);
|
||||
bool global_sp_updated;
|
||||
orb_check(global_setpoint_sub, &global_sp_updated);
|
||||
/* calculate crosstrack error */
|
||||
// Only the case of a straight line track following handled so far
|
||||
int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
|
||||
(double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d,
|
||||
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
|
||||
|
||||
/* load local copies */
|
||||
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
|
||||
// XXX what is xtrack_err.past_end?
|
||||
if (distance_res == OK /*&& !xtrack_err.past_end*/) {
|
||||
|
||||
if (pos_updated) {
|
||||
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
|
||||
}
|
||||
float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance
|
||||
|
||||
if (global_sp_updated) {
|
||||
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
|
||||
start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
|
||||
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("next wp direction: %0.4f\n", (double)psi_track);
|
||||
}
|
||||
float psi_c = psi_track + delta_psi_c;
|
||||
float psi_e = psi_c - att.yaw;
|
||||
|
||||
/* Simple Horizontal Control */
|
||||
if(global_sp_updated_set_once)
|
||||
{
|
||||
// if (counter % 100 == 0)
|
||||
// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
|
||||
/* wrap difference back onto -pi..pi range */
|
||||
psi_e = _wrap_pi(psi_e);
|
||||
|
||||
/* calculate crosstrack error */
|
||||
// Only the case of a straight line track following handled so far
|
||||
int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
|
||||
(double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d,
|
||||
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
|
||||
|
||||
// XXX what is xtrack_err.past_end?
|
||||
if(distance_res == OK /*&& !xtrack_err.past_end*/) {
|
||||
|
||||
float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance
|
||||
|
||||
float psi_c = psi_track + delta_psi_c;
|
||||
float psi_e = psi_c - att.yaw;
|
||||
|
||||
/* wrap difference back onto -pi..pi range */
|
||||
psi_e = _wrap_pi(psi_e);
|
||||
|
||||
if (verbose) {
|
||||
printf("xtrack_err.distance %.4f ", (double)xtrack_err.distance);
|
||||
printf("delta_psi_c %.4f ", (double)delta_psi_c);
|
||||
printf("psi_c %.4f ", (double)psi_c);
|
||||
printf("att.yaw %.4f ", (double)att.yaw);
|
||||
printf("psi_e %.4f ", (double)psi_e);
|
||||
}
|
||||
|
||||
/* calculate roll setpoint, do this artificially around zero */
|
||||
float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
|
||||
float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following
|
||||
float psi_rate_c = delta_psi_rate_c + psi_rate_track;
|
||||
|
||||
/* limit turn rate */
|
||||
if(psi_rate_c > p.headingr_lim) {
|
||||
psi_rate_c = p.headingr_lim;
|
||||
} else if(psi_rate_c < -p.headingr_lim) {
|
||||
psi_rate_c = -p.headingr_lim;
|
||||
}
|
||||
|
||||
float psi_rate_e = psi_rate_c - att.yawspeed;
|
||||
|
||||
// XXX sanity check: Assume 10 m/s stall speed and no stall condition
|
||||
float ground_speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
|
||||
|
||||
if (ground_speed < 10.0f) {
|
||||
ground_speed = 10.0f;
|
||||
}
|
||||
|
||||
float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g
|
||||
|
||||
attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
|
||||
|
||||
if (verbose) {
|
||||
printf("psi_rate_c %.4f ", (double)psi_rate_c);
|
||||
printf("psi_rate_e_scaled %.4f ", (double)psi_rate_e_scaled);
|
||||
printf("rollbody %.4f\n", (double)attitude_setpoint.roll_body);
|
||||
}
|
||||
|
||||
if (verbose && counter % 100 == 0)
|
||||
printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c);
|
||||
} else {
|
||||
if (verbose && counter % 100 == 0)
|
||||
printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end);
|
||||
if (verbose) {
|
||||
printf("xtrack_err.distance %.4f ", (double)xtrack_err.distance);
|
||||
printf("delta_psi_c %.4f ", (double)delta_psi_c);
|
||||
printf("psi_c %.4f ", (double)psi_c);
|
||||
printf("att.yaw %.4f ", (double)att.yaw);
|
||||
printf("psi_e %.4f ", (double)psi_e);
|
||||
}
|
||||
|
||||
/* Very simple Altitude Control */
|
||||
if(pos_updated)
|
||||
{
|
||||
/* calculate roll setpoint, do this artificially around zero */
|
||||
float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
|
||||
float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following
|
||||
float psi_rate_c = delta_psi_rate_c + psi_rate_track;
|
||||
|
||||
//TODO: take care of relative vs. ab. altitude
|
||||
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
|
||||
/* limit turn rate */
|
||||
if (psi_rate_c > p.headingr_lim) {
|
||||
psi_rate_c = p.headingr_lim;
|
||||
|
||||
} else if (psi_rate_c < -p.headingr_lim) {
|
||||
psi_rate_c = -p.headingr_lim;
|
||||
}
|
||||
|
||||
// XXX need speed control
|
||||
attitude_setpoint.thrust = 0.7f;
|
||||
float psi_rate_e = psi_rate_c - att.yawspeed;
|
||||
|
||||
/* publish the attitude setpoint */
|
||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint);
|
||||
// XXX sanity check: Assume 10 m/s stall speed and no stall condition
|
||||
float ground_speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
|
||||
|
||||
/* measure in what intervals the controller runs */
|
||||
perf_count(fw_interval_perf);
|
||||
if (ground_speed < 10.0f) {
|
||||
ground_speed = 10.0f;
|
||||
}
|
||||
|
||||
counter++;
|
||||
float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g
|
||||
|
||||
attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
|
||||
|
||||
if (verbose) {
|
||||
printf("psi_rate_c %.4f ", (double)psi_rate_c);
|
||||
printf("psi_rate_e_scaled %.4f ", (double)psi_rate_e_scaled);
|
||||
printf("rollbody %.4f\n", (double)attitude_setpoint.roll_body);
|
||||
}
|
||||
|
||||
if (verbose && counter % 100 == 0)
|
||||
printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n", xtrack_err.distance, delta_psi_c);
|
||||
|
||||
} else {
|
||||
// XXX no setpoint, decent default needed (loiter?)
|
||||
if (verbose && counter % 100 == 0)
|
||||
printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end);
|
||||
}
|
||||
|
||||
/* Very simple Altitude Control */
|
||||
if (pos_updated) {
|
||||
|
||||
//TODO: take care of relative vs. ab. altitude
|
||||
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
|
||||
|
||||
}
|
||||
|
||||
// XXX need speed control
|
||||
attitude_setpoint.thrust = 0.7f;
|
||||
|
||||
/* publish the attitude setpoint */
|
||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint);
|
||||
|
||||
/* measure in what intervals the controller runs */
|
||||
perf_count(fw_interval_perf);
|
||||
|
||||
counter++;
|
||||
|
||||
} else {
|
||||
// XXX no setpoint, decent default needed (loiter?)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
printf("[fixedwing_pos_control] exiting.\n");
|
||||
thread_running = false;
|
||||
printf("[fixedwing_pos_control] exiting.\n");
|
||||
thread_running = false;
|
||||
|
||||
|
||||
close(attitude_setpoint_pub);
|
||||
close(attitude_setpoint_pub);
|
||||
|
||||
fflush(stdout);
|
||||
exit(0);
|
||||
fflush(stdout);
|
||||
exit(0);
|
||||
|
||||
return 0;
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
@ -421,6 +421,7 @@ usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
|
||||
fprintf(stderr, "usage: fixedwing_pos_control {start|stop|status}\n\n");
|
||||
exit(1);
|
||||
}
|
||||
@ -465,9 +466,11 @@ int fixedwing_pos_control_main(int argc, char *argv[])
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
printf("\tfixedwing_pos_control is running\n");
|
||||
|
||||
} else {
|
||||
printf("\tfixedwing_pos_control not started\n");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user