From 4d1256e3f33ed78a6310695261f947273170dd56 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Nov 2012 17:36:52 +0100 Subject: [PATCH 1/5] Removed left-over comment --- apps/multirotor_att_control/multirotor_att_control_main.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index d2de1a7877..7e390bc6ca 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -265,9 +265,6 @@ mc_thread_main(int argc, char *argv[]) if (rc_loss_first_time) att_sp.yaw_body = att.yaw; - // XXX hard-limit it to prevent ballistic mishaps - this is just supposed to - // slow a crash down, not actually keep the system in-air. - rc_loss_first_time = false; } else { From 0193d590db92b96e93de9594b7db75a2c7805de6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Nov 2012 17:37:52 +0100 Subject: [PATCH 2/5] Removed debug printf --- apps/mavlink_onboard/mavlink_receiver.c | 1 - 1 file changed, 1 deletion(-) diff --git a/apps/mavlink_onboard/mavlink_receiver.c b/apps/mavlink_onboard/mavlink_receiver.c index 87e2496d8d..0acbea6757 100644 --- a/apps/mavlink_onboard/mavlink_receiver.c +++ b/apps/mavlink_onboard/mavlink_receiver.c @@ -162,7 +162,6 @@ handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(optical_flow), flow_pub, &f); } - printf("GOT FLOW!\n"); } if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) { From b1bc5e0e461856ca8f1cc532eb2023ddbe2cb957 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Nov 2012 22:52:37 +0100 Subject: [PATCH 3/5] Improved drivers, allowed parallel use of multiple gyros --- apps/drivers/device/spi.cpp | 11 +++++++---- apps/drivers/l3gd20/l3gd20.cpp | 8 ++++---- apps/systemcmds/preflight_check/preflight_check.c | 2 +- 3 files changed, 12 insertions(+), 9 deletions(-) diff --git a/apps/drivers/device/spi.cpp b/apps/drivers/device/spi.cpp index 63c7c12aa1..8fffd60cb4 100644 --- a/apps/drivers/device/spi.cpp +++ b/apps/drivers/device/spi.cpp @@ -86,7 +86,7 @@ SPI::init() { int ret = OK; - // attach to the spi bus + /* attach to the spi bus */ if (_dev == nullptr) _dev = up_spiinitialize(_bus); @@ -96,7 +96,10 @@ SPI::init() goto out; } - // call the probe function to check whether the device is present + /* deselect device to ensure high to low transition of pin select */ + SPI_SELECT(_dev, _device, false); + + /* call the probe function to check whether the device is present */ ret = probe(); if (ret != OK) { @@ -104,7 +107,7 @@ SPI::init() goto out; } - // do base class init, which will create the device node, etc. + /* do base class init, which will create the device node, etc. */ ret = CDev::init(); if (ret != OK) { @@ -112,7 +115,7 @@ SPI::init() goto out; } - // tell the workd where we are + /* tell the workd where we are */ log("on SPI bus %d at %d", _bus, _device); out: diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp index 4915b81e38..f2f585f42f 100644 --- a/apps/drivers/l3gd20/l3gd20.cpp +++ b/apps/drivers/l3gd20/l3gd20.cpp @@ -151,7 +151,7 @@ extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } class L3GD20 : public device::SPI { public: - L3GD20(int bus, spi_dev_e device); + L3GD20(int bus, const char* path, spi_dev_e device); ~L3GD20(); virtual int init(); @@ -265,8 +265,8 @@ private: #define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) -L3GD20::L3GD20(int bus, spi_dev_e device) : - SPI("L3GD20", GYRO_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000), +L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : + SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000), _call_interval(0), _num_reports(0), _next_report(0), @@ -745,7 +745,7 @@ start() errx(1, "already started"); /* create the driver */ - g_dev = new L3GD20(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_GYRO); + g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); if (g_dev == nullptr) goto fail; diff --git a/apps/systemcmds/preflight_check/preflight_check.c b/apps/systemcmds/preflight_check/preflight_check.c index 391eea3a8c..f25f812aed 100644 --- a/apps/systemcmds/preflight_check/preflight_check.c +++ b/apps/systemcmds/preflight_check/preflight_check.c @@ -145,7 +145,7 @@ system_eval: led_toggle(leds, LED_BLUE); /* display and sound error */ - for (int i = 0; i < 200; i++) + for (int i = 0; i < 150; i++) { led_toggle(leds, LED_BLUE); led_toggle(leds, LED_AMBER); From 9f35de51a60fcbe207974be43648aa523926f115 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Nov 2012 22:39:43 +0100 Subject: [PATCH 4/5] Removed unneeded header --- .../fixedwing_pos_control.h | 54 ------------------- 1 file changed, 54 deletions(-) delete mode 100644 apps/fixedwing_pos_control/fixedwing_pos_control.h diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control.h b/apps/fixedwing_pos_control/fixedwing_pos_control.h deleted file mode 100644 index 90d717d9f5..0000000000 --- a/apps/fixedwing_pos_control/fixedwing_pos_control.h +++ /dev/null @@ -1,54 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Doug Weibel - * @author Lorenz Meier - @author Thomas Gubler - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file fixedwing_pos_control.h - * Position control for fixed wing airframes. - */ - -#ifndef FIXEDWING_POS_CONTROL_H_ -#define FIXEDWING_POS_CONTROL_H_ - - - - -float _wrap180(float bearing); -float _wrap360(float bearing); -float _wrapPI(float bearing); -float _wrap2PI(float bearing); - -/* FIXEDWING_CONTROL_H_ */ - From dc72d467d4abe3d18bbf02091eb4eaddb4f491d2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Nov 2012 00:28:15 +0100 Subject: [PATCH 5/5] fixed wing manual setpoints in manual mode --- .../fixedwing_att_control_main.c | 46 +++++++++++++------ 1 file changed, 33 insertions(+), 13 deletions(-) diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index ad0f201e1b..38f58ac33a 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -135,6 +135,11 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) memset(&att_sp, 0, sizeof(att_sp)); struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); + struct manual_control_setpoint_s manual_sp; + memset(&manual_sp, 0, sizeof(manual_sp)); + struct vehicle_status_s vstatus; + memset(&vstatus, 0, sizeof(vstatus)); + // static struct debug_key_value_s debug_output; // memset(&debug_output, 0, sizeof(debug_output)); @@ -144,16 +149,19 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) /* publish actuator controls */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { actuators.control[i] = 0.0f; + } orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); // orb_advert_t debug_pub = orb_advertise(ORB_ID(debug_key_value), &debug_output); // debug_output.key[0] = '1'; - /* subscribe */ + /* subscribe */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); /* Setup of loop */ float gyro[3] = {0.0f, 0.0f, 0.0f}; @@ -164,10 +172,11 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) /* 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); orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); + orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); + orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); gyro[0] = att.rollspeed; gyro[1] = att.pitchspeed; @@ -175,20 +184,31 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) /* Control */ - /* Attitude Control */ - fixedwing_att_control_attitude(&att_sp, - &att, - &rates_sp); + if (vstatus.state_machine == SYSTEM_STATE_AUTO) { + /* Attitude Control */ + fixedwing_att_control_attitude(&att_sp, + &att, + &rates_sp); - /* Attitude Rate Control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); + /* Attitude Rate Control */ + fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - //REMOVEME XXX - actuators.control[3] = 0.7f; + //REMOVEME XXX + actuators.control[3] = 0.7f; + // debug_output.value = rates_sp.pitch; + // orb_publish(ORB_ID(debug_key_value), debug_pub, &debug_output); + } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { + /* directly pass through values */ + actuators.control[0] = manual_sp.roll; + /* positive pitch means negative actuator -> pull up */ + actuators.control[1] = -manual_sp.pitch; + actuators.control[2] = manual_sp.yaw; + actuators.control[3] = manual_sp.throttle; + } + + /* publish output */ orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); -// debug_output.value = rates_sp.pitch; -// orb_publish(ORB_ID(debug_key_value), debug_pub, &debug_output); } printf("[fixedwing_att_control] exiting, stopping all motors.\n");