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

[work in progess] added pos control skeleton

This commit is contained in:
Thomas Gubler 2012-10-21 21:01:22 +02:00
parent 6c8fb8177e
commit 5616f5c4b1
7 changed files with 299 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
apps/fixedwing_control2/Makefile → apps/fixedwing_att_control/Makefile
View File

@ -35,7 +35,7 @@
# Fixedwing Control application
#
APPNAME = fixedwing_control2
APPNAME = fixedwing_att_control
PRIORITY = SCHED_PRIORITY_MAX - 30
STACKSIZE = 2048

30
apps/fixedwing_control2/fixedwing_control2.c → apps/fixedwing_att_control/fixedwing_att_control_main.c
View File

@ -32,7 +32,7 @@
*
****************************************************************************/
/**
* @file fixedwing_control2.c
* @file fixedwing_att_control.c
* Implementation of a fixed wing attitude controller.
*/
@ -62,18 +62,18 @@
#include <systemlib/geo/geo.h>
#include <systemlib/systemlib.h>
#include <fixedwing_control2_rate.h>
#include <fixedwing_att_control_rate.h>
/* Prototypes */
/**
* Deamon management function.
*/
__EXPORT int fixedwing_control2_main(int argc, char *argv[]);
__EXPORT int fixedwing_att_control_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int fixedwing_control2_thread_main(int argc, char *argv[]);
int fixedwing_att_control_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
@ -86,7 +86,7 @@ static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
/* Main Thread */
int fixedwing_control2_thread_main(int argc, char *argv[])
int fixedwing_att_control_thread_main(int argc, char *argv[])
{
/* read arguments */
bool verbose = false;
@ -98,7 +98,7 @@ int fixedwing_control2_thread_main(int argc, char *argv[])
}
/* welcome user */
printf("[fixedwing control2] started\n");
printf("[fixedwing att_control] started\n");
/* declare and safely initialize all structs */
struct vehicle_attitude_s att;
@ -145,7 +145,7 @@ int fixedwing_control2_thread_main(int argc, char *argv[])
rates_sp.yaw = 0.0f;
/* Attitude Rate Control */
fixedwing_control2_rates(&rates_sp, gyro, &actuators);
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
//REMOVEME XXX
actuators.control[3] = 0.7f;
@ -153,7 +153,7 @@ int fixedwing_control2_thread_main(int argc, char *argv[])
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
}
printf("[fixedwing_control2] exiting, stopping all motors.\n");
printf("[fixedwing_att_control] exiting, stopping all motors.\n");
thread_running = false;
/* kill all outputs */
@ -179,7 +179,7 @@ usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: fixedwing_control2 {start|stop|status}\n\n");
fprintf(stderr, "usage: fixedwing_att_control {start|stop|status}\n\n");
exit(1);
}
@ -191,7 +191,7 @@ usage(const char *reason)
* The actual stack size should be set in the call
* to task_create().
*/
int fixedwing_control2_main(int argc, char *argv[])
int fixedwing_att_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
@ -199,17 +199,17 @@ int fixedwing_control2_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("fixedwing_control2 already running\n");
printf("fixedwing_att_control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("fixedwing_control2",
deamon_task = task_spawn("fixedwing_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
4096,
fixedwing_control2_thread_main,
fixedwing_att_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
@ -222,9 +222,9 @@ int fixedwing_control2_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tfixedwing_control2 is running\n");
printf("\tfixedwing_att_control is running\n");
} else {
printf("\tfixedwing_control2 not started\n");
printf("\tfixedwing_att_control not started\n");
}
exit(0);
}

30
apps/fixedwing_control2/fixedwing_control2_rate.c → apps/fixedwing_att_control/fixedwing_att_control_rate.c
View File

