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/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index d2fbe31a60..658bf3e695 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -138,7 +138,13 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) memset(&rates_sp, 0, sizeof(rates_sp)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); + struct manual_control_setpoint_s manual_sp; + memset(&manual_sp, 0, sizeof(manual_sp)); + struct vehicle_status_s vstatus; + memset(&vstatus, 0, sizeof(vstatus)); +// static struct debug_key_value_s debug_output; +// memset(&debug_output, 0, sizeof(debug_output)); /* output structs */ struct actuator_controls_s actuators; @@ -148,17 +154,20 @@ 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 global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); /* Setup of loop */ float gyro[3] = {0.0f, 0.0f, 0.0f}; @@ -200,24 +209,39 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) } } + orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); + orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); + gyro[0] = att.rollspeed; gyro[1] = att.pitchspeed; gyro[2] = att.yawspeed; /* Control */ + if (vstatus.state_machine == SYSTEM_STATE_AUTO) { + /* Attitude Control */ + fixedwing_att_control_attitude(&att_sp, + &att, + &speed_body, + &rates_sp); - /* Attitude Control */ - fixedwing_att_control_attitude(&att_sp, - &att, - &speed_body, - &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.yaw; orb_publish(ORB_ID(debug_key_value), debug_pub, &debug_output); 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_ */ - 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) { diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 10d155ffc8..ce1e52c1ba 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -272,9 +272,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 { 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);