From d0af62783d1b18da6a15cb2da63e7ea88f5c398a Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 11:39:22 +0100 Subject: uORB: Added vehicle_landed uORB topic --- src/modules/uORB/objects_common.cpp | 3 +++ src/modules/uORB/topics/vehicle_land_detected.h | 23 +++++++++++++++++++++++ 2 files changed, 26 insertions(+) create mode 100644 src/modules/uORB/topics/vehicle_land_detected.h (limited to 'src/modules/uORB') diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 293412455..78fdf4de7 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -83,6 +83,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s); #include "topics/vehicle_gps_position.h" ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s); +#include "topics/vehicle_land_detected.h" +ORB_DEFINE(vehicle_land_detected, struct vehicle_land_detected_s); + #include "topics/satellite_info.h" ORB_DEFINE(satellite_info, struct satellite_info_s); diff --git a/src/modules/uORB/topics/vehicle_land_detected.h b/src/modules/uORB/topics/vehicle_land_detected.h new file mode 100644 index 000000000..0de29498d --- /dev/null +++ b/src/modules/uORB/topics/vehicle_land_detected.h @@ -0,0 +1,23 @@ +#ifndef __TOPIC_VEHICLE_LANDED_H__ +#define __TOPIC_VEHICLE_LANDED_H__ + +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct vehicle_land_detected_s { + uint64_t timestamp; /**< timestamp of the setpoint */ + bool landed; /**< true if vehicle is currently landed on the ground*/ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_land_detected); + +#endif //__TOPIC_VEHICLE_LANDED_H__ -- cgit v1.2.3 From 051a6972281dc9f8c15b5d8c73ac808416944932 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 11:53:00 +0100 Subject: uORB: Added missing license header --- src/modules/uORB/topics/vehicle_land_detected.h | 40 +++++++++++++++++++++++++ 1 file changed, 40 insertions(+) (limited to 'src/modules/uORB') diff --git a/src/modules/uORB/topics/vehicle_land_detected.h b/src/modules/uORB/topics/vehicle_land_detected.h index 0de29498d..51b3568e8 100644 --- a/src/modules/uORB/topics/vehicle_land_detected.h +++ b/src/modules/uORB/topics/vehicle_land_detected.h @@ -1,3 +1,43 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 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. + * + ****************************************************************************/ + +/** + * @file vehicle_land_detected.h + * Land detected uORB topic + * + * @author Johan Jansen + */ + #ifndef __TOPIC_VEHICLE_LANDED_H__ #define __TOPIC_VEHICLE_LANDED_H__ -- cgit v1.2.3 From 98ab83142c1bf7e5170e7527dde1a9e5132b5422 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 12:31:05 +0100 Subject: InertialNav: Removed land detector from position estimator --- .../position_estimator_inav_main.c | 15 +++++++++------ src/modules/uORB/topics/vehicle_local_position.h | 2 +- 2 files changed, 10 insertions(+), 7 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 2f972fc9f..b418e3a76 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -246,9 +246,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted float surface_offset = 0.0f; // ground level offset from reference altitude float surface_offset_rate = 0.0f; // surface offset change rate - float alt_avg = 0.0f; - bool landed = true; - hrt_abstime landed_time = 0; + //float alt_avg = 0.0f; + //bool landed = true; + //hrt_abstime landed_time = 0; hrt_abstime accel_timestamp = 0; hrt_abstime baro_timestamp = 0; @@ -1068,12 +1068,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } - /* detect land */ + +/* + // detect land alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t; float alt_disp2 = - z_est[0] - alt_avg; alt_disp2 = alt_disp2 * alt_disp2; float land_disp2 = params.land_disp * params.land_disp; - /* get actual thrust output */ + // get actual thrust output float thrust = armed.armed ? actuator.control[3] : 0.0f; if (landed) { @@ -1097,6 +1099,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) landed_time = 0; } } +*/ if (verbose_mode) { /* print updates rate */ @@ -1148,7 +1151,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; - local_pos.landed = landed; + local_pos.landed = false; local_pos.yaw = att.yaw; local_pos.dist_bottom_valid = dist_bottom_valid; local_pos.eph = eph; diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 5d39c897d..0e0bc9781 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -77,7 +77,7 @@ struct vehicle_local_position_s { double ref_lat; /**< Reference point latitude in degrees */ double ref_lon; /**< Reference point longitude in degrees */ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ - bool landed; /**< true if vehicle is landed */ + bool landed; /**< true if vehicle is landed TODO: deprecated, use vehicle_land_detected instead (remove this line?)*/ /* Distance to surface */ float dist_bottom; /**< Distance to bottom surface (ground) */ float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */ -- cgit v1.2.3 From 2da6439f742f7743adca22ad3a887e936e6c2277 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 13:57:46 +0100 Subject: uORB: Removed landed boolean flag from vehicle_local_position topic --- src/modules/uORB/topics/vehicle_local_position.h | 1 - 1 file changed, 1 deletion(-) (limited to 'src/modules/uORB') diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 0e0bc9781..8b46c5a3f 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -77,7 +77,6 @@ struct vehicle_local_position_s { double ref_lat; /**< Reference point latitude in degrees */ double ref_lon; /**< Reference point longitude in degrees */ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ - bool landed; /**< true if vehicle is landed TODO: deprecated, use vehicle_land_detected instead (remove this line?)*/ /* Distance to surface */ float dist_bottom; /**< Distance to bottom surface (ground) */ float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */ -- cgit v1.2.3 From beab89367f5cc2765c747fb463a27ce001206dd9 Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 18 Jan 2015 17:46:01 +0100 Subject: calculate total airflow over elevons using physical of flow behind propeller. read local position topic for future use. --- src/modules/uORB/topics/vtol_vehicle_status.h | 1 + .../vtol_att_control/vtol_att_control_main.cpp | 104 +++++++++++++++++++-- .../vtol_att_control/vtol_att_control_params.c | 35 ++++++- 3 files changed, 129 insertions(+), 11 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/uORB/topics/vtol_vehicle_status.h b/src/modules/uORB/topics/vtol_vehicle_status.h index 7b4e22bc8..968c2b6bd 100644 --- a/src/modules/uORB/topics/vtol_vehicle_status.h +++ b/src/modules/uORB/topics/vtol_vehicle_status.h @@ -55,6 +55,7 @@ struct vtol_vehicle_status_s { uint64_t timestamp; /**< Microseconds since system boot */ bool vtol_in_rw_mode; /*true: vtol vehicle is in rotating wing mode */ bool fw_permanent_stab; /**< In fw mode stabilize attitude even if in manual mode*/ + float airspeed_tot; /*< Estimated airspeed over control surfaces */ }; /** diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index c72a85ecd..8e68730b8 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -65,6 +65,8 @@ #include #include #include +#include +#include #include #include #include @@ -105,7 +107,9 @@ private: int _params_sub; //parameter updates subscription int _manual_control_sp_sub; //manual control setpoint subscription int _armed_sub; //arming status subscription + int _local_pos_sub; // sensor subscription int _airspeed_sub; // airspeed subscription + int _battery_status_sub; // battery status subscription int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs @@ -129,7 +133,9 @@ private: struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control struct actuator_armed_s _armed; //actuator arming status + struct vehicle_local_position_s _local_pos; struct airspeed_s _airspeed; // airspeed + struct battery_status_s _batt_status; // battery status struct { param_t idle_pwm_mc; //pwm value for idle in mc mode @@ -139,6 +145,9 @@ private: float mc_airspeed_trim; // trim airspeed in multicopter mode float mc_airspeed_max; // max airpseed in multicopter mode float fw_pitch_trim; // trim for neutral elevon position in fw mode + float power_max; // maximum power of one engine + float prop_eff; // factor to calculate prop efficiency + float arsp_lp_gain; // total airspeed estimate low pass gain } _params; struct { @@ -149,6 +158,9 @@ private: param_t mc_airspeed_trim; param_t mc_airspeed_max; param_t fw_pitch_trim; + param_t power_max; + param_t prop_eff; + param_t arsp_lp_gain; } _params_handles; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -159,6 +171,7 @@ private: * to waste energy when gliding. */ bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" unsigned _motor_count; // number of motors + float _airspeed_tot; //*****************Member functions*********************************************************************** @@ -172,7 +185,9 @@ private: void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output void vehicle_rates_sp_mc_poll(); void vehicle_rates_sp_fw_poll(); + void vehicle_local_pos_poll(); // Check for changes in sensor values void vehicle_airspeed_poll(); // Check for changes in airspeed + void vehicle_battery_poll(); // Check for battery updates void parameters_update_poll(); //Check if parameters have changed int parameters_update(); //Update local paraemter cache void fill_mc_att_control_output(); //write mc_att_control results to actuator message @@ -182,6 +197,7 @@ private: void set_idle_fw(); void set_idle_mc(); void scale_mc_output(); + void calc_tot_airspeed(); // estimated airspeed seen by elevons }; namespace VTOL_att_control @@ -205,7 +221,9 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_sub(-1), _manual_control_sp_sub(-1), _armed_sub(-1), + _local_pos_sub(-1), _airspeed_sub(-1), + _battery_status_sub(-1), //init publication handlers _actuators_0_pub(-1), @@ -218,6 +236,7 @@ VtolAttitudeControl::VtolAttitudeControl() : { flag_idle_mc = true; + _airspeed_tot = 0.0f; memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ @@ -234,7 +253,9 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in)); memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in)); memset(&_armed, 0, sizeof(_armed)); + memset(&_local_pos,0,sizeof(_local_pos)); memset(&_airspeed,0,sizeof(_airspeed)); + memset(&_batt_status,0,sizeof(_batt_status)); _params.idle_pwm_mc = PWM_LOWEST_MIN; _params.vtol_motor_count = 0; @@ -247,6 +268,9 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.mc_airspeed_max = param_find("VT_MC_ARSPD_MAX"); _params_handles.mc_airspeed_trim = param_find("VT_MC_ARSPD_TRIM"); _params_handles.fw_pitch_trim = param_find("VT_FW_PITCH_TRIM"); + _params_handles.power_max = param_find("VT_POWER_MAX"); + _params_handles.prop_eff = param_find("VT_PROP_EFF"); + _params_handles.arsp_lp_gain = param_find("VT_ARSP_LP_GAIN"); /* fetch initial parameter values */ parameters_update(); @@ -387,6 +411,19 @@ VtolAttitudeControl::vehicle_airspeed_poll() { } } +/** +* Check for battery updates. +*/ +void +VtolAttitudeControl::vehicle_battery_poll() { + bool updated; + orb_check(_battery_status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(battery_status), _battery_status_sub , &_batt_status); + } +} + /** * Check for parameter updates. */ @@ -405,6 +442,22 @@ VtolAttitudeControl::parameters_update_poll() } } +/** +* Check for sensor updates. +*/ +void +VtolAttitudeControl::vehicle_local_pos_poll() +{ + bool updated; + /* Check if parameters have changed */ + orb_check(_local_pos_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub , &_local_pos); + } + +} + /** * Update parameters. */ @@ -437,6 +490,18 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.fw_pitch_trim, &v); _params.fw_pitch_trim = v; + /* vtol maximum power engine can produce */ + param_get(_params_handles.power_max, &v); + _params.power_max = v; + + /* vtol propeller efficiency factor */ + param_get(_params_handles.prop_eff, &v); + _params.prop_eff = v; + + /* vtol total airspeed estimate low pass gain */ + param_get(_params_handles.arsp_lp_gain, &v); + _params.arsp_lp_gain = v; + return OK; } @@ -555,7 +620,7 @@ void VtolAttitudeControl::scale_mc_output() { // scale around tuning airspeed float airspeed; - + calc_tot_airspeed(); // estimate air velocity seen by elevons // if airspeed is not updating, we assume the normal average speed if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { @@ -563,16 +628,12 @@ VtolAttitudeControl::scale_mc_output() { if (nonfinite) { perf_count(_nonfinite_input_perf); } - } else { - // prevent numerical drama by requiring 0.5 m/s minimal speed - airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); - } - - // Sanity check if airspeed is consistent with throttle - if(_manual_control_sp.z >= 0.35f && airspeed < _params.mc_airspeed_trim) { // XXX magic number, should be hover throttle param - airspeed = _params.mc_airspeed_trim; + } else { + airspeed = _airspeed_tot; + airspeed = math::constrain(airspeed,_params.mc_airspeed_min, _params.mc_airspeed_max); } + _vtol_vehicle_status.airspeed_tot = airspeed; // save value for logging /* * For scaling our actuators using anything less than the min (close to stall) * speed doesn't make any sense - its the strongest reasonable deflection we @@ -584,6 +645,23 @@ VtolAttitudeControl::scale_mc_output() { _actuators_mc_in.control[1] = math::constrain(_actuators_mc_in.control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f); } +void VtolAttitudeControl::calc_tot_airspeed() { + float airspeed = math::max(1.0f, _airspeed.true_airspeed_m_s); // prevent numerical drama + // calculate momentary power of one engine + float P = _batt_status.voltage_filtered_v * _batt_status.current_a / _params.vtol_motor_count; + P = math::constrain(P,1.0f,_params.power_max); + // calculate prop efficiency + float power_factor = 1.0f - P*_params.prop_eff/_params.power_max; + float eta = (1.0f/(1 + expf(-0.4f * power_factor * airspeed)) - 0.5f)*2.0f; + eta = math::constrain(eta,0.001f,1.0f); // live on the safe side + // calculate induced airspeed by propeller + float v_ind = (airspeed/eta - airspeed)*2.0f; + // calculate total airspeed + float airspeed_raw = airspeed + v_ind; + // apply low-pass filter + _airspeed_tot = _params.arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw; +} + void VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -604,7 +682,9 @@ void VtolAttitudeControl::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); @@ -674,7 +754,10 @@ void VtolAttitudeControl::task_main() vehicle_rates_sp_mc_poll(); vehicle_rates_sp_fw_poll(); parameters_update_poll(); + vehicle_local_pos_poll(); // Check for new sensor values vehicle_airspeed_poll(); + vehicle_battery_poll(); + if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc mode */ _vtol_vehicle_status.vtol_in_rw_mode = true; @@ -688,7 +771,8 @@ void VtolAttitudeControl::task_main() if (fds[0].revents & POLLIN) { vehicle_manual_poll(); /* update remote input */ orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); - // scale pitch control with airspeed + + // scale pitch control with total airspeed scale_mc_output(); fill_mc_att_control_output(); diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index d1d4697f3..33752b2c4 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -64,7 +64,7 @@ PARAM_DEFINE_INT32(VT_IDLE_PWM_MC,900); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,2.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,10.0f); /** * Maximum airspeed in multicopter mode @@ -109,3 +109,36 @@ PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0); */ PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM,0.0f); +/** + * Motor max power + * + * Indicates the maximum power the motor is able to produce. Used to calculate + * propeller efficiency map. + * + * @min 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_POWER_MAX,120.0f); + +/** + * Propeller efficiency parameter + * + * Influences propeller efficiency at different power settings. Should be tuned beforehand. + * + * @min 0.5 + * @max 0.9 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_PROP_EFF,0.0f); + +/** + * Total airspeed estimate low-pass filter gain + * + * Gain for tuning the low-pass filter for the total airspeed estimate + * + * @min 0.0 + * @max 0.99 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN,0.3f); + -- cgit v1.2.3 From a1a5a65dfadaaea721e9c87c57b1b1b6bf1d4184 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 13:38:50 +0100 Subject: Fixed Coverity CID #12412 --- src/modules/uORB/uORB.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 149b8f6d2..cc6617aea 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (Cc) 2012-2015 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 @@ -230,7 +230,7 @@ ORBDevNode::open(struct file *filp) ret = CDev::open(filp); if (ret != OK) - free(sd); + delete sd; return ret; } -- cgit v1.2.3 From 70c7764b9f9e780664ae13d0962a55dd226117e6 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 19 Jan 2015 09:56:42 +0100 Subject: removed secondary attitude from attitude topic --- src/modules/uORB/topics/vehicle_attitude.h | 18 ------------------ 1 file changed, 18 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h index 77324415a..019944dc0 100755 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -79,24 +79,6 @@ struct vehicle_attitude_s { float g_comp[3]; /**< Compensated gravity vector */ bool R_valid; /**< Rotation matrix valid */ bool q_valid; /**< Quaternion valid */ - - // secondary attitude, use for VTOL - float roll_sec; /**< Roll angle (rad, Tait-Bryan, NED) */ - float pitch_sec; /**< Pitch angle (rad, Tait-Bryan, NED) */ - float yaw_sec; /**< Yaw angle (rad, Tait-Bryan, NED) */ - float rollspeed_sec; /**< Roll angular speed (rad/s, Tait-Bryan, NED) */ - float pitchspeed_sec; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) */ - float yawspeed_sec; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) */ - float rollacc_sec; /**< Roll angular accelration (rad/s, Tait-Bryan, NED) */ - float pitchacc_sec; /**< Pitch angular acceleration (rad/s, Tait-Bryan, NED) */ - float yawacc_sec; /**< Yaw angular acceleration (rad/s, Tait-Bryan, NED) */ - float rate_offsets_sec[3]; /**< Offsets of the body angular rates from zero */ - float R_sec[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */ - float q_sec[4]; /**< Quaternion (NED) */ - float g_comp_sec[3]; /**< Compensated gravity vector */ - bool R_valid_sec; /**< Rotation matrix valid */ - bool q_valid_sec; /**< Quaternion valid */ - }; /** -- cgit v1.2.3 From 87aaf2a959433d2c3df3848ffdcb173d113e807e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 11:01:25 +0100 Subject: uORB: Remove unused function --- src/modules/uORB/uORB.cpp | 12 ------------ 1 file changed, 12 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index cc6617aea..3373aac83 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -817,18 +817,6 @@ uorb_main(int argc, char *argv[]) namespace { -void debug(const char *fmt, ...) -{ - va_list ap; - - va_start(ap, fmt); - vfprintf(stderr, fmt, ap); - va_end(ap); - fprintf(stderr, "\n"); - fflush(stderr); - usleep(100000); -} - /** * Advertise a node; don't consider it an error if the node has * already been advertised. -- cgit v1.2.3