@ -32,9 +32,10 @@
*
****************************************************************************/
/**
* @file fixedwing_control2.c
* @file fixedwing_att_control_rate.c
* Implementation of a fixed wing attitude controller.
*/
#include <fixedwing_att_control_rate.h>
#include <nuttx/config.h>
#include <stdio.h>
@ -53,14 +54,14 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/systemlib.h>
struct fw_rate_control_params {
param_t yawrate_p;
@ -97,6 +98,9 @@ static int parameters_init(struct fw_rate_control_params *h)
h->attrate_awu = param_find("MC_ATTRATE_AWU");
h->attrate_lim = param_find("MC_ATTRATE_LIM");
// if(h->attrate_i == PARAM_INVALID)
// printf("FATAL MC_ATTRATE_I does not exist\n");
return OK;
}
@ -114,11 +118,12 @@ static int parameters_update(const struct fw_rate_control_params *h, struct fw_r
param_get(h->attrate_awu, &(p->attrate_awu));
param_get(h->attrate_lim, &(p->attrate_lim));
p->attrate_i = 0.01f; //TODO: as long as the parameter is not implemented
return OK;
}
int fixedwing_control2_rates(const struct vehicle_rates_setpoint_s *rate_sp,
int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[],
struct actuator_controls_s *actuators)
{
@ -128,23 +133,30 @@ int fixedwing_control2_rates(const struct vehicle_rates_setpoint_s *rate_sp,
static struct fw_rate_control_params p;
static struct fw_rate_control_params h;
static PID_t roll_rate_controller;
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
if(!initialized)
{
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu, PID_MODE_DERIVATIV_SET); //D part set to 0 because the controller layout is with a PI rate controller
initialized = true;
}
/* load new parameters with lower rate */
if (counter % 2500 == 0) {
/* update parameters from storage */
pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu);
parameters_update(&h, &p);
printf("p.yawrate_p: %8.4f\n", (double)p.yawrate_p);
}
/* Roll Rate */
float roll_error = rate_sp->roll - rates[0];
actuators->control[0] =p.attrate_p*roll_error;
/* Roll Rate (PI) */
float roll_rate_error = rate_sp->roll - rates[0];
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT);;
actuators->control[1] = 0;

11
apps/fixedwing_control2/fixedwing_control2_rate.h → apps/fixedwing_att_control/fixedwing_att_control_rate.h
View File

@ -35,11 +35,14 @@
/* @file Main system state machine definition */
#ifndef FIXEDWING_CONTROL2_RATE_H_
#define FIXEDWING_CONTROL2_RATE_H_
#ifndef FIXEDWING_ATT_CONTROL_RATE_H_
#define FIXEDWING_ATT_CONTROL_RATE_H_
int fixedwing_control2_rates(const struct vehicle_rates_setpoint_s *rate_sp,
#include <uORB/topics/vehicle_rates_setpoint.h>
#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);
#endif /* FIXEDWING_CONTROL2_RATE_H_ */
#endif /* FIXEDWING_ATT_CONTROL_RATE_H_ */

45
apps/fixedwing_pos_control/Makefile Normal file
View File

@ -0,0 +1,45 @@
############################################################################
#
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Fixedwing Control application
#
APPNAME = fixedwing_pos_control
PRIORITY = SCHED_PRIORITY_MAX - 30
STACKSIZE = 2048
INCLUDES = $(TOPDIR)/../mavlink/include/mavlink
include $(APPDIR)/mk/app.mk

208
apps/fixedwing_pos_control/fixedwing_pos_control_main.c Normal file
View File

@ -0,0 +1,208 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Ivan Ovinnikov <oivan@ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file fixedwing_pos_control.c
* Implementation of a fixed wing attitude controller.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <arch/board/up_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/systemlib.h>
/* Prototypes */
/**
* Deamon management function.
*/
__EXPORT int fixedwing_pos_control_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int fixedwing_pos_control_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
/* Variables */
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 */
/* Main Thread */
int fixedwing_pos_control_thread_main(int argc, char *argv[])
{
/* read arguments */
bool verbose = false;
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 */
/* output structs */
struct vehicle_attitude_setpoint_s attitude_setpoint;
memset(&attitude_setpoint, 0, sizeof(attitude_setpoint));
/* publish attitude setpoint */
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 to attitude (for attitude rate) and rate septoint */
//
/* Setup of loop */
// 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);
sleep(500); //TODO removeme, this is for testing only
/* Control */
orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint);
}
printf("[fixedwing_pos_control] exiting.\n");
thread_running = false;
close(attitude_setpoint_pub);
fflush(stdout);
exit(0);
return 0;
}
/* Startup Functions */
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: fixedwing_pos_control {start|stop|status}\n\n");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int fixedwing_pos_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("fixedwing_pos_control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("fixedwing_pos_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
4096,
fixedwing_pos_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
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);
}
usage("unrecognized command");
exit(1);
}

3
nuttx/configs/px4fmu/nsh/appconfig
View File

@ -78,7 +78,8 @@ CONFIGURED_APPS += multirotor_att_control
CONFIGURED_APPS += multirotor_pos_control
CONFIGURED_APPS += px4/attitude_estimator_bm
CONFIGURED_APPS += fixedwing_control
CONFIGURED_APPS += fixedwing_control2
CONFIGURED_APPS += fixedwing_att_control
CONFIGURED_APPS += fixedwing_pos_control
CONFIGURED_APPS += position_estimator
CONFIGURED_APPS += attitude_estimator_ekf
CONFIGURED_APPS += px4/ground_estimator