diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-04-27 14:42:12 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-04-27 14:42:12 +0200 |
commit | 7ca82801bd4d4b9d6f4a0ac515e5cfcdc28f2e05 (patch) | |
tree | 8168592010ae6fac2cdc9ce26d224566a3ce0702 /src/modules | |
parent | 988bf1eb0a3d36532883734a416f9c9e1e6ba125 (diff) | |
download | px4-firmware-7ca82801bd4d4b9d6f4a0ac515e5cfcdc28f2e05.tar.gz px4-firmware-7ca82801bd4d4b9d6f4a0ac515e5cfcdc28f2e05.tar.bz2 px4-firmware-7ca82801bd4d4b9d6f4a0ac515e5cfcdc28f2e05.zip |
Moved multirotor controllers
Diffstat (limited to 'src/modules')
15 files changed, 2278 insertions, 0 deletions
diff --git a/src/modules/multirotor_att_control/module.mk b/src/modules/multirotor_att_control/module.mk new file mode 100755 index 000000000..2fd52c162 --- /dev/null +++ b/src/modules/multirotor_att_control/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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. +# +############################################################################ + +# +# Makefile to build the multirotor attitude controller +# + +MODULE_COMMAND = multirotor_att_control + +SRCS = multirotor_att_control_main.c \ + multirotor_attitude_control.c \ + multirotor_rate_control.c diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c new file mode 100644 index 000000000..d94c0a69c --- /dev/null +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -0,0 +1,485 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.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 multirotor_att_control_main.c + * + * Implementation of multirotor attitude control main loop. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> +#include <getopt.h> +#include <time.h> +#include <math.h> +#include <poll.h> +#include <sys/prctl.h> +#include <drivers/drv_hrt.h> +#include <uORB/uORB.h> +#include <drivers/drv_gyro.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/offboard_control_setpoint.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/parameter_update.h> + +#include <systemlib/perf_counter.h> +#include <systemlib/systemlib.h> +#include <systemlib/param/param.h> + +#include "multirotor_attitude_control.h" +#include "multirotor_rate_control.h" + +PARAM_DEFINE_FLOAT(MC_RCLOSS_THR, 0.0f); // This defines the throttle when the RC signal is lost. + +__EXPORT int multirotor_att_control_main(int argc, char *argv[]); + +static bool thread_should_exit; +static int mc_task; +static bool motor_test_mode = false; + +static orb_advert_t actuator_pub; + +static struct vehicle_status_s state; + +static int +mc_thread_main(int argc, char *argv[]) +{ + /* declare and safely initialize all structs */ + memset(&state, 0, sizeof(state)); + 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 manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + struct offboard_control_setpoint_s offboard_sp; + memset(&offboard_sp, 0, sizeof(offboard_sp)); + struct vehicle_rates_setpoint_s rates_sp; + memset(&rates_sp, 0, sizeof(rates_sp)); + + struct actuator_controls_s actuators; + memset(&actuators, 0, sizeof(actuators)); + + /* subscribe to attitude, motor setpoints and system state */ + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int param_sub = orb_subscribe(ORB_ID(parameter_update)); + int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + + /* + * Do not rate-limit the loop to prevent aliasing + * if rate-limiting would be desired later, the line below would + * enable it. + * + * rate-limit the attitude subscription to 200Hz to pace our loop + * orb_set_interval(att_sub, 5); + */ + struct pollfd fds[2] = { + { .fd = att_sub, .events = POLLIN }, + { .fd = param_sub, .events = POLLIN } + }; + + /* publish actuator controls */ + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { + actuators.control[i] = 0.0f; + } + + actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); + orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + + /* register the perf counter */ + perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime"); + perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval"); + perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err"); + + /* welcome user */ + printf("[multirotor_att_control] starting\n"); + + /* store last control mode to detect mode switches */ + bool flag_control_manual_enabled = false; + bool flag_control_attitude_enabled = false; + bool flag_system_armed = false; + + /* store if yaw position or yaw speed has been changed */ + bool control_yaw_position = true; + + /* store if we stopped a yaw movement */ + bool first_time_after_yaw_speed_control = true; + + /* prepare the handle for the failsafe throttle */ + param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); + float failsafe_throttle = 0.0f; + + + while (!thread_should_exit) { + + /* wait for a sensor update, check for exit condition every 500 ms */ + int ret = poll(fds, 2, 500); + + if (ret < 0) { + /* poll error, count it in perf */ + perf_count(mc_err_perf); + + } else if (ret == 0) { + /* no return value, ignore */ + } else { + + /* only update parameters if they changed */ + if (fds[1].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 */ + // XXX no params here yet + } + + /* only run controller if attitude changed */ + if (fds[0].revents & POLLIN) { + + perf_begin(mc_loop_perf); + + /* get a local copy of system state */ + bool updated; + orb_check(state_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + } + + /* get a local copy of manual setpoint */ + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); + /* get a local copy of attitude */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + /* get a local copy of attitude setpoint */ + orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); + /* get a local copy of rates setpoint */ + orb_check(setpoint_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp); + } + + /* get a local copy of the current sensor values */ + orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + + + /** STEP 1: Define which input is the dominating control input */ + if (state.flag_control_offboard_enabled) { + /* offboard inputs */ + if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { + rates_sp.roll = offboard_sp.p1; + rates_sp.pitch = offboard_sp.p2; + rates_sp.yaw = offboard_sp.p3; + rates_sp.thrust = offboard_sp.p4; +// printf("thrust_rate=%8.4f\n",offboard_sp.p4); + rates_sp.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); + + } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { + att_sp.roll_body = offboard_sp.p1; + att_sp.pitch_body = offboard_sp.p2; + att_sp.yaw_body = offboard_sp.p3; + att_sp.thrust = offboard_sp.p4; +// printf("thrust_att=%8.4f\n",offboard_sp.p4); + att_sp.timestamp = hrt_absolute_time(); + /* STEP 2: publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + + + } else if (state.flag_control_manual_enabled) { + + if (state.flag_control_attitude_enabled) { + + /* initialize to current yaw if switching to manual or att control */ + if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || + state.flag_control_manual_enabled != flag_control_manual_enabled || + state.flag_system_armed != flag_system_armed) { + att_sp.yaw_body = att.yaw; + } + + static bool rc_loss_first_time = true; + + /* if the RC signal is lost, try to stay level and go slowly back down to ground */ + if (state.rc_signal_lost) { + /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ + param_get(failsafe_throttle_handle, &failsafe_throttle); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + + /* + * Only go to failsafe throttle if last known throttle was + * high enough to create some lift to make hovering state likely. + * + * This is to prevent that someone landing, but not disarming his + * multicopter (throttle = 0) does not make it jump up in the air + * if shutting down his remote. + */ + if (isfinite(manual.throttle) && manual.throttle > 0.2f) { + att_sp.thrust = failsafe_throttle; + + } else { + att_sp.thrust = 0.0f; + } + + /* keep current yaw, do not attempt to go to north orientation, + * since if the pilot regains RC control, he will be lost regarding + * the current orientation. + */ + if (rc_loss_first_time) + att_sp.yaw_body = att.yaw; + + rc_loss_first_time = false; + + } else { + rc_loss_first_time = true; + + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; + + /* set attitude if arming */ + if (!flag_control_attitude_enabled && state.flag_system_armed) { + att_sp.yaw_body = att.yaw; + } + + /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ + if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + + if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + + } else { + /* + * This mode SHOULD be the default mode, which is: + * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS + * + * However, we fall back to this setting for all other (nonsense) + * settings as well. + */ + + /* only move setpoint if manual input is != 0 */ + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + first_time_after_yaw_speed_control = true; + + } else { + if (first_time_after_yaw_speed_control) { + att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; + } + + control_yaw_position = true; + } + } + } + + att_sp.thrust = manual.throttle; + att_sp.timestamp = hrt_absolute_time(); + } + + /* STEP 2: publish the controller output */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + + if (motor_test_mode) { + printf("testmode"); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.1f; + att_sp.timestamp = hrt_absolute_time(); + /* STEP 2: publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + + } else { + /* manual rate inputs, from RC control or joystick */ + if (state.flag_control_rates_enabled && + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { + rates_sp.roll = manual.roll; + + rates_sp.pitch = manual.pitch; + rates_sp.yaw = manual.yaw; + rates_sp.thrust = manual.throttle; + rates_sp.timestamp = hrt_absolute_time(); + } + } + + } + + /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ + if (state.flag_control_attitude_enabled) { + multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position); + + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); + } + + /* measure in what intervals the controller runs */ + perf_count(mc_interval_perf); + + float gyro[3]; + + /* get current rate setpoint */ + bool rates_sp_valid = false; + orb_check(rates_sp_sub, &rates_sp_valid); + + if (rates_sp_valid) { + orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); + } + + /* apply controller */ + gyro[0] = att.rollspeed; + gyro[1] = att.pitchspeed; + gyro[2] = att.yawspeed; + + multirotor_control_rates(&rates_sp, gyro, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + + /* update state */ + flag_control_attitude_enabled = state.flag_control_attitude_enabled; + flag_control_manual_enabled = state.flag_control_manual_enabled; + flag_system_armed = state.flag_system_armed; + + perf_end(mc_loop_perf); + } /* end of poll call for attitude updates */ + } /* end of poll return value check */ + } + + printf("[multirotor att control] stopping, disarming motors.\n"); + + /* 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(state_sub); + close(manual_sub); + close(actuator_pub); + close(att_sp_pub); + + perf_print_counter(mc_loop_perf); + perf_free(mc_loop_perf); + + fflush(stdout); + exit(0); +} + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: multirotor_att_control [-m <mode>] [-t] {start|status|stop}\n"); + fprintf(stderr, " <mode> is 'rates' or 'attitude'\n"); + fprintf(stderr, " -t enables motor test mode with 10%% thrust\n"); + exit(1); +} + +int multirotor_att_control_main(int argc, char *argv[]) +{ + int ch; + unsigned int optioncount = 0; + + while ((ch = getopt(argc, argv, "tm:")) != EOF) { + switch (ch) { + case 't': + motor_test_mode = true; + optioncount += 1; + break; + + case ':': + usage("missing parameter"); + break; + + default: + fprintf(stderr, "option: -%c\n", ch); + usage("unrecognized option"); + break; + } + } + + argc -= optioncount; + //argv += optioncount; + + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1 + optioncount], "start")) { + + thread_should_exit = false; + mc_task = task_spawn("multirotor_att_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 15, + 2048, + mc_thread_main, + NULL); + exit(0); + } + + if (!strcmp(argv[1 + optioncount], "stop")) { + thread_should_exit = true; + exit(0); + } + + usage("unrecognized command"); + exit(1); +} diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c new file mode 100644 index 000000000..76dbb36d3 --- /dev/null +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -0,0 +1,249 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Lorenz Meier <lm@inf.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 multirotor_attitude_control.c + * Implementation of attitude controller + */ + +#include "multirotor_attitude_control.h" +#include <stdio.h> +#include <stdlib.h> +#include <stdio.h> +#include <stdint.h> +#include <stdbool.h> +#include <float.h> +#include <math.h> +#include <systemlib/pid/pid.h> +#include <systemlib/param/param.h> +#include <drivers/drv_hrt.h> + +PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f); +PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); +PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); +//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); +//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); + +PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); +PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f); +//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); +//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); + +//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f); +//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f); + +struct mc_att_control_params { + float yaw_p; + float yaw_i; + float yaw_d; + //float yaw_awu; + //float yaw_lim; + + float att_p; + float att_i; + float att_d; + //float att_awu; + //float att_lim; + + //float att_xoff; + //float att_yoff; +}; + +struct mc_att_control_param_handles { + param_t yaw_p; + param_t yaw_i; + param_t yaw_d; + //param_t yaw_awu; + //param_t yaw_lim; + + param_t att_p; + param_t att_i; + param_t att_d; + //param_t att_awu; + //param_t att_lim; + + //param_t att_xoff; + //param_t att_yoff; +}; + +/** + * Initialize all parameter handles and values + * + */ +static int parameters_init(struct mc_att_control_param_handles *h); + +/** + * Update all parameters + * + */ +static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p); + + +static int parameters_init(struct mc_att_control_param_handles *h) +{ + /* PID parameters */ + h->yaw_p = param_find("MC_YAWPOS_P"); + h->yaw_i = param_find("MC_YAWPOS_I"); + h->yaw_d = param_find("MC_YAWPOS_D"); + //h->yaw_awu = param_find("MC_YAWPOS_AWU"); + //h->yaw_lim = param_find("MC_YAWPOS_LIM"); + + h->att_p = param_find("MC_ATT_P"); + h->att_i = param_find("MC_ATT_I"); + h->att_d = param_find("MC_ATT_D"); + //h->att_awu = param_find("MC_ATT_AWU"); + //h->att_lim = param_find("MC_ATT_LIM"); + + //h->att_xoff = param_find("MC_ATT_XOFF"); + //h->att_yoff = param_find("MC_ATT_YOFF"); + + return OK; +} + +static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p) +{ + param_get(h->yaw_p, &(p->yaw_p)); + param_get(h->yaw_i, &(p->yaw_i)); + param_get(h->yaw_d, &(p->yaw_d)); + //param_get(h->yaw_awu, &(p->yaw_awu)); + //param_get(h->yaw_lim, &(p->yaw_lim)); + + param_get(h->att_p, &(p->att_p)); + param_get(h->att_i, &(p->att_i)); + param_get(h->att_d, &(p->att_d)); + //param_get(h->att_awu, &(p->att_awu)); + //param_get(h->att_lim, &(p->att_lim)); + + //param_get(h->att_xoff, &(p->att_xoff)); + //param_get(h->att_yoff, &(p->att_yoff)); + + return OK; +} + +void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position) +{ + static uint64_t last_run = 0; + static uint64_t last_input = 0; + float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; + last_run = hrt_absolute_time(); + + if (last_input != att_sp->timestamp) { + last_input = att_sp->timestamp; + } + + static int sensor_delay; + sensor_delay = hrt_absolute_time() - att->timestamp; + + static int motor_skip_counter = 0; + + static PID_t pitch_controller; + static PID_t roll_controller; + + static struct mc_att_control_params p; + static struct mc_att_control_param_handles h; + + static bool initialized = false; + + static float yaw_error; + + /* initialize the pid controllers when the function is called for the first time */ + if (initialized == false) { + parameters_init(&h); + parameters_update(&h, &p); + + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, + 1000.0f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, + 1000.0f, PID_MODE_DERIVATIV_SET); + + initialized = true; + } + + /* load new parameters with lower rate */ + if (motor_skip_counter % 500 == 0) { + /* update parameters from storage */ + parameters_update(&h, &p); + + /* apply parameters */ + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); + } + + /* reset integral if on ground */ + if (att_sp->thrust < 0.1f) { + pid_reset_integral(&pitch_controller); + pid_reset_integral(&roll_controller); + } + + + /* calculate current control outputs */ + + /* control pitch (forward) output */ + rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , + att->pitch, att->pitchspeed, deltaT); + + /* control roll (left/right) output */ + rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , + att->roll, att->rollspeed, deltaT); + + if (control_yaw_position) { + /* control yaw rate */ + + /* positive error: rotate to right, negative error, rotate to left (NED frame) */ + // yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw); + + yaw_error = att_sp->yaw_body - att->yaw; + + if (yaw_error > M_PI_F) { + yaw_error -= M_TWOPI_F; + + } else if (yaw_error < -M_PI_F) { + yaw_error += M_TWOPI_F; + } + + rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed); + } + + rates_sp->thrust = att_sp->thrust; + + motor_skip_counter++; +} diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h new file mode 100644 index 000000000..2cf83e443 --- /dev/null +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.h @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Lorenz Meier <lm@inf.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 multirotor_attitude_control.h + * Attitude control for multi rotors. + */ + +#ifndef MULTIROTOR_ATTITUDE_CONTROL_H_ +#define MULTIROTOR_ATTITUDE_CONTROL_H_ + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/actuator_controls.h> + +void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position); + +#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c new file mode 100644 index 000000000..deba1ac03 --- /dev/null +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -0,0 +1,230 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli <naegelit@student.ethz.ch> + * Lorenz Meier <lm@inf.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 multirotor_rate_control.c + * + * Implementation of rate controller + * + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include "multirotor_rate_control.h" +#include <stdio.h> +#include <stdlib.h> +#include <stdio.h> +#include <stdint.h> +#include <stdbool.h> +#include <float.h> +#include <math.h> +#include <systemlib/pid/pid.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <drivers/drv_hrt.h> + +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +//PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f); +//PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f); + +PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.0f); /* 0.15 F405 Flamewheel */ +PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); +//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f); +//PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */ + +struct mc_rate_control_params { + + float yawrate_p; + float yawrate_d; + float yawrate_i; + //float yawrate_awu; + //float yawrate_lim; + + float attrate_p; + float attrate_d; + float attrate_i; + //float attrate_awu; + //float attrate_lim; + + float rate_lim; +}; + +struct mc_rate_control_param_handles { + + 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; +}; + +/** + * Initialize all parameter handles and values + * + */ +static int parameters_init(struct mc_rate_control_param_handles *h); + +/** + * Update all parameters + * + */ +static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p); + + +static int parameters_init(struct mc_rate_control_param_handles *h) +{ + /* PID parameters */ + h->yawrate_p = param_find("MC_YAWRATE_P"); + 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 mc_rate_control_param_handles *h, struct mc_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; +} + +void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, + const float rates[], struct actuator_controls_s *actuators) +{ + static float roll_control_last = 0; + static float pitch_control_last = 0; + static uint64_t last_run = 0; + const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + static uint64_t last_input = 0; + + float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; + + if (last_input != rate_sp->timestamp) { + last_input = rate_sp->timestamp; + } + + last_run = hrt_absolute_time(); + + static int motor_skip_counter = 0; + + static struct mc_rate_control_params p; + static struct mc_rate_control_param_handles h; + + static bool initialized = false; + + /* initialize the pid controllers when the function is called for the first time */ + if (initialized == false) { + parameters_init(&h); + parameters_update(&h, &p); + initialized = true; + } + + /* load new parameters with lower rate */ + if (motor_skip_counter % 2500 == 0) { + /* update parameters from storage */ + parameters_update(&h, &p); + // warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz", + // (double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input)); + } + + /* calculate current control outputs */ + + /* control pitch (forward) output */ + float pitch_control = p.attrate_p * (rate_sp->pitch - rates[1]) - (p.attrate_d * pitch_control_last); + + /* increase resilience to faulty control inputs */ + if (isfinite(pitch_control)) { + pitch_control_last = pitch_control; + + } else { + pitch_control = 0.0f; + warnx("rej. NaN ctrl pitch"); + } + + /* control roll (left/right) output */ + float roll_control = p.attrate_p * (rate_sp->roll - rates[0]) - (p.attrate_d * roll_control_last); + + /* increase resilience to faulty control inputs */ + if (isfinite(roll_control)) { + roll_control_last = roll_control; + + } else { + roll_control = 0.0f; + warnx("rej. NaN ctrl roll"); + } + + /* control yaw rate */ + float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); + + /* increase resilience to faulty control inputs */ + if (!isfinite(yaw_rate_control)) { + yaw_rate_control = 0.0f; + warnx("rej. NaN ctrl yaw"); + } + + actuators->control[0] = roll_control; + actuators->control[1] = pitch_control; + actuators->control[2] = yaw_rate_control; + actuators->control[3] = rate_sp->thrust; + + motor_skip_counter++; +} diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h new file mode 100644 index 000000000..03dec317a --- /dev/null +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Lorenz Meier <lm@inf.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 multirotor_attitude_control.h + * Attitude control for multi rotors. + */ + +#ifndef MULTIROTOR_RATE_CONTROL_H_ +#define MULTIROTOR_RATE_CONTROL_H_ + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/actuator_controls.h> + +void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, + const float rates[], struct actuator_controls_s *actuators); + +#endif /* MULTIROTOR_RATE_CONTROL_H_ */ diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/multirotor_pos_control/module.mk new file mode 100644 index 000000000..d04847745 --- /dev/null +++ b/src/modules/multirotor_pos_control/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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. +# +############################################################################ + +# +# Build multirotor position control +# + +MODULE_COMMAND = multirotor_pos_control + +SRCS = multirotor_pos_control.c \ + multirotor_pos_control_params.c diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c new file mode 100644 index 000000000..7b8d83aa8 --- /dev/null +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -0,0 +1,238 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.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 multirotor_pos_control.c + * + * Skeleton for multirotor position controller + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> +#include <termios.h> +#include <time.h> +#include <sys/prctl.h> +#include <drivers/drv_hrt.h> +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_local_position_setpoint.h> +#include <uORB/topics/vehicle_vicon_position.h> +#include <systemlib/systemlib.h> + +#include "multirotor_pos_control_params.h" + + +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 */ + +__EXPORT int multirotor_pos_control_main(int argc, char *argv[]); + +/** + * Mainloop of position controller. + */ +static int multirotor_pos_control_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\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_spawn(). + */ +int multirotor_pos_control_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("multirotor pos control already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn("multirotor pos control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 60, + 4096, + multirotor_pos_control_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tmultirotor pos control app is running\n"); + } else { + printf("\tmultirotor pos control app not started\n"); + } + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +static int +multirotor_pos_control_thread_main(int argc, char *argv[]) +{ + /* welcome user */ + printf("[multirotor pos control] Control started, taking over position control\n"); + + /* structures */ + struct vehicle_status_s state; + struct vehicle_attitude_s att; + //struct vehicle_global_position_setpoint_s global_pos_sp; + struct vehicle_local_position_setpoint_s local_pos_sp; + struct vehicle_vicon_position_s local_pos; + struct manual_control_setpoint_s manual; + struct vehicle_attitude_setpoint_s att_sp; + + /* subscribe to attitude, motor setpoints and system state */ + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); + //int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + + /* publish attitude setpoint */ + orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + + thread_running = true; + + int loopcounter = 0; + + struct multirotor_position_control_params p; + struct multirotor_position_control_param_handles h; + parameters_init(&h); + parameters_update(&h, &p); + + + while (1) { + /* get a local copy of the vehicle state */ + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + /* get a local copy of manual setpoint */ + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); + /* get a local copy of attitude */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + /* get a local copy of local position */ + orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos); + /* get a local copy of local position setpoint */ + orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); + + if (loopcounter == 500) { + parameters_update(&h, &p); + loopcounter = 0; + } + + // if (state.state_machine == SYSTEM_STATE_AUTO) { + + // XXX IMPLEMENT POSITION CONTROL HERE + + float dT = 1.0f / 50.0f; + + float x_setpoint = 0.0f; + + // XXX enable switching between Vicon and local position estimate + /* local pos is the Vicon position */ + + // XXX just an example, lacks rotation around world-body transformation + att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p; + att_sp.roll_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.3f; + att_sp.timestamp = hrt_absolute_time(); + + /* publish new attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + // } else if (state.state_machine == SYSTEM_STATE_STABILIZED) { + /* set setpoint to current position */ + // XXX select pos reset channel on remote + /* reset setpoint to current position (position hold) */ + // if (1 == 2) { + // local_pos_sp.x = local_pos.x; + // local_pos_sp.y = local_pos.y; + // local_pos_sp.z = local_pos.z; + // local_pos_sp.yaw = att.yaw; + // } + // } + + /* run at approximately 50 Hz */ + usleep(20000); + loopcounter++; + + } + + printf("[multirotor pos control] ending now...\n"); + + thread_running = false; + + fflush(stdout); + return 0; +} + diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c new file mode 100644 index 000000000..6b73dc405 --- /dev/null +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli <naegelit@student.ethz.ch> + * Lorenz Meier <lm@inf.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 multirotor_position_control_params.c + * + * Parameters for EKF filter + */ + +#include "multirotor_pos_control_params.h" + +/* Extended Kalman Filter covariances */ + +/* controller parameters */ +PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f); + +int parameters_init(struct multirotor_position_control_param_handles *h) +{ + /* PID parameters */ + h->p = param_find("MC_POS_P"); + + return OK; +} + +int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p) +{ + param_get(h->p, &(p->p)); + + return OK; +} diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h new file mode 100644 index 000000000..274f6c22a --- /dev/null +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli <naegelit@student.ethz.ch> + * Lorenz Meier <lm@inf.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 multirotor_position_control_params.h + * + * Parameters for position controller + */ + +#include <systemlib/param/param.h> + +struct multirotor_position_control_params { + float p; + float i; + float d; +}; + +struct multirotor_position_control_param_handles { + param_t p; + param_t i; + param_t d; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct multirotor_position_control_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p); diff --git a/src/modules/multirotor_pos_control/position_control.c b/src/modules/multirotor_pos_control/position_control.c new file mode 100644 index 000000000..9c53a5bf6 --- /dev/null +++ b/src/modules/multirotor_pos_control/position_control.c @@ -0,0 +1,235 @@ +// /**************************************************************************** +// * +// * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. +// * Author: @author Lorenz Meier <lm@inf.ethz.ch> +// * @author Laurens Mackay <mackayl@student.ethz.ch> +// * @author Tobias Naegeli <naegelit@student.ethz.ch> +// * @author Martin Rutschmann <rutmarti@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 multirotor_position_control.c +// * Implementation of the position control for a multirotor VTOL +// */ + +// #include <stdio.h> +// #include <stdlib.h> +// #include <stdio.h> +// #include <stdint.h> +// #include <math.h> +// #include <stdbool.h> +// #include <float.h> +// #include <systemlib/pid/pid.h> + +// #include "multirotor_position_control.h" + +// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, +// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, +// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp) +// { +// static PID_t distance_controller; + +// static int read_ret; +// static global_data_position_t position_estimated; + +// static uint16_t counter; + +// static bool initialized; +// static uint16_t pm_counter; + +// static float lat_next; +// static float lon_next; + +// static float pitch_current; + +// static float thrust_total; + + +// if (initialized == false) { + +// pid_init(&distance_controller, +// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], +// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], +// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], +// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU], +// PID_MODE_DERIVATIV_CALC, 150);//150 + +// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; +// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; + +// thrust_total = 0.0f; + +// /* Position initialization */ +// /* Wait for new position estimate */ +// do { +// read_ret = read_lock_position(&position_estimated); +// } while (read_ret != 0); + +// lat_next = position_estimated.lat; +// lon_next = position_estimated.lon; + +// /* attitude initialization */ +// global_data_lock(&global_data_attitude->access_conf); +// pitch_current = global_data_attitude->pitch; +// global_data_unlock(&global_data_attitude->access_conf); + +// initialized = true; +// } + +// /* load new parameters with 10Hz */ +// if (counter % 50 == 0) { +// if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) { +// /* check whether new parameters are available */ +// if (global_data_parameter_storage->counter > pm_counter) { +// pid_set_parameters(&distance_controller, +// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], +// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], +// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], +// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]); + +// // +// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; +// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; + +// pm_counter = global_data_parameter_storage->counter; +// printf("Position controller changed pid parameters\n"); +// } +// } + +// global_data_unlock(&global_data_parameter_storage->access_conf); +// } + + +// /* Wait for new position estimate */ +// do { +// read_ret = read_lock_position(&position_estimated); +// } while (read_ret != 0); + +// /* Get next waypoint */ //TODO: add local copy + +// if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) { +// lat_next = global_data_position_setpoint->x; +// lon_next = global_data_position_setpoint->y; +// global_data_unlock(&global_data_position_setpoint->access_conf); +// } + +// /* Get distance to waypoint */ +// float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next); +// // if(counter % 5 == 0) +// // printf("distance_to_waypoint: %.4f\n", distance_to_waypoint); + +// /* Get bearing to waypoint (direction on earth surface to next waypoint) */ +// float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next); + +// if (counter % 5 == 0) +// printf("bearing: %.4f\n", bearing); + +// /* Calculate speed in direction of bearing (needed for controller) */ +// float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy); +// // if(counter % 5 == 0) +// // printf("speed_norm: %.4f\n", speed_norm); +// float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller + +// /* Control Thrust in bearing direction */ +// float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint, +// CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else + +// // if(counter % 5 == 0) +// // printf("horizontal thrust: %.4f\n", horizontal_thrust); + +// /* Get total thrust (from remote for now) */ +// if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { +// thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum? +// global_data_unlock(&global_data_rc_channels->access_conf); +// } + +// const float max_gas = 500.0f; +// thrust_total *= max_gas / 20000.0f; //TODO: check this +// thrust_total += max_gas / 2.0f; + + +// if (horizontal_thrust > thrust_total) { +// horizontal_thrust = thrust_total; + +// } else if (horizontal_thrust < -thrust_total) { +// horizontal_thrust = -thrust_total; +// } + + + +// //TODO: maybe we want to add a speed controller later... + +// /* Calclulate thrust in east and north direction */ +// float thrust_north = cosf(bearing) * horizontal_thrust; +// float thrust_east = sinf(bearing) * horizontal_thrust; + +// if (counter % 10 == 0) { +// printf("thrust north: %.4f\n", thrust_north); +// printf("thrust east: %.4f\n", thrust_east); +// fflush(stdout); +// } + +// /* Get current attitude */ +// if (0 == global_data_trylock(&global_data_attitude->access_conf)) { +// pitch_current = global_data_attitude->pitch; +// global_data_unlock(&global_data_attitude->access_conf); +// } + +// /* Get desired pitch & roll */ +// float pitch_desired = 0.0f; +// float roll_desired = 0.0f; + +// if (thrust_total != 0) { +// float pitch_fraction = -thrust_north / thrust_total; +// float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total); + +// if (roll_fraction < -1) { +// roll_fraction = -1; + +// } else if (roll_fraction > 1) { +// roll_fraction = 1; +// } + +// // if(counter % 5 == 0) +// // { +// // printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction); +// // fflush(stdout); +// // } + +// pitch_desired = asinf(pitch_fraction); +// roll_desired = asinf(roll_fraction); +// } + +// att_sp.roll = roll_desired; +// att_sp.pitch = pitch_desired; + +// counter++; +// } diff --git a/src/modules/multirotor_pos_control/position_control.h b/src/modules/multirotor_pos_control/position_control.h new file mode 100644 index 000000000..2144ebc34 --- /dev/null +++ b/src/modules/multirotor_pos_control/position_control.h @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@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 multirotor_position_control.h + * Definition of the position control for a multirotor VTOL + */ + +// #ifndef POSITION_CONTROL_H_ +// #define POSITION_CONTROL_H_ + +// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, +// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, +// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp); + +// #endif /* POSITION_CONTROL_H_ */ diff --git a/src/modules/position_estimator/.context b/src/modules/position_estimator/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/src/modules/position_estimator/.context diff --git a/src/modules/position_estimator/module.mk b/src/modules/position_estimator/module.mk new file mode 100644 index 000000000..f64095d9d --- /dev/null +++ b/src/modules/position_estimator/module.mk @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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. +# +############################################################################ + +# +# Makefile to build the position estimator +# + +MODULE_COMMAND = position_estimator + +# XXX this should be converted to a deamon, its a pretty bad example app +MODULE_PRIORITY = SCHED_PRIORITY_DEFAULT +MODULE_STACKSIZE = 4096 + +SRCS = position_estimator_main.c diff --git a/src/modules/position_estimator/position_estimator_main.c b/src/modules/position_estimator/position_estimator_main.c new file mode 100644 index 000000000..e84945299 --- /dev/null +++ b/src/modules/position_estimator/position_estimator_main.c @@ -0,0 +1,423 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli <naegelit@student.ethz.ch> + * Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * Lorenz Meier <lm@inf.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 position_estimator_main.c + * Model-identification based position estimator for multirotors + */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdlib.h> +#include <stdio.h> +#include <stdbool.h> +#include <fcntl.h> +#include <float.h> +#include <nuttx/sched.h> +#include <sys/prctl.h> +#include <termios.h> +#include <errno.h> +#include <limits.h> +#include <math.h> +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_local_position.h> +#include <poll.h> + +#define N_STATES 6 +#define ERROR_COVARIANCE_INIT 3 +#define R_EARTH 6371000.0 + +#define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000 +#define REPROJECTION_COUNTER_LIMIT 125 + +__EXPORT int position_estimator_main(int argc, char *argv[]); + +static uint16_t position_estimator_counter_position_information; + +/* values for map projection */ +static double phi_1; +static double sin_phi_1; +static double cos_phi_1; +static double lambda_0; +static double scale; + +/** + * Initializes the map transformation. + * + * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +static void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + /* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ + phi_1 = lat_0 / 180.0 * M_PI; + lambda_0 = lon_0 / 180.0 * M_PI; + + sin_phi_1 = sin(phi_1); + cos_phi_1 = cos(phi_1); + + /* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale + + /* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */ + const double r_earth = 6371000; + + double lat1 = phi_1; + double lon1 = lambda_0; + + double lat2 = phi_1 + 0.5 / 180 * M_PI; + double lon2 = lambda_0 + 0.5 / 180 * M_PI; + double sin_lat_2 = sin(lat2); + double cos_lat_2 = cos(lat2); + double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth; + + /* 2) calculate distance rho on plane */ + double k_bar = 0; + double c = acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)); + + if (0 != c) + k_bar = c / sin(c); + + double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane + double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0))); + double rho = sqrt(pow(x2, 2) + pow(y2, 2)); + + scale = d / rho; + +} + +/** + * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +static void map_projection_project(double lat, double lon, float *x, float *y) +{ + /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ + double phi = lat / 180.0 * M_PI; + double lambda = lon / 180.0 * M_PI; + + double sin_phi = sin(phi); + double cos_phi = cos(phi); + + double k_bar = 0; + /* using small angle approximation (formula in comment is without aproximation) */ + double c = acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c = acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) ); + + if (0 != c) + k_bar = c / sin(c); + + /* using small angle approximation (formula in comment is without aproximation) */ + *y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale; + *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; // *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale; + +// printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0); +} + +/** + * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system + * + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +static void map_projection_reproject(float x, float y, double *lat, double *lon) +{ + /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ + + double x_descaled = x / scale; + double y_descaled = y / scale; + + double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2)); + double sin_c = sin(c); + double cos_c = cos(c); + + double lat_sphere = 0; + + if (c != 0) + lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c); + else + lat_sphere = asin(cos_c * sin_phi_1); + +// printf("lat_sphere = %.10f\n",lat_sphere); + + double lon_sphere = 0; + + if (phi_1 == M_PI / 2) { + //using small angle approximation (formula in comment is without aproximation) + lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled)); + + } else if (phi_1 == -M_PI / 2) { + //using small angle approximation (formula in comment is without aproximation) + lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled)); + + } else { + + lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c)); + //using small angle approximation +// double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c); +// if(denominator != 0) +// { +// lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator); +// } +// else +// { +// ... +// } + } + +// printf("lon_sphere = %.10f\n",lon_sphere); + + *lat = lat_sphere * 180.0 / M_PI; + *lon = lon_sphere * 180.0 / M_PI; + +} + +/**************************************************************************** + * main + ****************************************************************************/ + +int position_estimator_main(int argc, char *argv[]) +{ + + /* welcome user */ + printf("[multirotor position_estimator] started\n"); + + /* initialize values */ + static float u[2] = {0, 0}; + static float z[3] = {0, 0, 0}; + static float xapo[N_STATES] = {0, 0, 0, 0, 0, 0}; + static float Papo[N_STATES * N_STATES] = {ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, + ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, + ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, + ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, + ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, + ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0 + }; + + static float xapo1[N_STATES]; + static float Papo1[36]; + + static float gps_covariance[3] = {0.0f, 0.0f, 0.0f}; + + static uint16_t counter = 0; + position_estimator_counter_position_information = 0; + + uint8_t predict_only = 1; + + bool gps_valid = false; + + bool new_initialization = true; + + static double lat_current = 0.0d;//[°]] --> 47.0 + static double lon_current = 0.0d; //[°]] -->8.5 + float alt_current = 0.0f; + + + //TODO: handle flight without gps but with estimator + + /* subscribe to vehicle status, attitude, gps */ + struct vehicle_gps_position_s gps; + gps.fix_type = 0; + struct vehicle_status_s vstatus; + struct vehicle_attitude_s att; + + int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + + /* subscribe to attitude at 100 Hz */ + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + + /* wait until gps signal turns valid, only then can we initialize the projection */ + while (gps.fix_type < 3) { + struct pollfd fds[1] = { {.fd = vehicle_gps_sub, .events = POLLIN} }; + + /* wait for GPS updates, BUT READ VEHICLE STATUS (!) + * this choice is critical, since the vehicle status might not + * actually change, if this app is started after GPS lock was + * aquired. + */ + if (poll(fds, 1, 5000)) { + /* Wait for the GPS update to propagate (we have some time) */ + usleep(5000); + /* Read wether the vehicle status changed */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); + gps_valid = (gps.fix_type > 2); + } + } + + /* get gps value for first initialization */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); + lat_current = ((double)(gps.lat)) * 1e-7; + lon_current = ((double)(gps.lon)) * 1e-7; + alt_current = gps.alt * 1e-3; + + /* initialize coordinates */ + map_projection_init(lat_current, lon_current); + + /* publish global position messages only after first GPS message */ + struct vehicle_local_position_s local_pos = { + .x = 0, + .y = 0, + .z = 0 + }; + orb_advert_t local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); + + printf("[multirotor position estimator] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); + + while (1) { + + /*This runs at the rate of the sensors, if we have also a new gps update this is used in the position_estimator function */ + struct pollfd fds[1] = { {.fd = vehicle_attitude_sub, .events = POLLIN} }; + + if (poll(fds, 1, 5000) <= 0) { + /* error / timeout */ + } else { + + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + /* got attitude, updating pos as well */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + + /*copy attitude */ + u[0] = att.roll; + u[1] = att.pitch; + + /* initialize map projection with the last estimate (not at full rate) */ + if (gps.fix_type > 2) { + /* Project gps lat lon (Geographic coordinate system) to plane*/ + map_projection_project(((double)(gps.lat)) * 1e-7, ((double)(gps.lon)) * 1e-7, &(z[0]), &(z[1])); + + local_pos.x = z[0]; + local_pos.y = z[1]; + /* negative offset from initialization altitude */ + local_pos.z = alt_current - (gps.alt) * 1e-3; + + + orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos); + } + + + // gps_covariance[0] = gps.eph; //TODO: needs scaling + // gps_covariance[1] = gps.eph; + // gps_covariance[2] = gps.epv; + + // } else { + // /* we can not use the gps signal (it is of low quality) */ + // predict_only = 1; + // } + + // // predict_only = 0; //TODO: only for testing, removeme, XXX + // // z[0] = sinf(((float)counter)/180.0f*3.14159265f); //TODO: only for testing, removeme, XXX + // // usleep(100000); //TODO: only for testing, removeme, XXX + + + // /*Get new estimation (this is calculated in the plane) */ + // //TODO: if new_initialization == true: use 0,0,0, else use xapo + // if (true == new_initialization) { //TODO,XXX: uncomment! + // xapo[0] = 0; //we have a new plane initialization. the current estimate is in the center of the plane + // xapo[2] = 0; + // xapo[4] = 0; + // position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1); + + // } else { + // position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1); + // } + + + + // /* Copy values from xapo1 to xapo */ + // int i; + + // for (i = 0; i < N_STATES; i++) { + // xapo[i] = xapo1[i]; + // } + + // if ((counter % REPROJECTION_COUNTER_LIMIT == 0) || (counter % (PROJECTION_INITIALIZE_COUNTER_LIMIT - 1) == 0)) { + // /* Reproject from plane to geographic coordinate system */ + // // map_projection_reproject(xapo1[0], xapo1[2], map_scale, phi_1, lambda_0, &lat_current, &lon_current) //TODO,XXX: uncomment! + // map_projection_reproject(z[0], z[1], &lat_current, &lon_current); //do not use estimator for projection testing, removeme + // // //DEBUG + // // if(counter%500 == 0) + // // { + // // printf("phi_1: %.10f\n", phi_1); + // // printf("lambda_0: %.10f\n", lambda_0); + // // printf("lat_estimated: %.10f\n", lat_current); + // // printf("lon_estimated: %.10f\n", lon_current); + // // printf("z[0]=%.10f, z[1]=%.10f, z[2]=%f\n", z[0], z[1], z[2]); + // // fflush(stdout); + // // + // // } + + // // if(!isnan(lat_current) && !isnan(lon_current))// && !isnan(xapo1[4]) && !isnan(xapo1[1]) && !isnan(xapo1[3]) && !isnan(xapo1[5])) + // // { + // /* send out */ + + // global_pos.lat = lat_current; + // global_pos.lon = lon_current; + // global_pos.alt = xapo1[4]; + // global_pos.vx = xapo1[1]; + // global_pos.vy = xapo1[3]; + // global_pos.vz = xapo1[5]; + + /* publish current estimate */ + // orb_publish(ORB_ID(vehicle_global_position), global_pos_pub, &global_pos); + // } + // else + // { + // printf("[position estimator] ERROR: nan values, lat_current=%.4f, lon_current=%.4f, z[0]=%.4f z[1]=%.4f\n", lat_current, lon_current, z[0], z[1]); + // fflush(stdout); + // } + + // } + + counter++; + } + + } + + return 0; +} + + |