diff options
-rw-r--r-- | apps/fixedwing_control2/Makefile | 45 | ||||
-rw-r--r-- | apps/fixedwing_control2/fixedwing_control2.c | 237 | ||||
-rw-r--r-- | apps/fixedwing_control2/fixedwing_control2_rate.c | 159 | ||||
-rw-r--r-- | apps/fixedwing_control2/fixedwing_control2_rate.h | 45 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/nsh/appconfig | 1 |
5 files changed, 487 insertions, 0 deletions
diff --git a/apps/fixedwing_control2/Makefile b/apps/fixedwing_control2/Makefile new file mode 100644 index 000000000..0db19875a --- /dev/null +++ b/apps/fixedwing_control2/Makefile @@ -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_control2 +PRIORITY = SCHED_PRIORITY_MAX - 30 +STACKSIZE = 2048 + +INCLUDES = $(TOPDIR)/../mavlink/include/mavlink + +include $(APPDIR)/mk/app.mk + diff --git a/apps/fixedwing_control2/fixedwing_control2.c b/apps/fixedwing_control2/fixedwing_control2.c new file mode 100644 index 000000000..773074289 --- /dev/null +++ b/apps/fixedwing_control2/fixedwing_control2.c @@ -0,0 +1,237 @@ +/**************************************************************************** + * + * 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_control2.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.h> +#include <uORB/topics/vehicle_status.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> + +#include <fixedwing_control2_rate.h> + +/* Prototypes */ +/** + * Deamon management function. + */ +__EXPORT int fixedwing_control2_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int fixedwing_control2_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_control2_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 control2] started\n"); + + /* declare and safely initialize all structs */ + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + struct vehicle_rates_setpoint_s rates_sp; + memset(&rates_sp, 0, sizeof(rates_sp)); + + /* 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); + + + + /* subscribe to attitude (for attitude rate) and rate septoint */ + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + + /* Setup of loop */ + float gyro[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); + + /*Get Local Copies */ + /* get a local copy of attitude */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + + gyro[0] = att.rollspeed; + gyro[1] = att.pitchspeed; + gyro[2] = att.yawspeed; + + /* Control */ + + /* Attitude Control */ + rates_sp.roll = 0.0f; + rates_sp.pitch = 0.0f; + rates_sp.yaw = 0.0f; + + /* Attitude Rate Control */ + fixedwing_control2_rates(&rates_sp, gyro, &actuators); + + //REMOVEME XXX + actuators.control[3] = 0.7f; + + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + } + + printf("[fixedwing_control2] 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); + + 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_control2 {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_control2_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("fixedwing_control2 already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn("fixedwing_control2", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 20, + 4096, + fixedwing_control2_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_control2 is running\n"); + } else { + printf("\tfixedwing_control2 not started\n"); + } + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + + + diff --git a/apps/fixedwing_control2/fixedwing_control2_rate.c b/apps/fixedwing_control2/fixedwing_control2_rate.c new file mode 100644 index 000000000..9c6f9d591 --- /dev/null +++ b/apps/fixedwing_control2/fixedwing_control2_rate.c @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.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_control2.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_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; + param_t yawrate_i; + param_t yawrate_d; + param_t yawrate_awu; + param_t yawrate_lim; + + param_t attrate_p; + param_t attrate_i; + param_t attrate_d; + param_t attrate_awu; + param_t attrate_lim; +}; + + + +/* Internal Prototypes */ +static int parameters_init(struct fw_rate_control_params *h); +static int parameters_update(const struct fw_rate_control_params *h, struct fw_rate_control_params *p); + +static int parameters_init(struct fw_rate_control_params *h) +{ + /* PID parameters */ + h->yawrate_p = param_find("MC_YAWRATE_P"); //TODO define rate params for fixed wing + h->yawrate_i = param_find("MC_YAWRATE_I"); + h->yawrate_d = param_find("MC_YAWRATE_D"); + h->yawrate_awu = param_find("MC_YAWRATE_AWU"); + h->yawrate_lim = param_find("MC_YAWRATE_LIM"); + + h->attrate_p = param_find("MC_ATTRATE_P"); + h->attrate_i = param_find("MC_ATTRATE_I"); + h->attrate_d = param_find("MC_ATTRATE_D"); + h->attrate_awu = param_find("MC_ATTRATE_AWU"); + h->attrate_lim = param_find("MC_ATTRATE_LIM"); + + return OK; +} + +static int parameters_update(const struct fw_rate_control_params *h, struct fw_rate_control_params *p) +{ + param_get(h->yawrate_p, &(p->yawrate_p)); + param_get(h->yawrate_i, &(p->yawrate_i)); + param_get(h->yawrate_d, &(p->yawrate_d)); + param_get(h->yawrate_awu, &(p->yawrate_awu)); + param_get(h->yawrate_lim, &(p->yawrate_lim)); + + param_get(h->attrate_p, &(p->attrate_p)); + param_get(h->attrate_i, &(p->attrate_i)); + param_get(h->attrate_d, &(p->attrate_d)); + param_get(h->attrate_awu, &(p->attrate_awu)); + param_get(h->attrate_lim, &(p->attrate_lim)); + + return OK; +} + + +int fixedwing_control2_rates(const struct vehicle_rates_setpoint_s *rate_sp, + const float rates[], + struct actuator_controls_s *actuators) +{ + static int counter = 0; + static bool initialized = false; + + static struct fw_rate_control_params p; + static struct fw_rate_control_params h; + + if(!initialized) + { + parameters_init(&h); + parameters_update(&h, &p); + initialized = true; + } + + /* load new parameters with lower rate */ + if (counter % 2500 == 0) { + /* update parameters from storage */ + 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; + + + actuators->control[1] = 0; + actuators->control[2] = 0; + + counter++; + + return 0; +} + + + diff --git a/apps/fixedwing_control2/fixedwing_control2_rate.h b/apps/fixedwing_control2/fixedwing_control2_rate.h new file mode 100644 index 000000000..39aa54abe --- /dev/null +++ b/apps/fixedwing_control2/fixedwing_control2_rate.h @@ -0,0 +1,45 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.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 Main system state machine definition */ + +#ifndef FIXEDWING_CONTROL2_RATE_H_ +#define FIXEDWING_CONTROL2_RATE_H_ + +int fixedwing_control2_rates(const struct vehicle_rates_setpoint_s *rate_sp, + const float rates[], + struct actuator_controls_s *actuators); + +#endif /* FIXEDWING_CONTROL2_RATE_H_ */ diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index be0a3d1d7..96a587131 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -78,6 +78,7 @@ 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 += position_estimator CONFIGURED_APPS += attitude_estimator_ekf CONFIGURED_APPS += px4/ground_estimator |