From 9d5ea4bcebcb574ba55543ae0fe68c290dd0df10 Mon Sep 17 00:00:00 2001 From: mswingtra Date: Fri, 28 Aug 2015 11:06:12 +0200 Subject: [PATCH 001/161] px4_param_def to QGC fix --- Tools/px4params/srcparser.py | 16 +++++++++------- Tools/px4params/srcscanner.py | 10 ++++++++-- 2 files changed, 17 insertions(+), 9 deletions(-) diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index 918040bbf5..c69699cad7 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -1,5 +1,7 @@ import sys import re +global default_var +default_var = {} class ParameterGroup(object): """ @@ -98,6 +100,7 @@ class SourceParser(object): re_comment_end = re.compile(r'(.*?)\s*\*\/') re_parameter_definition = re.compile(r'PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*,\s*([^ ,\)]+)\s*\)\s*;') re_px4_parameter_definition = re.compile(r'PX4_PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*\)\s*;') + re_px4_param_default_definition = re.compile(r'#define\s*PARAM_([A-Z_][A-Z0-9_]*)\s*([^ ,\)]+)\s*') re_cut_type_specifier = re.compile(r'[a-z]+$') re_is_a_number = re.compile(r'^-?[0-9\.]') re_remove_dots = re.compile(r'\.+$') @@ -114,13 +117,6 @@ class SourceParser(object): def __init__(self): self.param_groups = {} - def GetSupportedExtensions(self): - """ - Returns list of supported file extensions that can be parsed by this - parser. - """ - return [".cpp", ".c"] - def Parse(self, contents): """ Incrementally parse program contents and append all found parameters @@ -193,6 +189,10 @@ class SourceParser(object): name = None defval = "" # Non-empty line outside the comment + m = self.re_px4_param_default_definition.match(line) + if m: + name_m, defval_m = m.group(1,2) + default_var[name_m] = defval_m m = self.re_parameter_definition.match(line) if m: tp, name, defval = m.group(1, 2, 3) @@ -200,6 +200,8 @@ class SourceParser(object): m = self.re_px4_parameter_definition.match(line) if m: tp, name = m.group(1, 2) + if default_var.has_key(name+'_DEFAULT'): + defval = default_var[name+'_DEFAULT'] if tp is not None: # Remove trailing type specifier from numbers: 0.1f => 0.1 if defval != "" and self.re_is_a_number.match(defval): diff --git a/Tools/px4params/srcscanner.py b/Tools/px4params/srcscanner.py index a49039c719..4db5698992 100644 --- a/Tools/px4params/srcscanner.py +++ b/Tools/px4params/srcscanner.py @@ -13,10 +13,16 @@ class SourceScanner(object): Scans provided path and passes all found contents to the parser using parser.Parse method. """ - extensions = tuple(parser.GetSupportedExtensions()) + extensions1 = tuple([".h"]) + extensions2 = tuple([".cpp", ".c"]) for dirname, dirnames, filenames in os.walk(srcdir): for filename in filenames: - if filename.endswith(extensions): + if filename.endswith(extensions1): + path = os.path.join(dirname, filename) + if not self.ScanFile(path, parser): + return False + for filename in filenames: + if filename.endswith(extensions2): path = os.path.join(dirname, filename) if not self.ScanFile(path, parser): return False From d0dd0fc4d489a8773073daca50a6addcf9cb5a2e Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Wed, 23 Sep 2015 16:19:14 +0200 Subject: [PATCH 002/161] add control state topic --- msg/control_state.msg | 18 ++++++++++++++++++ src/modules/uORB/objects_common.cpp | 3 +++ src/platforms/px4_includes.h | 1 + 3 files changed, 22 insertions(+) create mode 100644 msg/control_state.msg diff --git a/msg/control_state.msg b/msg/control_state.msg new file mode 100644 index 0000000000..d976fbae8d --- /dev/null +++ b/msg/control_state.msg @@ -0,0 +1,18 @@ +# This is similar to the mavlink message CONTROL_SYSTEM_STATE, but for onboard use */ +uint64 timestamp # in microseconds since system start +float32 x_acc # X acceleration in body frame +float32 y_acc # Y acceleration in body frame +float32 z_acc # Z acceleration in body frame +float32 x_vel # X velocity in body frame +float32 y_vel # Y velocity in body frame +float32 z_vel # Z velocity in body frame +float32 x_pos # X position in local frame +float32 y_pos # Y position in local frame +float32 z_pos # z position in local frame +float32 airspeed # Airspeed, estimated +float32[3] vel_variance # Variance in body velocity estimate +float32[3] pos_variance # Variance in local position estimate +float32[4] q # Attitude Quaternion +float32 roll_rate # Roll body angular rate (rad/s, x forward/y right/z down) +float32 pitch_rate # Pitch body angular rate (rad/s, x forward/y right/z down) +float32 yaw_rate # Yaw body angular rate (rad/s, x forward/y right/z down) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index ac98048771..8fe70d7604 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -69,6 +69,9 @@ ORB_DEFINE(pwm_input, struct pwm_input_s); #include "topics/vehicle_attitude.h" ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s); +#include "topics/control_state.h" +ORB_DEFINE(control_state, struct control_state_s); + #include "topics/sensor_combined.h" ORB_DEFINE(sensor_combined, struct sensor_combined_s); diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 83bb60d796..7437a9405a 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -87,6 +87,7 @@ #include #include #include +#include #include #include #include From ce70803d250435ae1fc6f103066759b0c49a576b Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Wed, 23 Sep 2015 16:19:48 +0200 Subject: [PATCH 003/161] publish control state from ekf - missing:airspeed --- .../AttitudePositionEstimatorEKF.h | 9 +++ .../ekf_att_pos_estimator_main.cpp | 78 ++++++++++++++++++- 2 files changed, 83 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index e4d924396a..f88c274a59 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -152,12 +153,14 @@ private: int _armedSub; orb_advert_t _att_pub; /**< vehicle attitude */ + orb_advert_t _ctrl_state_pub; /**< control state */ orb_advert_t _global_pos_pub; /**< global position */ orb_advert_t _local_pos_pub; /**< position in local frame */ orb_advert_t _estimator_status_pub; /**< status of the estimator */ orb_advert_t _wind_pub; /**< wind estimate */ struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct control_state_s _ctrl_state; /**< control state */ struct gyro_report _gyro; struct accel_report _accel; struct mag_report _mag; @@ -316,6 +319,12 @@ private: **/ void publishAttitude(); + /** + * @brief + * Publish the system state for control modules + **/ + void publishControlState(); + /** * @brief * Publish local position relative to reference point where filter was initialized diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index d23dd1b9e9..8fcaf29f47 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -140,12 +140,14 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : /* publications */ _att_pub(nullptr), + _ctrl_state_pub(nullptr), _global_pos_pub(nullptr), _local_pos_pub(nullptr), _estimator_status_pub(nullptr), _wind_pub(nullptr), _att{}, + _ctrl_state{}, _gyro{}, _accel{}, _mag{}, @@ -821,9 +823,9 @@ void AttitudePositionEstimatorEKF::publishAttitude() _att.pitch = euler(1); _att.yaw = euler(2); - _att.rollspeed = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt; - _att.pitchspeed = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt; - _att.yawspeed = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt; + _att.rollspeed = _ekf->dAngIMU.x / _ekf->dtIMU - _ekf->states[10] / _ekf->dtIMUfilt; + _att.pitchspeed = _ekf->dAngIMU.y / _ekf->dtIMU - _ekf->states[11] / _ekf->dtIMUfilt; + _att.yawspeed = _ekf->dAngIMU.z / _ekf->dtIMU - _ekf->states[12] / _ekf->dtIMUfilt; // gyro offsets _att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt; @@ -832,7 +834,7 @@ void AttitudePositionEstimatorEKF::publishAttitude() /* lazily publish the attitude only once available */ if (_att_pub != nullptr) { - /* publish the attitude setpoint */ + /* publish the attitude */ orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); } else { @@ -841,6 +843,74 @@ void AttitudePositionEstimatorEKF::publishAttitude() } } +void AttitudePositionEstimatorEKF::publishControlState() +{ + /* Accelerations in Body Frame */ + _ctrl_state.x_acc = _ekf->accel.x; + _ctrl_state.y_acc = _ekf->accel.y; + _ctrl_state.z_acc = _ekf->accel.z; + + /* Velocity in Body Frame */ + Vector3f v_n(_ekf->states[4], _ekf->states[5], _ekf->states[6]); + Vector3f v_n_var(_ekf->P[4][4], _ekf->P[5][5], _ekf->P[6][6]); + Vector3f v_b = _ekf->Tnb * v_n; + Vector3f v_b_var = _ekf->Tnb * v_n_var; + + _ctrl_state.x_vel = v_b.x; + _ctrl_state.y_vel = v_b.y; + _ctrl_state.z_vel = v_b.z; + + _ctrl_state.vel_variance[0] = v_b_var.x; + _ctrl_state.vel_variance[1] = v_b_var.y; + _ctrl_state.vel_variance[2] = v_b_var.z; + + /* Local Position */ + _ctrl_state.x_pos = _ekf->states[7]; + _ctrl_state.y_pos = _ekf->states[8]; + + // XXX need to announce change of Z reference somehow elegantly + _ctrl_state.z_pos = _ekf->states[9] - _filter_ref_offset; + //_local_pos.z_stable = _ekf->states[9]; + + _ctrl_state.pos_variance[0] = _ekf->P[7][7]; + _ctrl_state.pos_variance[1] = _ekf->P[8][8]; + _ctrl_state.pos_variance[2] = _ekf->P[9][9]; + + /* Attitude */ + _ctrl_state.timestamp = _last_sensor_timestamp; + _ctrl_state.q[0] = _ekf->states[0]; + _ctrl_state.q[1] = _ekf->states[1]; + _ctrl_state.q[2] = _ekf->states[2]; + _ctrl_state.q[3] = _ekf->states[3]; + + /* Attitude Rates */ + _ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt; + _ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt; + _ctrl_state.yaw_rate = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt; + + /* Guard from bad data */ + if (!PX4_ISFINITE(_ctrl_state.x_vel) || + !PX4_ISFINITE(_ctrl_state.y_vel) || + !PX4_ISFINITE(_ctrl_state.z_vel) || + !PX4_ISFINITE(_ctrl_state.x_pos) || + !PX4_ISFINITE(_ctrl_state.y_pos) || + !PX4_ISFINITE(_ctrl_state.z_pos)) + { + // bad data, abort publication + return; + } + + /* lazily publish the control state only once available */ + if (_ctrl_state_pub != nullptr) { + /* publish the control state */ + orb_publish(ORB_ID(control_state), _ctrl_state_pub, &_ctrl_state); + + } else { + /* advertise and publish */ + _ctrl_state_pub = orb_advertise(ORB_ID(control_state), &_ctrl_state); + } +} + void AttitudePositionEstimatorEKF::publishLocalPosition() { _local_pos.timestamp = _last_sensor_timestamp; From 9c26e71ef6014bb983556778899c912f472083c4 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Thu, 8 Oct 2015 11:53:37 +0200 Subject: [PATCH 004/161] Added robust airspeed measurement --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 4 +++- src/modules/ekf_att_pos_estimator/estimator_22states.cpp | 6 ++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 8fcaf29f47..88ba758cb3 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -870,7 +870,6 @@ void AttitudePositionEstimatorEKF::publishControlState() // XXX need to announce change of Z reference somehow elegantly _ctrl_state.z_pos = _ekf->states[9] - _filter_ref_offset; - //_local_pos.z_stable = _ekf->states[9]; _ctrl_state.pos_variance[0] = _ekf->P[7][7]; _ctrl_state.pos_variance[1] = _ekf->P[8][8]; @@ -883,6 +882,9 @@ void AttitudePositionEstimatorEKF::publishControlState() _ctrl_state.q[2] = _ekf->states[2]; _ctrl_state.q[3] = _ekf->states[3]; + /* Airspeed (Groundspeed - Windspeed) */ + _ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2)); + /* Attitude Rates */ _ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt; _ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt; diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index b920cb9458..839918728c 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -49,6 +49,8 @@ #define M_PI_F static_cast(M_PI) #endif +#define MIN_AIRSPEED_MEAS 5.0f + constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f; AttPosEKF::AttPosEKF() : @@ -1687,7 +1689,7 @@ void AttPosEKF::FuseAirspeed() // Calculate the predicted airspeed VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); // Perform fusion of True Airspeed measurement - if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) + if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > MIN_AIRSPEED_MEAS)) { // Calculate observation jacobians SH_TAS[0] = 1/(sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); @@ -2595,7 +2597,7 @@ void AttPosEKF::CovarianceInit() P[13][13] = sq(0.2f*dtIMU); //Wind velocities - P[14][14] = 0.0f; + P[14][14] = 0.01f; P[15][15] = P[14][14]; //Earth magnetic field From 2f4afa2da7ac24ccd38a3f532b749e1e46680e76 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Thu, 8 Oct 2015 21:19:51 +0200 Subject: [PATCH 005/161] added control state to logging --- src/modules/sdlog2/sdlog2.c | 18 +++++++ src/modules/sdlog2/sdlog2_messages.h | 81 ++++++++++++++++------------ 2 files changed, 65 insertions(+), 34 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 253949b26e..8ee40f1661 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -66,6 +66,7 @@ #include #include #include +#include #include #include #include @@ -1108,6 +1109,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_command_s cmd; struct sensor_combined_s sensor; struct vehicle_attitude_s att; + struct control_state_s ctrl_state; struct vehicle_attitude_setpoint_s att_sp; struct vehicle_rates_setpoint_s rates_sp; struct actuator_outputs_s act_outputs; @@ -1158,6 +1160,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; struct log_VTOL_s log_VTOL; + struct log_CTS_s log_CTS; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; struct log_AIRS_s log_AIRS; @@ -1199,6 +1202,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int vtol_status_sub; int sensor_sub; int att_sub; + int ctrl_state_sub; int att_sp_sub; int rates_sp_sub; int act_outputs_sub; @@ -1236,6 +1240,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.gps_pos_sub = -1; subs.sensor_sub = -1; subs.att_sub = -1; + subs.ctrl_state_sub = -1; subs.att_sp_sub = -1; subs.rates_sp_sub = -1; subs.act_outputs_sub = -1; @@ -1714,6 +1719,19 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(FLOW); } + /* --- CONTROL STATE --- */ + if (copy_if_updated(ORB_ID(control_state), &subs.ctrl_state_sub, &buf.ctrl_state)) { + log_msg.msg_type = LOG_CTS_MSG; + log_msg.body.log_CTS.vx_body = buf.ctrl_state.x_vel; + log_msg.body.log_CTS.vy_body = buf.ctrl_state.y_vel; + log_msg.body.log_CTS.vz_body = buf.ctrl_state.z_vel; + log_msg.body.log_CTS.airspeed = buf.ctrl_state.airspeed; + log_msg.body.log_CTS.roll_rate = buf.ctrl_state.roll_rate; + log_msg.body.log_CTS.pitch_rate = buf.ctrl_state.pitch_rate; + log_msg.body.log_CTS.yaw_rate = buf.ctrl_state.yaw_rate; + LOGBUFFER_WRITE_AND_COUNT(CTS); + } + /* --- RC CHANNELS --- */ if (copy_if_updated(ORB_ID(rc_channels), &subs.rc_sub, &buf.rc)) { log_msg.msg_type = LOG_RC_MSG; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 7f77a58e7b..16fafd3165 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -185,8 +185,20 @@ struct log_STAT_s { float load; }; +/* --- CONTROL STATE --- */ +#define LOG_CTS_MSG 11 +struct log_CTS_s { + float vx_body; + float vy_body; + float vz_body; + float airspeed; + float roll_rate; + float pitch_rate; + float yaw_rate; +}; + /* --- RC - RC INPUT CHANNELS --- */ -#define LOG_RC_MSG 11 +#define LOG_RC_MSG 12 struct log_RC_s { float channel[8]; uint8_t channel_count; @@ -194,13 +206,13 @@ struct log_RC_s { }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ -#define LOG_OUT0_MSG 12 +#define LOG_OUT0_MSG 13 struct log_OUT0_s { float output[8]; }; /* --- AIRS - AIRSPEED --- */ -#define LOG_AIRS_MSG 13 +#define LOG_AIRS_MSG 14 struct log_AIRS_s { float indicated_airspeed; float true_airspeed; @@ -208,7 +220,7 @@ struct log_AIRS_s { }; /* --- ARSP - ATTITUDE RATE SET POINT --- */ -#define LOG_ARSP_MSG 14 +#define LOG_ARSP_MSG 15 struct log_ARSP_s { float roll_rate_sp; float pitch_rate_sp; @@ -216,7 +228,7 @@ struct log_ARSP_s { }; /* --- FLOW - OPTICAL FLOW --- */ -#define LOG_FLOW_MSG 15 +#define LOG_FLOW_MSG 16 struct log_FLOW_s { uint8_t sensor_id; float pixel_flow_x_integral; @@ -233,7 +245,7 @@ struct log_FLOW_s { }; /* --- GPOS - GLOBAL POSITION ESTIMATE --- */ -#define LOG_GPOS_MSG 16 +#define LOG_GPOS_MSG 17 struct log_GPOS_s { int32_t lat; int32_t lon; @@ -247,7 +259,7 @@ struct log_GPOS_s { }; /* --- GPSP - GLOBAL POSITION SETPOINT --- */ -#define LOG_GPSP_MSG 17 +#define LOG_GPSP_MSG 18 struct log_GPSP_s { uint8_t nav_state; int32_t lat; @@ -261,7 +273,7 @@ struct log_GPSP_s { }; /* --- ESC - ESC STATE --- */ -#define LOG_ESC_MSG 18 +#define LOG_ESC_MSG 19 struct log_ESC_s { uint16_t counter; uint8_t esc_count; @@ -278,7 +290,7 @@ struct log_ESC_s { }; /* --- GVSP - GLOBAL VELOCITY SETPOINT --- */ -#define LOG_GVSP_MSG 19 +#define LOG_GVSP_MSG 20 struct log_GVSP_s { float vx; float vy; @@ -286,7 +298,7 @@ struct log_GVSP_s { }; /* --- BATT - BATTERY --- */ -#define LOG_BATT_MSG 20 +#define LOG_BATT_MSG 21 struct log_BATT_s { float voltage; float voltage_filtered; @@ -295,7 +307,7 @@ struct log_BATT_s { }; /* --- DIST - RANGE SENSOR DISTANCE --- */ -#define LOG_DIST_MSG 21 +#define LOG_DIST_MSG 22 struct log_DIST_s { uint8_t id; uint8_t type; @@ -304,11 +316,11 @@ struct log_DIST_s { float covariance; }; -/* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */ +/* LOG IMU1 and IMU2 MSGs consume IDs 23 and 24 */ /* --- PWR - ONBOARD POWER SYSTEM --- */ -#define LOG_PWR_MSG 24 +#define LOG_PWR_MSG 25 struct log_PWR_s { float peripherals_5v; float servo_rail_5v; @@ -321,7 +333,7 @@ struct log_PWR_s { }; /* --- MOCP - MOCAP ATTITUDE AND POSITION --- */ -#define LOG_MOCP_MSG 25 +#define LOG_MOCP_MSG 26 struct log_MOCP_s { float qw; float qx; @@ -333,31 +345,31 @@ struct log_MOCP_s { }; /* --- GS0A - GPS SNR #0, SAT GROUP A --- */ -#define LOG_GS0A_MSG 26 +#define LOG_GS0A_MSG 27 struct log_GS0A_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- GS0B - GPS SNR #0, SAT GROUP B --- */ -#define LOG_GS0B_MSG 27 +#define LOG_GS0B_MSG 28 struct log_GS0B_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- GS1A - GPS SNR #1, SAT GROUP A --- */ -#define LOG_GS1A_MSG 28 +#define LOG_GS1A_MSG 29 struct log_GS1A_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- GS1B - GPS SNR #1, SAT GROUP B --- */ -#define LOG_GS1B_MSG 29 +#define LOG_GS1B_MSG 30 struct log_GS1B_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- TECS - TECS STATUS --- */ -#define LOG_TECS_MSG 30 +#define LOG_TECS_MSG 31 struct log_TECS_s { float altitudeSp; float altitudeFiltered; @@ -378,7 +390,7 @@ struct log_TECS_s { }; /* --- WIND - WIND ESTIMATE --- */ -#define LOG_WIND_MSG 31 +#define LOG_WIND_MSG 32 struct log_WIND_s { float x; float y; @@ -387,7 +399,7 @@ struct log_WIND_s { }; /* --- EST0 - ESTIMATOR STATUS --- */ -#define LOG_EST0_MSG 32 +#define LOG_EST0_MSG 33 struct log_EST0_s { float s[12]; uint8_t n_states; @@ -397,28 +409,28 @@ struct log_EST0_s { }; /* --- EST1 - ESTIMATOR STATUS --- */ -#define LOG_EST1_MSG 33 +#define LOG_EST1_MSG 34 struct log_EST1_s { float s[16]; }; /* --- EST2 - ESTIMATOR STATUS --- */ -#define LOG_EST2_MSG 34 +#define LOG_EST2_MSG 35 struct log_EST2_s { float cov[12]; }; /* --- EST3 - ESTIMATOR STATUS --- */ -#define LOG_EST3_MSG 35 +#define LOG_EST3_MSG 36 struct log_EST3_s { float cov[16]; }; /* --- TEL0..3 - TELEMETRY STATUS --- */ -#define LOG_TEL0_MSG 36 -#define LOG_TEL1_MSG 37 -#define LOG_TEL2_MSG 38 -#define LOG_TEL3_MSG 39 +#define LOG_TEL0_MSG 37 +#define LOG_TEL1_MSG 38 +#define LOG_TEL2_MSG 39 +#define LOG_TEL3_MSG 40 struct log_TEL_s { uint8_t rssi; uint8_t remote_rssi; @@ -431,7 +443,7 @@ struct log_TEL_s { }; /* --- VISN - VISION POSITION --- */ -#define LOG_VISN_MSG 40 +#define LOG_VISN_MSG 41 struct log_VISN_s { float x; float y; @@ -446,7 +458,7 @@ struct log_VISN_s { }; /* --- ENCODERS - ENCODER DATA --- */ -#define LOG_ENCD_MSG 41 +#define LOG_ENCD_MSG 42 struct log_ENCD_s { int64_t cnt0; float vel0; @@ -455,22 +467,22 @@ struct log_ENCD_s { }; /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */ -#define LOG_AIR1_MSG 42 +#define LOG_AIR1_MSG 43 /* --- VTOL - VTOL VEHICLE STATUS */ -#define LOG_VTOL_MSG 43 +#define LOG_VTOL_MSG 44 struct log_VTOL_s { float airspeed_tot; }; /* --- TIMESYNC - TIME SYNCHRONISATION OFFSET */ -#define LOG_TSYN_MSG 44 +#define LOG_TSYN_MSG 45 struct log_TSYN_s { uint64_t time_offset; }; /* --- MACS - MULTIROTOR ATTITUDE CONTROLLER STATUS */ -#define LOG_MACS_MSG 45 +#define LOG_MACS_MSG 47 struct log_MACS_s { float roll_rate_integ; float pitch_rate_integ; @@ -519,6 +531,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBfBBf", "MainState,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"), LOG_FORMAT(VTOL, "f", "Arsp"), + LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"), LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), From 78f5a35d1214f3d60878e4c0104428b98826d919 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Thu, 8 Oct 2015 22:22:11 +0200 Subject: [PATCH 006/161] integrated control state in fw l1 controller --- .../fw_pos_control_l1_main.cpp | 77 +++++++++++-------- 1 file changed, 43 insertions(+), 34 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index c3816f5374..9bfebe5f8b 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -76,6 +76,7 @@ #include #include #include +#include #include #include #include @@ -153,6 +154,7 @@ private: int _global_pos_sub; int _pos_sp_triplet_sub; int _att_sub; /**< vehicle attitude subscription */ + int _ctrl_state_sub; /**< control state subscription */ int _airspeed_sub; /**< airspeed subscription */ int _control_mode_sub; /**< control mode subscription */ int _vehicle_status_sub; /**< vehicle status subscription */ @@ -165,6 +167,7 @@ private: orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct control_state_s _ctrl_state; /**< control state */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ @@ -364,6 +367,11 @@ private: */ void vehicle_attitude_poll(); + /** + * Check for changes in control state. + */ + void control_state_poll(); + /** * Check for accel updates. */ @@ -481,6 +489,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _global_pos_sub(-1), _pos_sp_triplet_sub(-1), _att_sub(-1), + _ctrl_state_sub(-1), _airspeed_sub(-1), _control_mode_sub(-1), _vehicle_status_sub(-1), @@ -495,6 +504,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* states */ _att(), + _ctrl_state(), _att_sp(), _nav_capabilities(), _manual(), @@ -748,32 +758,6 @@ FixedwingPositionControl::vehicle_status_poll() } } -bool -FixedwingPositionControl::vehicle_airspeed_poll() -{ - /* check if there is an airspeed update or if it timed out */ - bool airspeed_updated; - orb_check(_airspeed_sub, &airspeed_updated); - - if (airspeed_updated) { - orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - _airspeed_valid = true; - _airspeed_last_valid = hrt_absolute_time(); - - } else { - - /* no airspeed updates for one second */ - if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) { - _airspeed_valid = false; - } - } - - /* update TECS state */ - _tecs.enable_airspeed(_airspeed_valid); - - return airspeed_updated; -} - bool FixedwingPositionControl::vehicle_manual_control_setpoint_poll() { @@ -806,6 +790,30 @@ FixedwingPositionControl::vehicle_attitude_poll() } } +void +FixedwingPositionControl::control_state_poll() +{ + /* check if there is a new position */ + bool ctrl_state_updated; + orb_check(_ctrl_state_sub, &ctrl_state_updated); + + if (ctrl_state_updated) { + orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); + _airspeed_valid = true; + _airspeed_last_valid = hrt_absolute_time(); + + } else { + + /* no airspeed updates for one second */ + if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) { + _airspeed_valid = false; + } + } + + /* update TECS state */ + _tecs.enable_airspeed(_airspeed_valid); +} + void FixedwingPositionControl::vehicle_sensor_combined_poll() { @@ -852,7 +860,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) float airspeed; if (_airspeed_valid) { - airspeed = _airspeed.true_airspeed_m_s; + airspeed = _ctrl_state.airspeed; } else { airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f; @@ -1083,7 +1091,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* update TECS filters */ if (!_mTecs.getEnabled()) { - _tecs.update_state(_global_pos.alt, _airspeed.indicated_airspeed_m_s, _R_nb, + _tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb, accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control); } @@ -1116,7 +1124,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); - _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + _mTecs.resetDerivatives(_ctrl_state.airspeed); } } _control_mode_current = FW_POSCTRL_MODE_AUTO; @@ -1475,7 +1483,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); - _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + _mTecs.resetDerivatives(_ctrl_state.airspeed); } } _control_mode_current = FW_POSCTRL_MODE_POSITION; @@ -1586,7 +1594,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); - _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + _mTecs.resetDerivatives(_ctrl_state.airspeed); } } _control_mode_current = FW_POSCTRL_MODE_ALTITUDE; @@ -1698,6 +1706,7 @@ FixedwingPositionControl::task_main() _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -1779,9 +1788,9 @@ FixedwingPositionControl::task_main() _global_pos_valid = true; vehicle_attitude_poll(); + control_state_poll(); vehicle_setpoint_poll(); vehicle_sensor_combined_poll(); - vehicle_airspeed_poll(); vehicle_manual_control_setpoint_poll(); // vehicle_baro_poll(); @@ -1887,7 +1896,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ /* use pitch max set by MT param */ limitOverride.disablePitchMaxOverride(); } - _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _ctrl_state.airspeed, v_sp, mode, limitOverride); } else { if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { @@ -1901,7 +1910,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ /* Using tecs library */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, - _airspeed.indicated_airspeed_m_s, eas2tas, + _ctrl_state.airspeed, eas2tas, climbout_mode, climbout_pitch_min_rad, throttle_min, throttle_max, throttle_cruise, pitch_min_rad, pitch_max_rad); From 6dbf4e45733c551d67038ba85f704ce937fbae3c Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Thu, 8 Oct 2015 22:43:00 +0200 Subject: [PATCH 007/161] integrated ctrl state rates in mc_att_control --- .../mc_att_control/mc_att_control_main.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 830f48dff8..360362f75f 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -76,6 +76,7 @@ #include #include #include +#include #include #include #include @@ -128,6 +129,7 @@ private: int _control_task; /**< task handle */ int _v_att_sub; /**< vehicle attitude subscription */ + int _ctrl_state_sub; /**< control state subscription */ int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */ int _v_control_mode_sub; /**< vehicle control mode subscription */ @@ -147,6 +149,7 @@ private: bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ struct vehicle_attitude_s _v_att; /**< vehicle attitude */ + struct control_state_s _ctrl_state; /**< control state */ struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ @@ -304,6 +307,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* subscriptions */ _v_att_sub(-1), + _ctrl_state_sub(-1), _v_att_sp_sub(-1), _v_control_mode_sub(-1), _params_sub(-1), @@ -326,6 +330,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : { memset(&_v_att, 0, sizeof(_v_att)); + memset(&_ctrl_state, 0, sizeof(_ctrl_state)); memset(&_v_att_sp, 0, sizeof(_v_att_sp)); memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); @@ -708,9 +713,9 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) /* current body angular rates */ math::Vector<3> rates; - rates(0) = _v_att.rollspeed; - rates(1) = _v_att.pitchspeed; - rates(2) = _v_att.yawspeed; + rates(0) = _ctrl_state.roll_rate; + rates(1) = _ctrl_state.pitch_rate; + rates(2) = _ctrl_state.yaw_rate; /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; @@ -749,6 +754,7 @@ MulticopterAttitudeControl::task_main() _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -798,8 +804,9 @@ MulticopterAttitudeControl::task_main() dt = 0.02f; } - /* copy attitude topic */ + /* copy attitude and control state topics */ orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); + orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); /* check for updates in other topics */ parameter_update_poll(); From 8d756055b8517a153667ca78a0c00f73cf09158d Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Thu, 8 Oct 2015 23:00:44 +0200 Subject: [PATCH 008/161] integrated control state in fw_att_control --- .../fw_att_control/fw_att_control_main.cpp | 51 ++++++------------- 1 file changed, 15 insertions(+), 36 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index d06e9b9a74..3c326aec6f 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -58,7 +58,6 @@ #include #include #include -#include #include #include #include @@ -68,6 +67,7 @@ #include #include #include +#include #include #include #include @@ -126,10 +126,10 @@ private: int _control_task; /**< task handle */ int _att_sub; /**< vehicle attitude subscription */ + int _ctrl_state_sub; /**< control state subscription */ int _accel_sub; /**< accelerometer subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _attitude_sub; /**< raw rc channels data subscription */ - int _airspeed_sub; /**< airspeed subscription */ int _vcontrol_mode_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ @@ -145,11 +145,11 @@ private: orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct control_state_s _ctrl_state; /**< control state */ struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct vehicle_rates_setpoint_s _rates_sp; /* attitude rates setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator control inputs */ struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ @@ -269,12 +269,6 @@ private: */ void vehicle_manual_poll(); - - /** - * Check for airspeed updates. - */ - void vehicle_airspeed_poll(); - /** * Check for accel updates. */ @@ -327,8 +321,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* subscriptions */ _att_sub(-1), + _ctrl_state_sub(-1), _accel_sub(-1), - _airspeed_sub(-1), _vcontrol_mode_sub(-1), _params_sub(-1), _manual_sub(-1), @@ -354,11 +348,11 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : { /* safely initialize structs */ _att = {}; + _ctrl_state = {}; _accel = {}; _att_sp = {}; _rates_sp = {}; _manual = {}; - _airspeed = {}; _vcontrol_mode = {}; _actuators = {}; _actuators_airframe = {}; @@ -539,18 +533,6 @@ FixedwingAttitudeControl::vehicle_manual_poll() } } -void -FixedwingAttitudeControl::vehicle_airspeed_poll() -{ - /* check if there is a new position */ - bool airspeed_updated; - orb_check(_airspeed_sub, &airspeed_updated); - - if (airspeed_updated) { - orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - } -} - void FixedwingAttitudeControl::vehicle_accel_poll() { @@ -624,8 +606,8 @@ FixedwingAttitudeControl::task_main() */ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); - _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -641,7 +623,6 @@ FixedwingAttitudeControl::task_main() parameters_update(); /* get an initial update for all sensor and status data */ - vehicle_airspeed_poll(); vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_control_mode_poll(); @@ -700,6 +681,7 @@ FixedwingAttitudeControl::task_main() /* load local copies */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) { @@ -756,13 +738,11 @@ FixedwingAttitudeControl::task_main() PX4_R(_att.R, 2, 2) = R_adapted(2, 2); /* lastly, roll- and yawspeed have to be swaped */ - float helper = _att.rollspeed; - _att.rollspeed = -_att.yawspeed; - _att.yawspeed = helper; + float helper = _ctrl_state.roll_rate; + _ctrl_state.roll_rate = -_ctrl_state.yaw_rate; + _ctrl_state.yaw_rate = helper; } - vehicle_airspeed_poll(); - vehicle_setpoint_poll(); vehicle_accel_poll(); @@ -813,15 +793,14 @@ FixedwingAttitudeControl::task_main() float airspeed; /* if airspeed is not updating, we assume the normal average speed */ - if (bool nonfinite = !PX4_ISFINITE(_airspeed.true_airspeed_m_s) || - hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { + if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed)) { airspeed = _parameters.airspeed_trim; 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); + airspeed = math::max(0.5f, _ctrl_state.airspeed); } /* @@ -975,9 +954,9 @@ FixedwingAttitudeControl::task_main() control_input.roll = _att.roll; control_input.pitch = _att.pitch; control_input.yaw = _att.yaw; - control_input.roll_rate = _att.rollspeed; - control_input.pitch_rate = _att.pitchspeed; - control_input.yaw_rate = _att.yawspeed; + control_input.roll_rate = _ctrl_state.roll_rate; + control_input.pitch_rate = _ctrl_state.pitch_rate; + control_input.yaw_rate = _ctrl_state.yaw_rate; control_input.speed_body_u = speed_body_u; control_input.speed_body_v = speed_body_v; control_input.speed_body_w = speed_body_w; From 5ee9bb5363517f6e15bfd6f436202123b4023f71 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Fri, 9 Oct 2015 09:06:02 +0200 Subject: [PATCH 009/161] clean up fw_pos_control_l1 --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 9bfebe5f8b..48edca2b52 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -68,7 +68,6 @@ #include #include #include -#include #include #include #include @@ -155,7 +154,6 @@ private: int _pos_sp_triplet_sub; int _att_sub; /**< vehicle attitude subscription */ int _ctrl_state_sub; /**< control state subscription */ - int _airspeed_sub; /**< airspeed subscription */ int _control_mode_sub; /**< control mode subscription */ int _vehicle_status_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ @@ -171,7 +169,6 @@ private: struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _control_mode; /**< control mode */ struct vehicle_status_s _vehicle_status; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ @@ -357,11 +354,6 @@ private: */ bool vehicle_manual_control_setpoint_poll(); - /** - * Check for airspeed updates. - */ - bool vehicle_airspeed_poll(); - /** * Check for position updates. */ @@ -490,7 +482,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _pos_sp_triplet_sub(-1), _att_sub(-1), _ctrl_state_sub(-1), - _airspeed_sub(-1), _control_mode_sub(-1), _vehicle_status_sub(-1), _params_sub(-1), @@ -508,7 +499,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _att_sp(), _nav_capabilities(), _manual(), - _airspeed(), _control_mode(), _vehicle_status(), _global_pos(), @@ -1710,7 +1700,6 @@ FixedwingPositionControl::task_main() _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); From 65837bdc98e67504f11db73b3d6f36aaa8c9e551 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Sat, 10 Oct 2015 18:16:17 +0200 Subject: [PATCH 010/161] publish control state from attitude_estimator_q --- .../attitude_estimator_q_main.cpp | 33 +++++++++++++++---- 1 file changed, 27 insertions(+), 6 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 9b60701413..a3fbefb13b 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -53,6 +53,7 @@ #include #include #include + #include #include #include #include @@ -116,6 +117,7 @@ private: int _params_sub = -1; int _global_pos_sub = -1; orb_advert_t _att_pub = nullptr; + orb_advert_t _ctrl_state_pub = nullptr; struct { param_t w_acc; @@ -417,11 +419,8 @@ void AttitudeEstimatorQ::task_main() att.pitch = euler(1); att.yaw = euler(2); - /* the complimentary filter should reflect the true system - * state, but we need smoother outputs for the control system - */ - att.rollspeed = _lp_roll_rate.apply(_rates(0)); - att.pitchspeed = _lp_pitch_rate.apply(_rates(1)); + att.rollspeed = _rates(0); + att.pitchspeed = _rates(1); att.yawspeed = _rates(2); for (int i = 0; i < 3; i++) { @@ -446,6 +445,29 @@ void AttitudeEstimatorQ::task_main() } else { orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); } + + struct control_state_s ctrl_state = {}; + ctrl_state.timestamp = sensors.timestamp; + + /* Attitude quaternions for control state */ + ctrl_state.q[0] = _q(0); + ctrl_state.q[1] = _q(1); + ctrl_state.q[2] = _q(2); + ctrl_state.q[3] = _q(3); + + /* Attitude rates for control state */ + ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0)); + ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1)); + ctrl_state.roll_rate = _rates(2); + + /* Publish to control state topic */ + if (_ctrl_state_pub == nullptr) + { + _ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); + + } else { + orb_publish(ORB_ID(control_state), _ctrl_state_pub, &ctrl_state); + } } } @@ -571,7 +593,6 @@ bool AttitudeEstimatorQ::update(float dt) { return true; } - int attitude_estimator_q_main(int argc, char *argv[]) { if (argc < 1) { warnx("usage: attitude_estimator_q {start|stop|status}"); From 035216fc9ccb86df18d7d1ddd57d91e32ed89ad3 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Tue, 13 Oct 2015 17:54:28 +0200 Subject: [PATCH 011/161] fully replaced vehicle_attitude with control_state in mc_att_control --- .../mc_att_control/mc_att_control_main.cpp | 17 +++++------------ 1 file changed, 5 insertions(+), 12 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 360362f75f..8a097cbe50 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -75,7 +75,6 @@ #include #include #include -#include #include #include #include @@ -128,7 +127,6 @@ private: bool _task_should_exit; /**< if true, task_main() should exit */ int _control_task; /**< task handle */ - int _v_att_sub; /**< vehicle attitude subscription */ int _ctrl_state_sub; /**< control state subscription */ int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */ @@ -148,7 +146,6 @@ private: bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - struct vehicle_attitude_s _v_att; /**< vehicle attitude */ struct control_state_s _ctrl_state; /**< control state */ struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ @@ -306,7 +303,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _control_task(-1), /* subscriptions */ - _v_att_sub(-1), _ctrl_state_sub(-1), _v_att_sp_sub(-1), _v_control_mode_sub(-1), @@ -329,7 +325,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")) { - memset(&_v_att, 0, sizeof(_v_att)); memset(&_ctrl_state, 0, sizeof(_ctrl_state)); memset(&_v_att_sp, 0, sizeof(_v_att_sp)); memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); @@ -619,9 +614,9 @@ MulticopterAttitudeControl::control_attitude(float dt) math::Matrix<3, 3> R_sp; R_sp.set(_v_att_sp.R_body); - /* rotation matrix for current state */ - math::Matrix<3, 3> R; - R.set(_v_att.R); + /* get current rotation matrix from control state quaternions */ + math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + math::Matrix<3, 3> R = q_att.to_dcm(); /* all input data is ready, run controller itself */ @@ -753,7 +748,6 @@ MulticopterAttitudeControl::task_main() */ _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -768,7 +762,7 @@ MulticopterAttitudeControl::task_main() /* wakeup source: vehicle attitude */ px4_pollfd_struct_t fds[1]; - fds[0].fd = _v_att_sub; + fds[0].fd = _ctrl_state_sub; fds[0].events = POLLIN; while (!_task_should_exit) { @@ -805,7 +799,6 @@ MulticopterAttitudeControl::task_main() } /* copy attitude and control state topics */ - orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); /* check for updates in other topics */ @@ -873,7 +866,7 @@ MulticopterAttitudeControl::task_main() _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); - _actuators.timestamp_sample = _v_att.timestamp; + _actuators.timestamp_sample = _ctrl_state.timestamp; _controller_status.roll_rate_integ = _rates_int(0); _controller_status.pitch_rate_integ = _rates_int(1); From 5092bcae70a5e81254ed0fdba509e9190d01a6e6 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Wed, 14 Oct 2015 09:33:07 +0200 Subject: [PATCH 012/161] fully ported control state attitude into fw_att_control --- .../fw_att_control/fw_att_control_main.cpp | 69 ++++++++++--------- 1 file changed, 36 insertions(+), 33 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 3c326aec6f..83b5caaf1f 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -242,6 +242,11 @@ private: } _parameter_handles; /**< handles for interesting parameters */ + // Rotation matrix and euler angles to extract from control state + math::Matrix<3, 3> _R; + float _roll; + float _pitch; + float _yaw; ECL_RollController _roll_ctrl; ECL_PitchController _pitch_ctrl; @@ -684,6 +689,16 @@ FixedwingAttitudeControl::task_main() orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); + /* get current rotation matrix and euler angles from control state quaternions */ + math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + _R = q_att.to_dcm(); + + math::Vector<3> euler_angles; + euler_angles = _R.to_euler(); + _roll = euler_angles(0); + _pitch = euler_angles(1); + _yaw = euler_angles(2); + if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) { /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode * @@ -701,41 +716,29 @@ FixedwingAttitudeControl::task_main() * Rxy Ryy Rzy -Rzy Ryy Rxy * Rxz Ryz Rzz -Rzz Ryz Rxz * */ - math::Matrix<3, 3> R; //original rotation matrix - math::Matrix<3, 3> R_adapted; //modified rotation matrix - R.set(_att.R); - R_adapted.set(_att.R); + math::Matrix<3, 3> R_adapted = _R; //modified rotation matrix /* move z to x */ - R_adapted(0, 0) = R(0, 2); - R_adapted(1, 0) = R(1, 2); - R_adapted(2, 0) = R(2, 2); + R_adapted(0, 0) = _R(0, 2); + R_adapted(1, 0) = _R(1, 2); + R_adapted(2, 0) = _R(2, 2); /* move x to z */ - R_adapted(0, 2) = R(0, 0); - R_adapted(1, 2) = R(1, 0); - R_adapted(2, 2) = R(2, 0); + R_adapted(0, 2) = _R(0, 0); + R_adapted(1, 2) = _R(1, 0); + R_adapted(2, 2) = _R(2, 0); /* change direction of pitch (convert to right handed system) */ R_adapted(0, 0) = -R_adapted(0, 0); R_adapted(1, 0) = -R_adapted(1, 0); R_adapted(2, 0) = -R_adapted(2, 0); - math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation - euler_angles = R_adapted.to_euler(); + euler_angles = R_adapted.to_euler(); //adapted euler angles for fixed wing operation /* fill in new attitude data */ - _att.roll = euler_angles(0); - _att.pitch = euler_angles(1); - _att.yaw = euler_angles(2); - PX4_R(_att.R, 0, 0) = R_adapted(0, 0); - PX4_R(_att.R, 0, 1) = R_adapted(0, 1); - PX4_R(_att.R, 0, 2) = R_adapted(0, 2); - PX4_R(_att.R, 1, 0) = R_adapted(1, 0); - PX4_R(_att.R, 1, 1) = R_adapted(1, 1); - PX4_R(_att.R, 1, 2) = R_adapted(1, 2); - PX4_R(_att.R, 2, 0) = R_adapted(2, 0); - PX4_R(_att.R, 2, 1) = R_adapted(2, 1); - PX4_R(_att.R, 2, 2) = R_adapted(2, 2); + _R = R_adapted; + _roll = euler_angles(0); + _pitch = euler_angles(1); + _yaw = euler_angles(2); /* lastly, roll- and yawspeed have to be swaped */ float helper = _ctrl_state.roll_rate; @@ -845,7 +848,7 @@ FixedwingAttitudeControl::task_main() /* the pilot does not want to change direction, * take straight attitude setpoint from position controller */ - if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) { + if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) { roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; } else { roll_sp = (_manual.y * _parameters.man_roll_max) @@ -940,9 +943,9 @@ FixedwingAttitudeControl::task_main() float speed_body_v = 0.0f; float speed_body_w = 0.0f; if(_att.R_valid) { - speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d; - speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d; - speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d; + speed_body_u = _R(0, 0) * _global_pos.vel_n + _R(1, 0) * _global_pos.vel_e + _R(2, 0) * _global_pos.vel_d; + speed_body_v = _R(0, 1) * _global_pos.vel_n + _R(1, 1) * _global_pos.vel_e + _R(2, 1) * _global_pos.vel_d; + speed_body_w = _R(0, 2) * _global_pos.vel_n + _R(1, 2) * _global_pos.vel_e + _R(2, 2) * _global_pos.vel_d; } else { if (_debug && loop_counter % 10 == 0) { warnx("Did not get a valid R\n"); @@ -951,9 +954,9 @@ FixedwingAttitudeControl::task_main() /* Prepare data for attitude controllers */ struct ECL_ControlData control_input = {}; - control_input.roll = _att.roll; - control_input.pitch = _att.pitch; - control_input.yaw = _att.yaw; + control_input.roll = _roll; + control_input.pitch = _pitch; + control_input.yaw = _yaw; control_input.roll_rate = _ctrl_state.roll_rate; control_input.pitch_rate = _ctrl_state.pitch_rate; control_input.yaw_rate = _ctrl_state.yaw_rate; @@ -1079,9 +1082,9 @@ FixedwingAttitudeControl::task_main() /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); - _actuators.timestamp_sample = _att.timestamp; + _actuators.timestamp_sample = _ctrl_state.timestamp; _actuators_airframe.timestamp = hrt_absolute_time(); - _actuators_airframe.timestamp_sample = _att.timestamp; + _actuators_airframe.timestamp_sample = _ctrl_state.timestamp; /* Only publish if any of the proper modes are enabled */ if(_vcontrol_mode.flag_control_rates_enabled || From e72e72cd652d6d266b90db45545b2a54a20d8942 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Wed, 14 Oct 2015 10:53:19 +0200 Subject: [PATCH 013/161] removed vehicle_attitude subscription from fw_att_control --- .../fw_att_control/fw_att_control_main.cpp | 24 ++++--------------- 1 file changed, 4 insertions(+), 20 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 83b5caaf1f..8127d0ac55 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -66,7 +66,6 @@ #include #include #include -#include #include #include #include @@ -125,7 +124,6 @@ private: bool _task_running; /**< if true, task is running in its mainloop */ int _control_task; /**< task handle */ - int _att_sub; /**< vehicle attitude subscription */ int _ctrl_state_sub; /**< control state subscription */ int _accel_sub; /**< accelerometer subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ @@ -144,7 +142,6 @@ private: orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure - struct vehicle_attitude_s _att; /**< vehicle attitude */ struct control_state_s _ctrl_state; /**< control state */ struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ @@ -325,7 +322,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _control_task(-1), /* subscriptions */ - _att_sub(-1), _ctrl_state_sub(-1), _accel_sub(-1), _vcontrol_mode_sub(-1), @@ -352,7 +348,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _debug(false) { /* safely initialize structs */ - _att = {}; _ctrl_state = {}; _accel = {}; _att_sp = {}; @@ -610,7 +605,6 @@ FixedwingAttitudeControl::task_main() * do subscriptions */ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -640,7 +634,7 @@ FixedwingAttitudeControl::task_main() /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; - fds[1].fd = _att_sub; + fds[1].fd = _ctrl_state_sub; fds[1].events = POLLIN; _task_running = true; @@ -685,7 +679,6 @@ FixedwingAttitudeControl::task_main() deltaT = 0.01f; /* load local copies */ - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); @@ -939,18 +932,9 @@ FixedwingAttitudeControl::task_main() } /* Prepare speed_body_u and speed_body_w */ - float speed_body_u = 0.0f; - float speed_body_v = 0.0f; - float speed_body_w = 0.0f; - if(_att.R_valid) { - speed_body_u = _R(0, 0) * _global_pos.vel_n + _R(1, 0) * _global_pos.vel_e + _R(2, 0) * _global_pos.vel_d; - speed_body_v = _R(0, 1) * _global_pos.vel_n + _R(1, 1) * _global_pos.vel_e + _R(2, 1) * _global_pos.vel_d; - speed_body_w = _R(0, 2) * _global_pos.vel_n + _R(1, 2) * _global_pos.vel_e + _R(2, 2) * _global_pos.vel_d; - } else { - if (_debug && loop_counter % 10 == 0) { - warnx("Did not get a valid R\n"); - } - } + float speed_body_u = _R(0, 0) * _global_pos.vel_n + _R(1, 0) * _global_pos.vel_e + _R(2, 0) * _global_pos.vel_d; + float speed_body_v = _R(0, 1) * _global_pos.vel_n + _R(1, 1) * _global_pos.vel_e + _R(2, 1) * _global_pos.vel_d; + float speed_body_w = _R(0, 2) * _global_pos.vel_n + _R(1, 2) * _global_pos.vel_e + _R(2, 2) * _global_pos.vel_d; /* Prepare data for attitude controllers */ struct ECL_ControlData control_input = {}; From 1ec96d87d77da91325ad2822bef4a60e8857adf4 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Wed, 14 Oct 2015 10:58:58 +0200 Subject: [PATCH 014/161] fully ported control state into fw_pos_control_l1 --- .../fw_pos_control_l1_main.cpp | 58 +++++++------------ 1 file changed, 21 insertions(+), 37 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 48edca2b52..5ba05f7184 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -74,7 +74,6 @@ #include #include #include -#include #include #include #include @@ -152,7 +151,6 @@ private: int _global_pos_sub; int _pos_sp_triplet_sub; - int _att_sub; /**< vehicle attitude subscription */ int _ctrl_state_sub; /**< control state subscription */ int _control_mode_sub; /**< control mode subscription */ int _vehicle_status_sub; /**< vehicle status subscription */ @@ -164,7 +162,6 @@ private: orb_advert_t _tecs_status_pub; /**< TECS status publication */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ struct control_state_s _ctrl_state; /**< control state */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ @@ -221,6 +218,9 @@ private: float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s bool _global_pos_valid; ///< global position is valid math::Matrix<3, 3> _R_nb; ///< current attitude + float _roll; + float _pitch; + float _yaw; ECL_L1_Pos_Controller _l1_control; TECS _tecs; @@ -354,11 +354,6 @@ private: */ bool vehicle_manual_control_setpoint_poll(); - /** - * Check for position updates. - */ - void vehicle_attitude_poll(); - /** * Check for changes in control state. */ @@ -480,7 +475,6 @@ FixedwingPositionControl::FixedwingPositionControl() : /* subscriptions */ _global_pos_sub(-1), _pos_sp_triplet_sub(-1), - _att_sub(-1), _ctrl_state_sub(-1), _control_mode_sub(-1), _vehicle_status_sub(-1), @@ -494,7 +488,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _nav_capabilities_pub(nullptr), /* states */ - _att(), _ctrl_state(), _att_sp(), _nav_capabilities(), @@ -763,23 +756,6 @@ FixedwingPositionControl::vehicle_manual_control_setpoint_poll() return manual_updated; } - -void -FixedwingPositionControl::vehicle_attitude_poll() -{ - /* check if there is a new position */ - bool att_updated; - orb_check(_att_sub, &att_updated); - - if (att_updated) { - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); - - /* set rotation matrix */ - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - _R_nb(i, j) = PX4_R(_att.R, i, j); - } -} - void FixedwingPositionControl::control_state_poll() { @@ -800,6 +776,16 @@ FixedwingPositionControl::control_state_poll() } } + /* set rotation matrix and euler angles */ + math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + _R_nb = q_att.to_dcm(); + + math::Vector<3> euler_angles; + euler_angles = _R_nb.to_euler(); + _roll = euler_angles(0); + _pitch = euler_angles(1); + _yaw = euler_angles(2); + /* update TECS state */ _tecs.enable_airspeed(_airspeed_valid); } @@ -1122,7 +1108,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* reset hold altitude */ _hold_alt = _global_pos.alt; /* reset hold yaw */ - _hdg_hold_yaw = _att.yaw; + _hdg_hold_yaw = _yaw; /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); @@ -1216,14 +1202,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (pos_sp_triplet.previous.valid) { target_bearing = bearing_lastwp_currwp; } else { - target_bearing = _att.yaw; + target_bearing = _yaw; } mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold"); } -// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); +// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw)); - _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d); + _l1_control.navigate_heading(target_bearing, _yaw, ground_speed_2d); land_noreturn_horizontal = true; @@ -1464,7 +1450,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_control_mode_current != FW_POSCTRL_MODE_POSITION) { /* Need to init because last loop iteration was in a different mode */ _hold_alt = _global_pos.alt; - _hdg_hold_yaw = _att.yaw; + _hdg_hold_yaw = _yaw; _hdg_hold_enabled = false; // this makes sure the waypoints are reset below _yaw_lock_engaged = false; } @@ -1517,7 +1503,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH) { /* heading / roll is zero, lock onto current heading */ - if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { + if (fabsf(_ctrl_state.yaw_rate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { // little yaw movement, lock to current heading _yaw_lock_engaged = true; @@ -1536,7 +1522,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* just switched back from non heading-hold to heading hold */ if (!_hdg_hold_enabled) { _hdg_hold_enabled = true; - _hdg_hold_yaw = _att.yaw; + _hdg_hold_yaw = _yaw; get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true); } @@ -1695,7 +1681,6 @@ FixedwingPositionControl::task_main() */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -1776,7 +1761,6 @@ FixedwingPositionControl::task_main() // XXX add timestamp check _global_pos_valid = true; - vehicle_attitude_poll(); control_state_poll(); vehicle_setpoint_poll(); vehicle_sensor_combined_poll(); @@ -1898,7 +1882,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ _tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM)); /* Using tecs library */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, + _tecs.update_pitch_throttle(_R_nb, _pitch, altitude, alt_sp, v_sp, _ctrl_state.airspeed, eas2tas, climbout_mode, climbout_pitch_min_rad, throttle_min, throttle_max, throttle_cruise, From 104f38eea8ac4a40ae58948677845ea0072041c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Sep 2015 11:05:46 +0200 Subject: [PATCH 015/161] MC position controller: Guard against invalid setpoints --- .../mc_pos_control/mc_pos_control_main.cpp | 42 ++++++++++++++----- 1 file changed, 32 insertions(+), 10 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 36d87b9383..ecfb16e40f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -791,18 +791,45 @@ void MulticopterPositionControl::control_auto(float dt) } } + bool current_setpoint_valid = false; + bool previous_setpoint_valid = false; + + math::Vector<3> prev_sp; + math::Vector<3> curr_sp; + if (_pos_sp_triplet.current.valid) { - /* in case of interrupted mission don't go to waypoint but stay at current position */ - _reset_pos_sp = true; - _reset_alt_sp = true; /* project setpoint to local frame */ - math::Vector<3> curr_sp; map_projection_project(&_ref_pos, _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, &curr_sp.data[0], &curr_sp.data[1]); curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); + if (PX4_ISFINITE(curr_sp(0)) && + PX4_ISFINITE(curr_sp(1)) && + PX4_ISFINITE(curr_sp(2))) { + current_setpoint_valid = true; + } + } + + if (_pos_sp_triplet.previous.valid) { + map_projection_project(&_ref_pos, + _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, + &prev_sp.data[0], &prev_sp.data[1]); + prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt); + + if (PX4_ISFINITE(prev_sp(0)) && + PX4_ISFINITE(prev_sp(1)) && + PX4_ISFINITE(prev_sp(2))) { + previous_setpoint_valid = true; + } + } + + if (current_setpoint_valid) { + /* in case of interrupted mission don't go to waypoint but stay at current position */ + _reset_pos_sp = true; + _reset_alt_sp = true; + /* scaled space: 1 == position error resulting max allowed speed */ math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here @@ -812,13 +839,8 @@ void MulticopterPositionControl::control_auto(float dt) /* by default use current setpoint as is */ math::Vector<3> pos_sp_s = curr_sp_s; - if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) { + if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && previous_setpoint_valid) { /* follow "previous - current" line */ - math::Vector<3> prev_sp; - map_projection_project(&_ref_pos, - _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, - &prev_sp.data[0], &prev_sp.data[1]); - prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt); if ((curr_sp - prev_sp).length() > MIN_DIST) { From efe82194af9655e5e14a733a5ab8fc9dd784c126 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Fri, 16 Oct 2015 16:03:25 +0200 Subject: [PATCH 016/161] ported control state attitude into mc_pos_ctrl --- .../mc_pos_control/mc_pos_control_main.cpp | 46 ++++++++++++------- 1 file changed, 30 insertions(+), 16 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 36d87b9383..95ba66361a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -68,7 +68,7 @@ #include #include #include -#include +#include #include #include #include @@ -126,7 +126,7 @@ private: int _mavlink_fd; /**< mavlink fd */ int _vehicle_status_sub; /**< vehicle status subscription */ - int _att_sub; /**< vehicle attitude subscription */ + int _ctrl_state_sub; /**< control state subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _control_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ @@ -142,7 +142,7 @@ private: orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ struct vehicle_status_s _vehicle_status; /**< vehicle status */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct control_state_s _ctrl_state; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ @@ -215,6 +215,9 @@ private: math::Vector<3> _vel_ff; math::Vector<3> _sp_move_rate; + math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */ + float _yaw; /**< yaw angle (euler) */ + /** * Update our local parameter cache. */ @@ -304,7 +307,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _mavlink_fd(-1), /* subscriptions */ - _att_sub(-1), + _ctrl_state_sub(-1), _att_sp_sub(-1), _control_mode_sub(-1), _params_sub(-1), @@ -325,10 +328,11 @@ MulticopterPositionControl::MulticopterPositionControl() : _reset_pos_sp(true), _reset_alt_sp(true), - _mode_auto(false) + _mode_auto(false), + _yaw(0.0f) { memset(&_vehicle_status, 0, sizeof(_vehicle_status)); - memset(&_att, 0, sizeof(_att)); + memset(&_ctrl_state, 0, sizeof(_ctrl_state)); memset(&_att_sp, 0, sizeof(_att_sp)); memset(&_manual, 0, sizeof(_manual)); memset(&_control_mode, 0, sizeof(_control_mode)); @@ -356,6 +360,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel_ff.zero(); _sp_move_rate.zero(); + _R.identity(); + _params_handles.thr_min = param_find("MPC_THR_MIN"); _params_handles.thr_max = param_find("MPC_THR_MAX"); _params_handles.z_p = param_find("MPC_Z_P"); @@ -493,10 +499,10 @@ MulticopterPositionControl::poll_subscriptions() orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); } - orb_check(_att_sub, &updated); + orb_check(_ctrl_state_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); } orb_check(_att_sp_sub, &updated); @@ -923,7 +929,7 @@ MulticopterPositionControl::task_main() * do subscriptions */ _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -978,6 +984,14 @@ MulticopterPositionControl::task_main() } poll_subscriptions(); + + /* get current rotation matrix and euler angles from control state quaternions */ + math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + _R = q_att.to_dcm(); + math::Vector<3> euler_angles; + euler_angles = _R.to_euler(); + _yaw = euler_angles(2); + parameters_update(false); hrt_abstime t = hrt_absolute_time(); @@ -1046,7 +1060,7 @@ MulticopterPositionControl::task_main() _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; - _att_sp.yaw_body = _att.yaw; + _att_sp.yaw_body = _yaw; _att_sp.thrust = 0.0f; _att_sp.timestamp = hrt_absolute_time(); @@ -1216,11 +1230,11 @@ MulticopterPositionControl::task_main() /* thrust compensation for altitude only control mode */ float att_comp; - if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) { - att_comp = 1.0f / PX4_R(_att.R, 2, 2); + if (_R(2, 2) > TILT_COS_MAX) { + att_comp = 1.0f / _R(2, 2); - } else if (PX4_R(_att.R, 2, 2) > 0.0f) { - att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f; + } else if (_R(2, 2) > 0.0f) { + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _R(2, 2) + 1.0f; saturation_z = true; } else { @@ -1407,7 +1421,7 @@ MulticopterPositionControl::task_main() /* reset yaw setpoint to current position if needed */ if (reset_yaw_sp) { reset_yaw_sp = false; - _att_sp.yaw_body = _att.yaw; + _att_sp.yaw_body = _yaw; } /* do not move yaw while arming */ @@ -1417,7 +1431,7 @@ MulticopterPositionControl::task_main() _att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max; float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt); - float yaw_offs = _wrap_pi(yaw_target - _att.yaw); + float yaw_offs = _wrap_pi(yaw_target - _yaw); // If the yaw offset became too big for the system to track stop // shifting it From 560efcbc5bc6de5b1e3aae9214cef4039f10a9aa Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Wed, 21 Oct 2015 13:57:26 +0200 Subject: [PATCH 017/161] added all platforms to px4_includes --- src/platforms/px4_includes.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 7437a9405a..0286705968 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -58,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -121,6 +122,7 @@ #include #include #include +#include #include #include #include @@ -151,6 +153,7 @@ #include #include #include +#include #include #include #include From 66785916d06047440957a2c66a48d4093a4b4635 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Thu, 22 Oct 2015 14:03:58 +0200 Subject: [PATCH 018/161] several first changes to optical flow estimation (lidar, gyro comp., weights). --- .../position_estimator_inav/CMakeLists.txt | 2 +- .../position_estimator_inav_main.c | 172 +++++++++++++----- .../position_estimator_inav_params.c | 32 ++-- .../position_estimator_inav_params.h | 12 +- 4 files changed, 150 insertions(+), 68 deletions(-) diff --git a/src/modules/position_estimator_inav/CMakeLists.txt b/src/modules/position_estimator_inav/CMakeLists.txt index 90ab4ad74c..ed5c05c870 100644 --- a/src/modules/position_estimator_inav/CMakeLists.txt +++ b/src/modules/position_estimator_inav/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ if(${OS} STREQUAL "nuttx") - list(APPEND MODULE_CFLAGS -Wframe-larger-than=3800) + list(APPEND MODULE_CFLAGS -Wframe-larger-than=3850) endif() px4_add_module( MODULE modules__position_estimator_inav 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 53a7c678f6..ad5698989e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -38,7 +38,6 @@ * @author Anton Babushkin * @author Nuno Marques */ - #include #include #include @@ -66,6 +65,7 @@ #include #include #include +#include #include #include #include @@ -90,8 +90,8 @@ static const hrt_abstime vision_topic_timeout = 500000; // Vision topic timeout static const hrt_abstime mocap_topic_timeout = 500000; // Mocap topic timeout = 0.5s static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5s static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s -static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms -static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss +static const hrt_abstime lidar_timeout = 150000; // lidar timeout = 150ms +static const hrt_abstime lidar_valid_timeout = 1000000; // estimate lidar distance during this time after lidar loss static const unsigned updates_counter_len = 1000000; static const float max_flow = 1.0f; // max flow value that can be used, rad/s @@ -311,19 +311,30 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { 0.0f }, // D (pos) }; - float corr_sonar = 0.0f; - float corr_sonar_filtered = 0.0f; + float corr_lidar = 0.0f; + float corr_lidar_filtered = 0.0f; float corr_flow[] = { 0.0f, 0.0f }; // N E float w_flow = 0.0f; - float sonar_prev = 0.0f; + float lidar_prev = 0.0f; //hrt_abstime flow_prev = 0; // time of last flow measurement - hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) - hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) + hrt_abstime lidar_time = 0; // time of last lidar measurement (not filtered) + hrt_abstime lidar_valid_time = 0; // time of last lidar measurement used for correction (filtered) + + //----------test-------------------------- + float flow_test[] = { 0.0f, 0.0f }; + float pos_test[] = { 0.0f, 0.0f }; + float flow_test_average[] = { 0.0f, 0.0f }; + int n_flow = 0; + float gyro_offset_filtered[] = { 0.0f, 0.0f }; + float flow_gyrospeed[] = { 0.0f, 0.0f }; + float flow_gyrospeed_filtered[] = { 0.0f, 0.0f }; + float att_gyrospeed_filtered[] = { 0.0f, 0.0f }; + //---------------------------------------- bool gps_valid = false; // GPS is valid - bool sonar_valid = false; // sonar is valid + bool lidar_valid = false; // lidar is valid bool flow_valid = false; // flow is valid bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false) bool vision_valid = false; // vision is valid @@ -352,6 +363,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&mocap, 0, sizeof(mocap)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); + struct distance_sensor_s lidar; + memset(&lidar, 0, sizeof(lidar)); /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -364,6 +377,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate)); int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); int home_position_sub = orb_subscribe(ORB_ID(home_position)); + int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor)); /* advertise */ orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); @@ -445,6 +459,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) attitude_updates++; bool updated; + bool updated2; /* parameter update */ orb_check(parameter_update_sub, &updated); @@ -490,6 +505,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j]; } } + //------------test---------------------- + //dont convert into NED for tests + //acc[0] = sensor.accelerometer_m_s2[0]; + //acc[1] = sensor.accelerometer_m_s2[1]; + //acc[2] = sensor.accelerometer_m_s2[2]; + //--------------------------------------- acc[2] += CONSTANTS_ONE_G; @@ -510,56 +531,60 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* optical flow */ orb_check(optical_flow_sub, &updated); + orb_check(distance_sensor_sub, &updated2); - if (updated) { + if (updated && updated2) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); + orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar); /* calculate time from previous update */ // float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; // flow_prev = flow.flow_timestamp; - if ((flow.ground_distance_m > 0.31f) && - (flow.ground_distance_m < 4.0f) && - (PX4_R(att.R, 2, 2) > 0.7f) && - (fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) { + if ((lidar.current_distance > 0.21f) && + (lidar.current_distance < 4.0f) && + /*(PX4_R(att.R, 2, 2) > 0.7f) &&*/ + (fabsf(lidar.current_distance - lidar_prev) > FLT_EPSILON)) { - sonar_time = t; - sonar_prev = flow.ground_distance_m; - corr_sonar = flow.ground_distance_m + surface_offset + z_est[0]; - corr_sonar_filtered += (corr_sonar - corr_sonar_filtered) * params.sonar_filt; + lidar_time = t; + lidar_prev = lidar.current_distance; + corr_lidar = lidar.current_distance + surface_offset + z_est[0]; + corr_lidar_filtered += (corr_lidar - corr_lidar_filtered) * params.lidar_filt; - if (fabsf(corr_sonar) > params.sonar_err) { + if (fabsf(corr_lidar) > params.lidar_err) { /* correction is too large: spike or new ground level? */ - if (fabsf(corr_sonar - corr_sonar_filtered) > params.sonar_err) { + if (fabsf(corr_lidar - corr_lidar_filtered) > params.lidar_err) { /* spike detected, ignore */ - corr_sonar = 0.0f; - sonar_valid = false; + corr_lidar = 0.0f; + lidar_valid = false; } else { /* new ground level */ - surface_offset -= corr_sonar; + surface_offset -= corr_lidar; surface_offset_rate = 0.0f; - corr_sonar = 0.0f; - corr_sonar_filtered = 0.0f; - sonar_valid_time = t; - sonar_valid = true; + corr_lidar = 0.0f; + corr_lidar_filtered = 0.0f; + lidar_valid_time = t; + lidar_valid = true; local_pos.surface_bottom_timestamp = t; mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset); } } else { /* correction is ok, use it */ - sonar_valid_time = t; - sonar_valid = true; + lidar_valid_time = t; + lidar_valid = true; } } float flow_q = flow.quality / 255.0f; - float dist_bottom = - z_est[0] - surface_offset; + float dist_bottom = - z_est[0] - surface_offset; //lidar.current_distance; - if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f) { + if (dist_bottom > 0.21f && flow_q > params.flow_q_min /*&& (t < lidar_valid_time + lidar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f*/) { /* distance to surface */ - float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); + //float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonar + float flow_dist = dist_bottom; //use this if using lidar + /* check if flow if too large for accurate measurements */ /* calculate estimated velocity in body frame */ float body_v_est[2] = { 0.0f, 0.0f }; @@ -571,12 +596,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* set this flag if flow should be accurate according to current velocity and attitude rate estimate */ flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; + //this flag is not working --> + flow_accurate = true; //already checked if flow_q > 0.3 /* convert raw flow to angular flow (rad/s) */ float flow_ang[2]; - //todo check direction of x und y axis - flow_ang[0] = flow.pixel_flow_x_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; - flow_ang[1] = flow.pixel_flow_y_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; + //calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro) + flow_ang[0] = (flow.pixel_flow_x_integral - flow.gyro_x_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[0];//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; + flow_ang[1] = (flow.pixel_flow_y_integral - flow.gyro_y_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[1];//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; /* flow measurements vector */ float flow_m[3]; flow_m[0] = -flow_ang[0] * flow_dist; @@ -592,6 +619,43 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } + //-----------test------------------- + //do not transform to NED for testing + //flow_v[0] = flow_m[0]; + //flow_v[1] = flow_m[1]; + + flow_test[0] = flow_v[0]; + flow_test[1] = flow_v[1]; + + flow_test_average[0] = (flow.pixel_flow_x_integral + flow_test_average[0] * n_flow) / (n_flow + 1); + flow_test_average[1] = (flow.pixel_flow_y_integral + flow_test_average[1] * n_flow) / (n_flow + 1); + + //calculate offset of flow-gyro using already calibrated gyro from autopilot + flow_gyrospeed[0] = flow.gyro_x_rate_integral/(float)flow.integration_timespan*1000000.0f; + flow_gyrospeed[1] = flow.gyro_y_rate_integral/(float)flow.integration_timespan*1000000.0f; + + + + //moving average + if (n_flow >= 100) { + gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0]; + gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1]; + n_flow = 0; + flow_gyrospeed_filtered[0] = 0.0f; + flow_gyrospeed_filtered[1] = 0.0f; + att_gyrospeed_filtered[0] = 0.0f; + att_gyrospeed_filtered[1] = 0.0f; + } else { + flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1); + flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1); + att_gyrospeed_filtered[0] = (att.rollspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1); + att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1); + n_flow++; + //mavlink_log_info(mavlink_fd, "n_flow = %d\n", (int)n_flow); + } + + //---------------------------------- + /* velocity correction */ corr_flow[0] = flow_v[0] - x_est[1]; corr_flow[1] = flow_v[1] - y_est[1]; @@ -599,12 +663,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist); + /* if flow is not accurate, reduce weight for it */ // TODO make this more fuzzy if (!flow_accurate) { w_flow *= 0.05f; } + //--------test------------- + //mavlink_log_info(mavlink_fd, "flow_accurate = %d\t w_flow = %4.4f\n", (int)flow_accurate, (double)w_flow); + //w_flow = 0.8; + //------------------------- + /* under ideal conditions, on 1m distance assume EPH = 10cm */ eph_flow = 0.1f / w_flow; @@ -862,9 +932,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* check for timeout on FLOW topic */ - if ((flow_valid || sonar_valid) && t > flow.timestamp + flow_topic_timeout) { + if ((flow_valid || lidar_valid) && t > flow.timestamp + flow_topic_timeout) { flow_valid = false; - sonar_valid = false; + lidar_valid = false; warnx("FLOW timeout"); mavlink_log_info(mavlink_fd, "[inav] FLOW timeout"); } @@ -890,10 +960,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[inav] MOCAP timeout"); } - /* check for sonar measurement timeout */ - if (sonar_valid && (t > (sonar_time + sonar_timeout))) { - corr_sonar = 0.0f; - sonar_valid = false; + /* check for lidar measurement timeout */ + if (lidar_valid && (t > (lidar_time + lidar_timeout))) { + corr_lidar = 0.0f; + lidar_valid = false; } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; @@ -921,16 +991,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap; - bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); + bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout); if (dist_bottom_valid) { /* surface distance prediction */ surface_offset += surface_offset_rate * dt; /* surface distance correction */ - if (sonar_valid) { - surface_offset_rate -= corr_sonar * 0.5f * params.w_z_sonar * params.w_z_sonar * dt; - surface_offset -= corr_sonar * params.w_z_sonar * dt; + if (lidar_valid) { + surface_offset_rate -= corr_lidar * 0.5f * params.w_z_lidar * params.w_z_lidar * dt; + surface_offset -= corr_lidar * params.w_z_lidar * dt; } } @@ -1114,6 +1184,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow); inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow); + //mavlink_log_info(mavlink_fd, "w_flow = %2.4f\t w_xy_flow = %2.4f\n", (double)w_flow, (double)params.w_xy_flow); } if (use_gps_xy) { @@ -1211,16 +1282,27 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) buf_ptr = 0; } + //-------------test-------------- + pos_test[0] += flow_test[0] * dt; + pos_test[1] += flow_test[1] * dt; + //------------------------------- + /* publish local position */ local_pos.xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy; local_pos.xy_global = local_pos.xy_valid && use_gps_xy; local_pos.z_global = local_pos.z_valid && use_gps_z; + local_pos.x = corr_baro; //test local_pos.x = x_est[0]; + local_pos.vx = surface_offset; // flow_test[0]; //test local_pos.vx = x_est[1]; + local_pos.y = acc_bias[2]; //test local_pos.y = y_est[0]; + local_pos.vy = flow_gyrospeed_filtered[0]; //test local_pos.vy = y_est[1]; + local_pos.z = lidar.current_distance; //flow_test[0]; //test local_pos.z = z_est[0]; + local_pos.vz = - z_est[0] - surface_offset; //flow_test[1]; //test local_pos.vz = z_est[1]; local_pos.yaw = att.yaw; local_pos.dist_bottom_valid = dist_bottom_valid; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 90fb472952..acda5ffc70 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -86,15 +86,15 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f); PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 5.0f); /** - * Z axis weight for sonar + * Z axis weight for lidar * - * Weight (cutoff frequency) for sonar measurements. + * Weight (cutoff frequency) for lidar measurements. * * @min 0.0 * @max 10.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); +PARAM_DEFINE_FLOAT(INAV_W_Z_LIDAR, 3.0f); /** * XY axis weight for GPS position @@ -158,10 +158,10 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f); * Weight (cutoff frequency) for optical flow (velocity) measurements. * * @min 0.0 - * @max 10.0 + * @max 30.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 10.0f); /** * XY axis weight for resetting velocity @@ -217,18 +217,18 @@ PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f); * @max 1.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f); +PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.3f); /** - * Weight for sonar filter + * Weight for lidar filter * - * Sonar filter detects spikes on sonar measurements and used to detect new surface level. + * Lidar filter detects spikes on lidar measurements and used to detect new surface level. * * @min 0.0 * @max 1.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); +PARAM_DEFINE_FLOAT(INAV_LIDAR_FILT, 0.05f); /** * Sonar maximal error for new surface @@ -240,7 +240,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); * @unit m * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); +PARAM_DEFINE_FLOAT(INAV_LIDAR_ERR, 0.5f); /** * Land detector time @@ -319,7 +319,7 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h) h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); h->w_z_gps_v = param_find("INAV_W_Z_GPS_V"); h->w_z_vision_p = param_find("INAV_W_Z_VIS_P"); - h->w_z_sonar = param_find("INAV_W_Z_SONAR"); + h->w_z_lidar = param_find("INAV_W_Z_LIDAR"); h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P"); @@ -331,8 +331,8 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h) h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); h->flow_k = param_find("INAV_FLOW_K"); h->flow_q_min = param_find("INAV_FLOW_Q_MIN"); - h->sonar_filt = param_find("INAV_SONAR_FILT"); - h->sonar_err = param_find("INAV_SONAR_ERR"); + h->lidar_filt = param_find("INAV_LIDAR_FILT"); + h->lidar_err = param_find("INAV_LIDAR_ERR"); h->land_t = param_find("INAV_LAND_T"); h->land_disp = param_find("INAV_LAND_DISP"); h->land_thr = param_find("INAV_LAND_THR"); @@ -347,7 +347,7 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h param_get(h->w_z_baro, &(p->w_z_baro)); param_get(h->w_z_gps_p, &(p->w_z_gps_p)); param_get(h->w_z_vision_p, &(p->w_z_vision_p)); - param_get(h->w_z_sonar, &(p->w_z_sonar)); + param_get(h->w_z_lidar, &(p->w_z_lidar)); param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); param_get(h->w_xy_vision_p, &(p->w_xy_vision_p)); @@ -359,8 +359,8 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h param_get(h->w_acc_bias, &(p->w_acc_bias)); param_get(h->flow_k, &(p->flow_k)); param_get(h->flow_q_min, &(p->flow_q_min)); - param_get(h->sonar_filt, &(p->sonar_filt)); - param_get(h->sonar_err, &(p->sonar_err)); + param_get(h->lidar_filt, &(p->lidar_filt)); + param_get(h->lidar_err, &(p->lidar_err)); param_get(h->land_t, &(p->land_t)); param_get(h->land_disp, &(p->land_disp)); param_get(h->land_thr, &(p->land_thr)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 9adc27d7c0..434e3dfe0a 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -46,7 +46,7 @@ struct position_estimator_inav_params { float w_z_gps_p; float w_z_gps_v; float w_z_vision_p; - float w_z_sonar; + float w_z_lidar; float w_xy_gps_p; float w_xy_gps_v; float w_xy_vision_p; @@ -58,8 +58,8 @@ struct position_estimator_inav_params { float w_acc_bias; float flow_k; float flow_q_min; - float sonar_filt; - float sonar_err; + float lidar_filt; + float lidar_err; float land_t; float land_disp; float land_thr; @@ -72,7 +72,7 @@ struct position_estimator_inav_param_handles { param_t w_z_gps_p; param_t w_z_gps_v; param_t w_z_vision_p; - param_t w_z_sonar; + param_t w_z_lidar; param_t w_xy_gps_p; param_t w_xy_gps_v; param_t w_xy_vision_p; @@ -84,8 +84,8 @@ struct position_estimator_inav_param_handles { param_t w_acc_bias; param_t flow_k; param_t flow_q_min; - param_t sonar_filt; - param_t sonar_err; + param_t lidar_filt; + param_t lidar_err; param_t land_t; param_t land_disp; param_t land_thr; From 55cf08d3830aef57f927a9f325cdb2a08434aadf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Oct 2015 17:47:22 +0200 Subject: [PATCH 019/161] Commander: prevent the user from arming the system when USB was ever connected --- src/modules/commander/commander.cpp | 12 ++++++++++++ src/modules/commander/state_machine_helper.cpp | 9 ++++++++- 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 79d0811b72..b6a2846773 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1494,6 +1494,18 @@ int commander_thread_main(int argc, char *argv[]) /* copy avionics voltage */ status.avionics_power_rail_voltage = system_power.voltage5V_v; + + if (status.usb_connected && !system_power.usb_connected) { + /* + * apparently the USB cable went away but we are still powered, + * so lets reset to a classic non-usb state. + */ + usleep(100000); + mavlink_log_critical(mavlink_fd, "USB disconnected, rebooting.") + usleep(400000); + px4_systemreset(false); + } + status.usb_connected = system_power.usb_connected; } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index d44f1f3506..3b169068d4 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -737,5 +737,12 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) checkAirspeed = true; } - return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true); + bool prearm_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true); + + if (status->usb_connected) { + prearm_ok = false; + mavlink_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); + } + + return !prearm_ok; } From 09fcbde52b7b75d13ecec16358f1890e2ed4b6a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 11:58:30 +0200 Subject: [PATCH 020/161] Make some space in flash --- cmake/configs/nuttx_px4fmu-v2_default.cmake | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index f3638c5749..0c6458cad1 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -37,7 +37,7 @@ set(config_module_list drivers/meas_airspeed drivers/frsky_telemetry modules/sensors - drivers/mkblctrl + #drivers/mkblctrl drivers/px4flow drivers/oreoled drivers/gimbal @@ -54,7 +54,7 @@ set(config_module_list systemcmds/pwm systemcmds/esc_calib systemcmds/reboot - systemcmds/topic_listener + #systemcmds/topic_listener systemcmds/top systemcmds/config systemcmds/nshterm From 9239a93a71681bca31d6fcbfd6436906b9e9f17e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 11:59:00 +0200 Subject: [PATCH 021/161] Include both ESC images in standard binary --- .travis.yml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.travis.yml b/.travis.yml index 3786602b09..9d89caca85 100644 --- a/.travis.yml +++ b/.travis.yml @@ -100,6 +100,14 @@ script: - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default - echo 'Running Tests..' && make px4fmu-v2_default test + - echo 'Building UAVCAN node firmware..' && git clone https://github.com/thiemar/vectorcontrol + - cd vectorcontrol + - BOARD=s2740vc_1_0 make && BOARD=px4esc_1_6 make + - cd .. + - mkdir -p ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/ + - mkdir -p ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/ + - cp vectorcontrol/firmware/com.thiemar.s2740vc-v1-1.0.00000000.uavcan.bin ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/00000000.uavcan.bin + - cp vectorcontrol/firmware/org.pixhawk.px4esc-v1-1.0.00000000.uavcan.bin ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/00000000.uavcan.bin after_success: - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then From 96968911e4a246a2d5e700de7132d3a404069a15 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 12:12:18 +0200 Subject: [PATCH 022/161] Keep size command in ccache path env --- .travis.yml | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/.travis.yml b/.travis.yml index 9d89caca85..e0d40aebae 100644 --- a/.travis.yml +++ b/.travis.yml @@ -73,6 +73,7 @@ before_script: - mkdir -p ~/bin - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-g++ - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-gcc + - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-size - ln -s /usr/bin/ccache ~/bin/clang++ - ln -s /usr/bin/ccache ~/bin/clang++-3.4 - ln -s /usr/bin/ccache ~/bin/clang++-3.5 @@ -96,10 +97,7 @@ script: - make check_format - arm-none-eabi-gcc --version - echo 'Building POSIX Firmware..' && make posix_sitl_simple - - echo 'Running Tests..' && make posix_sitl_simple test && cat build_posix_sitl_simple/src/modules/systemlib/mixer/mixer_multirotor.generated.h - - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default - - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default - - echo 'Running Tests..' && make px4fmu-v2_default test + - echo 'Running Tests..' && make posix_sitl_simple test - echo 'Building UAVCAN node firmware..' && git clone https://github.com/thiemar/vectorcontrol - cd vectorcontrol - BOARD=s2740vc_1_0 make && BOARD=px4esc_1_6 make @@ -108,6 +106,9 @@ script: - mkdir -p ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/ - cp vectorcontrol/firmware/com.thiemar.s2740vc-v1-1.0.00000000.uavcan.bin ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/00000000.uavcan.bin - cp vectorcontrol/firmware/org.pixhawk.px4esc-v1-1.0.00000000.uavcan.bin ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/00000000.uavcan.bin + - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default + - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default + - echo 'Running Tests..' && make px4fmu-v2_default test after_success: - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then From 4d6094cf70d2c4b959187bec824945fd2429aa45 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 12:22:45 +0200 Subject: [PATCH 023/161] Add objcopy --- .travis.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.travis.yml b/.travis.yml index e0d40aebae..70342e4ef3 100644 --- a/.travis.yml +++ b/.travis.yml @@ -74,6 +74,7 @@ before_script: - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-g++ - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-gcc - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-size + - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-objcopy - ln -s /usr/bin/ccache ~/bin/clang++ - ln -s /usr/bin/ccache ~/bin/clang++-3.4 - ln -s /usr/bin/ccache ~/bin/clang++-3.5 From 89e6a80fd6b8fc6bc3279a2367b5b5eeb837f9dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Oct 2015 09:43:21 +0100 Subject: [PATCH 024/161] Fix path according to Ben's instructions --- .travis.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index 70342e4ef3..d9b5b4e75a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -105,8 +105,8 @@ script: - cd .. - mkdir -p ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/ - mkdir -p ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/ - - cp vectorcontrol/firmware/com.thiemar.s2740vc-v1-1.0.00000000.uavcan.bin ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/00000000.uavcan.bin - - cp vectorcontrol/firmware/org.pixhawk.px4esc-v1-1.0.00000000.uavcan.bin ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/00000000.uavcan.bin + - cp vectorcontrol/firmware/com.thiemar.s2740vc-v1-1.0.00000000.uavcan.bin ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/00000000.bin + - cp vectorcontrol/firmware/org.pixhawk.px4esc-v1-1.0.00000000.uavcan.bin ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/00000000.bin - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default - echo 'Running Tests..' && make px4fmu-v2_default test From 327481d749f2042fab121aa3949ca2eb995c17fe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Oct 2015 11:58:11 +0100 Subject: [PATCH 025/161] Fix EKF frame size flag use --- src/modules/ekf_att_pos_estimator/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/CMakeLists.txt b/src/modules/ekf_att_pos_estimator/CMakeLists.txt index fc7fefd2bf..13eaeb6d2b 100644 --- a/src/modules/ekf_att_pos_estimator/CMakeLists.txt +++ b/src/modules/ekf_att_pos_estimator/CMakeLists.txt @@ -32,7 +32,7 @@ ############################################################################ set(MODULE_CFLAGS ) -if(NOT ${OS} STREQUAL "qurt") +if(${OS} STREQUAL "nuttx") list(APPEND MODULE_CFLAGS -Wframe-larger-than=3400) endif() From 0c3f51944500b713eebba524e2a072b77fb47a65 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 26 Oct 2015 12:18:24 +0100 Subject: [PATCH 026/161] renamed SITL startup scripts --- posix-configs/SITL/init/{rcS_iris_gazebo => rcS_gazebo} | 0 posix-configs/SITL/init/{rcS => rcS_jmavsim} | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename posix-configs/SITL/init/{rcS_iris_gazebo => rcS_gazebo} (100%) rename posix-configs/SITL/init/{rcS => rcS_jmavsim} (100%) diff --git a/posix-configs/SITL/init/rcS_iris_gazebo b/posix-configs/SITL/init/rcS_gazebo similarity index 100% rename from posix-configs/SITL/init/rcS_iris_gazebo rename to posix-configs/SITL/init/rcS_gazebo diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS_jmavsim similarity index 100% rename from posix-configs/SITL/init/rcS rename to posix-configs/SITL/init/rcS_jmavsim From 1767acd1e9966d198916a84a1421f630b229d241 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 26 Oct 2015 12:23:22 +0100 Subject: [PATCH 027/161] use correct startup scipt depending on simulator --- Tools/sitl_run.sh | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index f13ed94509..fff0080ca9 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -68,12 +68,12 @@ touch rootfs/eeprom/parameters # Start Java simulator if [ "$debugger" == "lldb" ] then - lldb -- mainapp ../../../../$rc_script + lldb -- mainapp ../../../../${rc_script}_${program} elif [ "$debugger" == "gdb" ] then - gdb --args mainapp ../../../../$rc_script + gdb --args mainapp ../../../../${rc_script}_${program} else - ./mainapp ../../../../$rc_script + ./mainapp ../../../../${rc_script}_${program} fi if [ "$3" == "jmavsim" ] From 1e7da5064f726529be58c561d722b7e9733857f5 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Mon, 26 Oct 2015 15:59:36 -0400 Subject: [PATCH 028/161] Fixed LPE bug for accel bias estimation. --- .../local_position_estimator/BlockLocalPositionEstimator.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 08cf51fb5c..f3295e1fc9 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -749,8 +749,7 @@ void BlockLocalPositionEstimator::predict() if (_integrate.get() && _sub_att.get().R_valid) { Matrix3f R_att(_sub_att.get().R); Vector3f a(_sub_sensor.get().accelerometer_m_s2); - Vector3f b(_x(X_bx), _x(X_by), _x(X_bz)); - _u = R_att * (a - b); + _u = R_att * a; _u(U_az) += 9.81f; // add g } else { From 4241e526aac9fad483949ce6b7cf03597acbd9b6 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Mon, 26 Oct 2015 16:03:22 -0400 Subject: [PATCH 029/161] Updated sitl scripts for LPE. --- Tools/sitl_run.sh | 2 +- posix-configs/SITL/init/rcS_lpe_gazebo | 66 +++++++++++++++++++ .../SITL/init/{rcS_lpe => rcS_lpe_jmavsim} | 0 3 files changed, 67 insertions(+), 1 deletion(-) create mode 100644 posix-configs/SITL/init/rcS_lpe_gazebo rename posix-configs/SITL/init/{rcS_lpe => rcS_lpe_jmavsim} (100%) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index fff0080ca9..6eb69eb46b 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -38,7 +38,7 @@ then cd Tools/jMAVSim ant java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 & - SIM_PID=echo $! + SIM_PID=`echo $!` elif [ "$3" == "gazebo" ] then if [ -x "$(command -v gazebo)" ] diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo b/posix-configs/SITL/init/rcS_lpe_gazebo new file mode 100644 index 0000000000..6093991c13 --- /dev/null +++ b/posix-configs/SITL/init/rcS_lpe_gazebo @@ -0,0 +1,66 @@ +uorb start +simulator start -s +param load +param set MAV_TYPE 2 +param set MC_PITCHRATE_P 0.3 +param set MC_ROLLRATE_P 0.3 +param set MC_YAW_P 2.0 +param set MC_YAWRATE_P 0.35 +param set SYS_AUTOSTART 4010 +param set SYS_RESTART_TYPE 2 +dataman start +param set CAL_GYRO0_ID 2293760 +param set CAL_ACC0_ID 1376256 +param set CAL_ACC1_ID 1310720 +param set CAL_MAG0_ID 196608 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_ACC0_XOFF 1 +param set CAL_ACC0_YOFF 2 +param set CAL_ACC0_ZOFF 3 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_XOFF 0.01 +param set CAL_MAG0_XOFF 0.01 +param set MPC_XY_P 0.15 +param set MPC_XY_VEL_P 0.05 +param set MPC_XY_VEL_D 0.005 +param set MPC_XY_FF 0.1 +param set SENS_BOARD_ROT 8 + +# changes for LPE +param set COM_RC_IN_MODE 1 +param set LPE_BETA_MAX 10000 + +rgbled start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start multicopter +navigator start +attitude_estimator_q start +mc_pos_control start +mc_att_control start +mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix +mavlink start -u 14556 -r 2000000 +mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556 +mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 80 -s ATTITUDE -u 14556 +mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 20 -s RC_CHANNELS -u 14556 +mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +mavlink boot_complete +sdlog2 start -r 100 -e -t -a + +# start LPE at end, when we know it is ok to init sensors +sleep 5 +local_position_estimator start diff --git a/posix-configs/SITL/init/rcS_lpe b/posix-configs/SITL/init/rcS_lpe_jmavsim similarity index 100% rename from posix-configs/SITL/init/rcS_lpe rename to posix-configs/SITL/init/rcS_lpe_jmavsim From 6fd1daf279a912b4821125161b90342295a34db3 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 27 Sep 2015 19:49:01 -0400 Subject: [PATCH 030/161] delete unused function declaration --- src/modules/commander/commander.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 79d0811b72..62a187ff93 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -252,9 +252,6 @@ void print_reject_arm(const char *msg); void print_status(); -transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, - struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); - transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy); /** From a2ba34d1aedfc84b8ece97ebdaa9d17781ad07aa Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 27 Sep 2015 19:49:01 -0400 Subject: [PATCH 031/161] geofence violation actions --- msg/geofence_result.msg | 7 ++ src/modules/commander/commander.cpp | 146 ++++++++++++++++++----- src/modules/navigator/geofence.cpp | 28 ++--- src/modules/navigator/geofence.h | 14 ++- src/modules/navigator/geofence_params.c | 12 +- src/modules/navigator/navigator_main.cpp | 6 +- 6 files changed, 149 insertions(+), 64 deletions(-) diff --git a/msg/geofence_result.msg b/msg/geofence_result.msg index 7fc21c2af8..165d92b989 100644 --- a/msg/geofence_result.msg +++ b/msg/geofence_result.msg @@ -1 +1,8 @@ +uint8 GF_ACTION_NONE = 0 # no action on geofence violation +uint8 GF_ACTION_WARN = 1 # critical mavlink message +uint8 GF_ACTION_LOITER = 2 # switch to AUTO|LOITER +uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL +uint8 GF_ACTION_TERMINATE = 4 # flight termination + bool geofence_violated # true if the geofence is violated +uint8 geofence_action # action to take when geofence is violated \ No newline at end of file diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 62a187ff93..3395b8c069 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -195,13 +195,7 @@ static struct offboard_control_mode_s offboard_control_mode; static struct home_position_s _home; static unsigned _last_mission_instance = 0; -static uint64_t last_manual_input = 0; -static switch_pos_t last_offboard_switch = 0; -static switch_pos_t last_return_switch = 0; -static switch_pos_t last_mode_switch = 0; -static switch_pos_t last_acro_switch = 0; -static switch_pos_t last_posctl_switch = 0; -static switch_pos_t last_loiter_switch = 0; +static manual_control_setpoint_s _last_sp_man; struct vtol_vehicle_status_s vtol_status; @@ -922,6 +916,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_rc_in_off = param_find("COM_RC_IN_MODE"); param_t _param_eph = param_find("COM_HOME_H_T"); param_t _param_epv = param_find("COM_HOME_V_T"); + param_t _param_geofence_action = param_find("GF_ACTION"); // const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX]; // main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; @@ -1260,6 +1255,8 @@ int commander_thread_main(int argc, char *argv[]) float rc_loss_timeout = 0.5; int32_t datalink_regain_timeout = 0; + uint8_t geofence_action = 0; + /* Thresholds for engine failure detection */ int32_t ef_throttle_thres = 1.0f; int32_t ef_current2throttle_thres = 0.0f; @@ -1346,6 +1343,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_ef_throttle_thres, &ef_throttle_thres); param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); param_get(_param_ef_time_thres, &ef_time_thres); + param_get(_param_geofence_action, &geofence_action); /* Autostart id */ param_get(_param_autostart_id, &autostart_id); @@ -1835,24 +1833,106 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result); } - /* Check for geofence violation */ - if (armed.armed && (geofence_result.geofence_violated || mission_result.flight_termination)) { - //XXX: make this configurable to select different actions (e.g. navigation modes) - /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ + // Geofence actions + if (armed.armed && (geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE)) { + + static bool geofence_loiter_on = false; + static bool geofence_rtl_on = false; + + static uint8_t geofence_main_state_before_violation = vehicle_status_s::MAIN_STATE_MAX; + + // check for geofence violation + if (geofence_result.geofence_violated) { + static hrt_abstime last_geofence_violation = 0; + const hrt_abstime geofence_violation_action_interval = 10000000; // 10 seconds + if (hrt_elapsed_time(&last_geofence_violation) > geofence_violation_action_interval) { + + last_geofence_violation = hrt_absolute_time(); + + switch (geofence_result.geofence_action) { + case (geofence_result_s::GF_ACTION_NONE) : { + // do nothing + break; + } + case (geofence_result_s::GF_ACTION_WARN) : { + // do nothing, mavlink critical messages are sent by navigator + break; + } + case (geofence_result_s::GF_ACTION_LOITER) : { + if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LOITER)) { + geofence_loiter_on = true; + } + break; + } + case (geofence_result_s::GF_ACTION_RTL) : { + if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_RTL)) { + geofence_rtl_on = true; + } + break; + } + case (geofence_result_s::GF_ACTION_TERMINATE) : { + warnx("Flight termination because of geofence"); + mavlink_log_critical(mavlink_fd, "Geofence violation: flight termination"); + armed.force_failsafe = true; + status_changed = true; + break; + } + } + } + } + + // reset if no longer in LOITER or if manually switched to LOITER + geofence_loiter_on = geofence_loiter_on + && (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LOITER) + && (sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_OFF); + + // reset if no longer in RTL or if manually switched to RTL + geofence_rtl_on = geofence_rtl_on + && (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_RTL) + && (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_OFF); + + if (!geofence_loiter_on && !geofence_rtl_on) { + // store the last good main_state when not in a geofence triggered action (LOITER or RTL) + geofence_main_state_before_violation = status.main_state; + } + + // revert geofence failsafe transition if sticks are moved and we were previously in MANUAL or ASSIST + if ((geofence_loiter_on || geofence_rtl_on) && + (geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_MANUAL || + geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_ALTCTL || + geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_POSCTL || + geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_ACRO || + geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_STAB)) { + + // transition to previous state if sticks are increased + const float min_stick_change = 0.2; + if ((_last_sp_man.timestamp != sp_man.timestamp) && + ((fabsf(sp_man.x) - fabsf(_last_sp_man.x) > min_stick_change) || + (fabsf(sp_man.y) - fabsf(_last_sp_man.y) > min_stick_change) || + (fabsf(sp_man.z) - fabsf(_last_sp_man.z) > min_stick_change) || + (fabsf(sp_man.r) - fabsf(_last_sp_man.r) > min_stick_change))) { + + main_state_transition(&status, geofence_main_state_before_violation); + } + } + } + + + /* Check for mission flight termination */ + if (armed.armed && mission_result.flight_termination) { armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; if (!flight_termination_printed) { - warnx("Flight termination because of navigator request or geofence"); - mavlink_log_critical(mavlink_fd, "Geofence violation: flight termination"); + warnx("Flight termination because of navigator request"); flight_termination_printed = true; } if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { mavlink_log_critical(mavlink_fd, "Flight termination active"); } - } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination + } /* Only evaluate mission state if home is set, * this prevents false positives for the mission @@ -1937,10 +2017,15 @@ int commander_thread_main(int argc, char *argv[]) * for being in manual mode only applies to manual arming actions. * the system can be armed in auto if armed via the GCS. */ + if ((status.main_state != vehicle_status_s::MAIN_STATE_MANUAL) && (status.main_state != vehicle_status_s::MAIN_STATE_STAB)) { print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + } else if (!status.condition_home_position_valid && + geofence_action == geofence_result_s::GF_ACTION_RTL) { + print_reject_arm("NOT ARMING: Geofence RTL requires valid home"); + } else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd); @@ -2107,7 +2192,7 @@ int commander_thread_main(int argc, char *argv[]) /* Check for failure combinations which lead to flight termination */ if (armed.armed) { /* At this point the data link and the gps system have been checked - * If we are not in a manual (RC stick controlled mode) + * If we are not in a manual (RC stick controlled mode) * and both failed we want to terminate the flight */ if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL && status.main_state !=vehicle_status_s::MAIN_STATE_ACRO && @@ -2422,31 +2507,32 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; - /* if offboard is set allready by a mavlink command, abort */ + /* if offboard is set already by a mavlink command, abort */ if (status.offboard_control_set_by_command) { return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD); } /* manual setpoint has not updated, do not re-evaluate it */ - if ((last_manual_input == sp_man->timestamp) || - ((last_offboard_switch == sp_man->offboard_switch) && - (last_return_switch == sp_man->return_switch) && - (last_mode_switch == sp_man->mode_switch) && - (last_acro_switch == sp_man->acro_switch) && - (last_posctl_switch == sp_man->posctl_switch) && - (last_loiter_switch == sp_man->loiter_switch))) { + if ((_last_sp_man.timestamp == sp_man->timestamp) || + ((_last_sp_man.offboard_switch == sp_man->offboard_switch) && + (_last_sp_man.return_switch == sp_man->return_switch) && + (_last_sp_man.mode_switch == sp_man->mode_switch) && + (_last_sp_man.acro_switch == sp_man->acro_switch) && + (_last_sp_man.posctl_switch == sp_man->posctl_switch) && + (_last_sp_man.loiter_switch == sp_man->loiter_switch))) { + + // update these fields for the geofence system + _last_sp_man.timestamp = sp_man->timestamp; + _last_sp_man.x = sp_man->x; + _last_sp_man.y = sp_man->y; + _last_sp_man.z = sp_man->z; + _last_sp_man.r = sp_man->r; /* no timestamp change or no switch change -> nothing changed */ return TRANSITION_NOT_CHANGED; } - last_manual_input = sp_man->timestamp; - last_offboard_switch = sp_man->offboard_switch; - last_return_switch = sp_man->return_switch; - last_mode_switch = sp_man->mode_switch; - last_acro_switch = sp_man->acro_switch; - last_posctl_switch = sp_man->posctl_switch; - last_loiter_switch = sp_man->loiter_switch; + _last_sp_man = *sp_man; /* offboard switch overrides main switch */ if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) { diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 7c6bb281ac..2baa0a7581 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -53,14 +53,8 @@ #include #include -#define GEOFENCE_OFF 0 -#define GEOFENCE_FILE_ONLY 1 -#define GEOFENCE_MAX_DISTANCES_ONLY 2 -#define GEOFENCE_FILE_AND_MAX_DISTANCES 3 - #define GEOFENCE_RANGE_WARNING_LIMIT 3000000 - /* Oddly, ERROR is not defined for C++ */ #ifdef ERROR # undef ERROR @@ -76,8 +70,8 @@ Geofence::Geofence() : _last_vertical_range_warning(0), _altitude_min(0), _altitude_max(0), - _verticesCount(0), - _param_geofence_mode(this, "MODE"), + _vertices_count(0), + _param_action(this, "ACTION"), _param_altitude_mode(this, "ALTMODE"), _param_source(this, "SOURCE"), _param_counter_threshold(this, "COUNT"), @@ -138,7 +132,6 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, bool Geofence::inside(double lat, double lon, float altitude) { - if (_param_geofence_mode.get() >= GEOFENCE_MAX_DISTANCES_ONLY) { int32_t max_horizontal_distance = _param_max_hor_distance.get(); int32_t max_vertical_distance = _param_max_ver_distance.get(); @@ -152,7 +145,7 @@ bool Geofence::inside(double lat, double lon, float altitude) if (max_vertical_distance > 0 && (dist_z > max_vertical_distance)) { if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { - mavlink_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.1f m", + mavlink_and_console_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.1f m", (double)(dist_z - max_vertical_distance)); _last_vertical_range_warning = hrt_absolute_time(); } @@ -162,7 +155,7 @@ bool Geofence::inside(double lat, double lon, float altitude) if (max_horizontal_distance > 0 && (dist_xy > max_horizontal_distance)) { if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { - mavlink_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.1f m", + mavlink_and_console_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.1f m", (double)(dist_xy - max_horizontal_distance)); _last_horizontal_range_warning = hrt_absolute_time(); } @@ -171,7 +164,6 @@ bool Geofence::inside(double lat, double lon, float altitude) } } } - } bool inside_fence = inside_polygon(lat, lon, altitude); @@ -194,12 +186,6 @@ bool Geofence::inside(double lat, double lon, float altitude) bool Geofence::inside_polygon(double lat, double lon, float altitude) { - /* Return true if geofence is disabled or only checking max distances */ - if ((_param_geofence_mode.get() == GEOFENCE_OFF) - || (_param_geofence_mode.get() == GEOFENCE_MAX_DISTANCES_ONLY)) { - return true; - } - if (valid()) { if (!isEmpty()) { @@ -219,7 +205,7 @@ bool Geofence::inside_polygon(double lat, double lon, float altitude) struct fence_vertex_s temp_vertex_j; /* Red until fence is finished */ - for (unsigned i = 0, j = _verticesCount - 1; i < _verticesCount; j = i++) { + for (unsigned i = 0, j = _vertices_count - 1; i < _vertices_count; j = i++) { if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { break; } @@ -259,7 +245,7 @@ Geofence::valid() } // Otherwise - if ((_verticesCount < 4) || (_verticesCount > fence_s::GEOFENCE_MAX_VERTICES)) { + if ((_vertices_count < 4) || (_vertices_count > fence_s::GEOFENCE_MAX_VERTICES)) { warnx("Fence must have at least 3 sides and not more than %d", fence_s::GEOFENCE_MAX_VERTICES - 1); return false; } @@ -415,7 +401,7 @@ Geofence::loadFromFile(const char *filename) /* Check if import was successful */ if (gotVertical && pointCounter > 0) { - _verticesCount = pointCounter; + _vertices_count = pointCounter; warnx("Geofence: imported successfully"); mavlink_log_info(_mavlinkFd, "Geofence imported"); rc = OK; diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index c9518872f1..a19e13ca2f 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -97,12 +97,14 @@ public: int loadFromFile(const char *filename); - bool isEmpty() {return _verticesCount == 0;} + bool isEmpty() {return _vertices_count == 0;} int getAltitudeMode() { return _param_altitude_mode.get(); } int getSource() { return _param_source.get(); } + int getGeofenceAction() { return _param_action.get(); } + void setMavlinkFd(int value) { _mavlinkFd = value; } private: @@ -114,20 +116,20 @@ private: hrt_abstime _last_horizontal_range_warning; hrt_abstime _last_vertical_range_warning; - float _altitude_min; - float _altitude_max; + float _altitude_min; + float _altitude_max; - unsigned _verticesCount; + uint8_t _vertices_count; /* Params */ - control::BlockParamInt _param_geofence_mode; + control::BlockParamInt _param_action; control::BlockParamInt _param_altitude_mode; control::BlockParamInt _param_source; control::BlockParamInt _param_counter_threshold; control::BlockParamInt _param_max_hor_distance; control::BlockParamInt _param_max_ver_distance; - uint8_t _outside_counter; + uint8_t _outside_counter; int _mavlinkFd; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 63eec7213d..070fde6174 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -44,15 +44,15 @@ */ /** - * Geofence mode. + * Geofence violation action. * - * 0 = disabled, 1 = geofence file only, 2 = max horizontal (GF_MAX_HOR_DIST) and vertical (GF_MAX_VER_DIST) distances, 3 = both + * 0 = none, 1 = warning (default), 2 = loiter, 3 = return to launch, 4 = fight termination * * @min 0 - * @max 3 + * @max 4 * @group Geofence */ -PARAM_DEFINE_INT32(GF_MODE, 0); +PARAM_DEFINE_INT32(GF_ACTION, 1); /** * Geofence altitude mode @@ -93,7 +93,7 @@ PARAM_DEFINE_INT32(GF_COUNT, -1); /** * Max horizontal distance in meters. * - * Set to > 0 to activate RTL if horizontal distance to home exceeds this value. + * Set to > 0 to activate a geofence action if horizontal distance to home exceeds this value. * * @group Geofence */ @@ -102,7 +102,7 @@ PARAM_DEFINE_INT32(GF_MAX_HOR_DIST, -1); /** * Max vertical distance in meters. * - * Set to > 0 to activate RTL if vertical distance to home exceeds this value. + * Set to > 0 to activate a geofence action if vertical distance to home exceeds this value. * * @group Geofence */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index bf8974cd8c..92f3e2fecb 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -405,10 +405,14 @@ Navigator::task_main() /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; - if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) { + if (have_geofence_position_data && + (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && + (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, _home_position_set); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; + + _geofence_result.geofence_action = _geofence.getGeofenceAction(); if (!inside) { /* inform other apps via the mission result */ _geofence_result.geofence_violated = true; From a99390503a3bad553ca78d94bc7044c6b8ef51bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Oct 2015 09:30:22 +0100 Subject: [PATCH 032/161] QuRT travis target: Make accessible via makefile --- Makefile | 3 +++ cmake/qurt/px4_impl_qurt.cmake | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/Makefile b/Makefile index e67c21e1af..f25bd92233 100644 --- a/Makefile +++ b/Makefile @@ -143,6 +143,9 @@ ros_sitl_simple: qurt_eagle_travis: $(call cmake-build,$@) +qurt_eagle_release: + $(call cmake-build,$@) + posix_eagle_release: $(call cmake-build,$@) diff --git a/cmake/qurt/px4_impl_qurt.cmake b/cmake/qurt/px4_impl_qurt.cmake index 8f9f3dc5d3..312c818ed5 100644 --- a/cmake/qurt/px4_impl_qurt.cmake +++ b/cmake/qurt/px4_impl_qurt.cmake @@ -222,7 +222,7 @@ function(px4_os_prebuild_targets) ONE_VALUE OUT BOARD THREADS REQUIRED OUT BOARD ARGN ${ARGN}) - add_custom_target(${OUT} DEPENDS git_dspal git_eigen) + add_custom_target(${OUT} DEPENDS git_dspal) endfunction() From 146def64339d227d8900b333c8b9b721ab766a0d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Oct 2015 09:30:42 +0100 Subject: [PATCH 033/161] Travis CI: Load config early on to allow s3cmd usage across the scripts --- .travis.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index 3786602b09..15e4599d72 100644 --- a/.travis.yml +++ b/.travis.yml @@ -50,6 +50,8 @@ before_install: && if grep -Fxq "$exportline" ~/.profile; then echo nothing to do ; else echo $exportline >> ~/.profile; fi && . ~/.profile && popd + && git clone git://github.com/PX4/CI-Tools.git + && ./CI-Tools/s3cmd-configure && mkdir -p ~/bin && wget -O ~/bin/astyle https://github.com/PX4/astyle/releases/download/2.05.1/astyle-linux && chmod +x ~/bin/astyle && astyle --version @@ -106,8 +108,6 @@ after_success: cp build_px4fmu-v1_default/src/firmware/nuttx/nuttx-px4fmu-v1-default.px4 px4fmu-v1_default.px4 && cp build_px4fmu-v2_default/src/firmware/nuttx/nuttx-px4fmu-v2-default.px4 px4fmu-v2_default.px4 && zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4 - && git clone git://github.com/PX4/CI-Tools.git - && ./CI-Tools/s3cmd-configure && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ && ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/ && ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/ From 8d0e10e830b4f3b676391acdcc63b95374118ff8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 19:14:23 +0200 Subject: [PATCH 034/161] Commander: Set home on takeoff --- src/modules/commander/commander.cpp | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3395b8c069..0b0b8dd5d4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -881,7 +881,7 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = false; bool arm_tune_played = false; - bool was_armed = false; + bool was_landed = true; bool startup_in_hil = false; @@ -1029,17 +1029,15 @@ int commander_thread_main(int argc, char *argv[]) px4_task_exit(ERROR); } - /* armed topic */ - orb_advert_t armed_pub; /* Initialize armed with all false */ memset(&armed, 0, sizeof(armed)); + /* armed topic */ + orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); /* vehicle control mode topic */ memset(&control_mode, 0, sizeof(control_mode)); orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); - armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - /* home position */ orb_advert_t home_pub = nullptr; memset(&_home, 0, sizeof(_home)); @@ -2250,9 +2248,10 @@ int commander_thread_main(int argc, char *argv[]) } /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ - else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { + else if (was_landed && !status.condition_landed && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { commander_set_home_position(home_pub, _home, local_position, global_position); } + was_landed = status.condition_landed; /* print new state */ if (arming_state_changed) { @@ -2261,8 +2260,6 @@ int commander_thread_main(int argc, char *argv[]) arming_state_changed = false; } - was_armed = armed.armed; - /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, mission_result.finished, From b7f3c96d4a28a8432eb838364f059d6e79cda90d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Oct 2015 09:36:09 +0100 Subject: [PATCH 035/161] Set home on arming and on takeoff --- src/modules/commander/commander.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0b0b8dd5d4..86f8ad546b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -149,7 +149,7 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 10000000 -#define INAIR_RESTART_HOLDOFF_INTERVAL 2000000 +#define INAIR_RESTART_HOLDOFF_INTERVAL 1000000 #define HIL_ID_MIN 1000 #define HIL_ID_MAX 1999 @@ -882,6 +882,7 @@ int commander_thread_main(int argc, char *argv[]) bool arm_tune_played = false; bool was_landed = true; + bool was_armed = false; bool startup_in_hil = false; @@ -2248,10 +2249,12 @@ int commander_thread_main(int argc, char *argv[]) } /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ - else if (was_landed && !status.condition_landed && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { + else if (((!was_armed && armed.armed) || (was_landed && !status.condition_landed)) && + (now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) { commander_set_home_position(home_pub, _home, local_position, global_position); } was_landed = status.condition_landed; + was_armed = armed.armed; /* print new state */ if (arming_state_changed) { From 34ff90c62483bd4c983ae8fcf4ed6bede9792ce0 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Tue, 27 Oct 2015 09:41:40 +0100 Subject: [PATCH 036/161] fixed logging --- src/modules/sdlog2/sdlog2.c | 34 +++++----- src/modules/sdlog2/sdlog2_messages.h | 92 ++++++++++++++-------------- 2 files changed, 63 insertions(+), 63 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index fa3ede9dc6..408fa396bf 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1060,7 +1060,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_command_s cmd; struct sensor_combined_s sensor; struct vehicle_attitude_s att; - struct control_state_s ctrl_state; struct vehicle_attitude_setpoint_s att_sp; struct vehicle_rates_setpoint_s rates_sp; struct actuator_outputs_s act_outputs; @@ -1091,6 +1090,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vtol_vehicle_status_s vtol_status; struct time_offset_s time_offset; struct mc_att_ctrl_status_s mc_att_ctrl_status; + struct control_state_s ctrl_state; } buf; memset(&buf, 0, sizeof(buf)); @@ -1111,7 +1111,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; struct log_VTOL_s log_VTOL; - struct log_CTS_s log_CTS; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; struct log_AIRS_s log_AIRS; @@ -1140,6 +1139,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_ENCD_s log_ENCD; struct log_TSYN_s log_TSYN; struct log_MACS_s log_MACS; + struct log_CTS_s log_CTS; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1153,7 +1153,6 @@ int sdlog2_thread_main(int argc, char *argv[]) int vtol_status_sub; int sensor_sub; int att_sub; - int ctrl_state_sub; int att_sp_sub; int rates_sp_sub; int act_outputs_sub; @@ -1183,6 +1182,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int encoders_sub; int tsync_sub; int mc_att_ctrl_status_sub; + int ctrl_state_sub; } subs; subs.cmd_sub = -1; @@ -1191,7 +1191,6 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.gps_pos_sub = -1; subs.sensor_sub = -1; subs.att_sub = -1; - subs.ctrl_state_sub = -1; subs.att_sp_sub = -1; subs.rates_sp_sub = -1; subs.act_outputs_sub = -1; @@ -1217,6 +1216,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.wind_sub = -1; subs.tsync_sub = -1; subs.mc_att_ctrl_status_sub = -1; + subs.ctrl_state_sub = -1; subs.encoders_sub = -1; /* add new topics HERE */ @@ -1670,19 +1670,6 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(FLOW); } - /* --- CONTROL STATE --- */ - if (copy_if_updated(ORB_ID(control_state), &subs.ctrl_state_sub, &buf.ctrl_state)) { - log_msg.msg_type = LOG_CTS_MSG; - log_msg.body.log_CTS.vx_body = buf.ctrl_state.x_vel; - log_msg.body.log_CTS.vy_body = buf.ctrl_state.y_vel; - log_msg.body.log_CTS.vz_body = buf.ctrl_state.z_vel; - log_msg.body.log_CTS.airspeed = buf.ctrl_state.airspeed; - log_msg.body.log_CTS.roll_rate = buf.ctrl_state.roll_rate; - log_msg.body.log_CTS.pitch_rate = buf.ctrl_state.pitch_rate; - log_msg.body.log_CTS.yaw_rate = buf.ctrl_state.yaw_rate; - LOGBUFFER_WRITE_AND_COUNT(CTS); - } - /* --- RC CHANNELS --- */ if (copy_if_updated(ORB_ID(rc_channels), &subs.rc_sub, &buf.rc)) { log_msg.msg_type = LOG_RC_MSG; @@ -1874,6 +1861,19 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(MACS); } + /* --- CONTROL STATE --- */ + if (copy_if_updated(ORB_ID(control_state), &subs.ctrl_state_sub, &buf.ctrl_state)) { + log_msg.msg_type = LOG_CTS_MSG; + log_msg.body.log_CTS.vx_body = buf.ctrl_state.x_vel; + log_msg.body.log_CTS.vy_body = buf.ctrl_state.y_vel; + log_msg.body.log_CTS.vz_body = buf.ctrl_state.z_vel; + log_msg.body.log_CTS.airspeed = buf.ctrl_state.airspeed; + log_msg.body.log_CTS.roll_rate = buf.ctrl_state.roll_rate; + log_msg.body.log_CTS.pitch_rate = buf.ctrl_state.pitch_rate; + log_msg.body.log_CTS.yaw_rate = buf.ctrl_state.yaw_rate; + LOGBUFFER_WRITE_AND_COUNT(CTS); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 30efd95e26..462b753f98 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -185,20 +185,8 @@ struct log_STAT_s { float load; }; -/* --- CONTROL STATE --- */ -#define LOG_CTS_MSG 11 -struct log_CTS_s { - float vx_body; - float vy_body; - float vz_body; - float airspeed; - float roll_rate; - float pitch_rate; - float yaw_rate; -}; - /* --- RC - RC INPUT CHANNELS --- */ -#define LOG_RC_MSG 12 +#define LOG_RC_MSG 11 struct log_RC_s { float channel[8]; uint8_t channel_count; @@ -206,13 +194,13 @@ struct log_RC_s { }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ -#define LOG_OUT0_MSG 13 +#define LOG_OUT0_MSG 12 struct log_OUT0_s { float output[8]; }; /* --- AIRS - AIRSPEED --- */ -#define LOG_AIRS_MSG 14 +#define LOG_AIRS_MSG 13 struct log_AIRS_s { float indicated_airspeed; float true_airspeed; @@ -220,7 +208,7 @@ struct log_AIRS_s { }; /* --- ARSP - ATTITUDE RATE SET POINT --- */ -#define LOG_ARSP_MSG 15 +#define LOG_ARSP_MSG 14 struct log_ARSP_s { float roll_rate_sp; float pitch_rate_sp; @@ -228,7 +216,7 @@ struct log_ARSP_s { }; /* --- FLOW - OPTICAL FLOW --- */ -#define LOG_FLOW_MSG 16 +#define LOG_FLOW_MSG 15 struct log_FLOW_s { uint8_t sensor_id; float pixel_flow_x_integral; @@ -245,7 +233,7 @@ struct log_FLOW_s { }; /* --- GPOS - GLOBAL POSITION ESTIMATE --- */ -#define LOG_GPOS_MSG 17 +#define LOG_GPOS_MSG 16 struct log_GPOS_s { int32_t lat; int32_t lon; @@ -259,7 +247,7 @@ struct log_GPOS_s { }; /* --- GPSP - GLOBAL POSITION SETPOINT --- */ -#define LOG_GPSP_MSG 18 +#define LOG_GPSP_MSG 17 struct log_GPSP_s { uint8_t nav_state; int32_t lat; @@ -273,7 +261,7 @@ struct log_GPSP_s { }; /* --- ESC - ESC STATE --- */ -#define LOG_ESC_MSG 19 +#define LOG_ESC_MSG 18 struct log_ESC_s { uint16_t counter; uint8_t esc_count; @@ -290,7 +278,7 @@ struct log_ESC_s { }; /* --- GVSP - GLOBAL VELOCITY SETPOINT --- */ -#define LOG_GVSP_MSG 20 +#define LOG_GVSP_MSG 19 struct log_GVSP_s { float vx; float vy; @@ -298,7 +286,7 @@ struct log_GVSP_s { }; /* --- BATT - BATTERY --- */ -#define LOG_BATT_MSG 21 +#define LOG_BATT_MSG 20 struct log_BATT_s { float voltage; float voltage_filtered; @@ -307,7 +295,7 @@ struct log_BATT_s { }; /* --- DIST - RANGE SENSOR DISTANCE --- */ -#define LOG_DIST_MSG 22 +#define LOG_DIST_MSG 21 struct log_DIST_s { uint8_t id; uint8_t type; @@ -316,11 +304,11 @@ struct log_DIST_s { float covariance; }; -/* LOG IMU1 and IMU2 MSGs consume IDs 23 and 24 */ +/* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */ /* --- PWR - ONBOARD POWER SYSTEM --- */ -#define LOG_PWR_MSG 25 +#define LOG_PWR_MSG 24 struct log_PWR_s { float peripherals_5v; float servo_rail_5v; @@ -333,7 +321,7 @@ struct log_PWR_s { }; /* --- MOCP - MOCAP ATTITUDE AND POSITION --- */ -#define LOG_MOCP_MSG 26 +#define LOG_MOCP_MSG 25 struct log_MOCP_s { float qw; float qx; @@ -345,31 +333,31 @@ struct log_MOCP_s { }; /* --- GS0A - GPS SNR #0, SAT GROUP A --- */ -#define LOG_GS0A_MSG 27 +#define LOG_GS0A_MSG 26 struct log_GS0A_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- GS0B - GPS SNR #0, SAT GROUP B --- */ -#define LOG_GS0B_MSG 28 +#define LOG_GS0B_MSG 27 struct log_GS0B_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- GS1A - GPS SNR #1, SAT GROUP A --- */ -#define LOG_GS1A_MSG 29 +#define LOG_GS1A_MSG 28 struct log_GS1A_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- GS1B - GPS SNR #1, SAT GROUP B --- */ -#define LOG_GS1B_MSG 30 +#define LOG_GS1B_MSG 29 struct log_GS1B_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- TECS - TECS STATUS --- */ -#define LOG_TECS_MSG 31 +#define LOG_TECS_MSG 30 struct log_TECS_s { float altitudeSp; float altitudeFiltered; @@ -390,7 +378,7 @@ struct log_TECS_s { }; /* --- WIND - WIND ESTIMATE --- */ -#define LOG_WIND_MSG 32 +#define LOG_WIND_MSG 31 struct log_WIND_s { float x; float y; @@ -399,7 +387,7 @@ struct log_WIND_s { }; /* --- EST0 - ESTIMATOR STATUS --- */ -#define LOG_EST0_MSG 33 +#define LOG_EST0_MSG 32 struct log_EST0_s { float s[12]; uint8_t n_states; @@ -409,28 +397,28 @@ struct log_EST0_s { }; /* --- EST1 - ESTIMATOR STATUS --- */ -#define LOG_EST1_MSG 34 +#define LOG_EST1_MSG 33 struct log_EST1_s { float s[16]; }; /* --- EST2 - ESTIMATOR STATUS --- */ -#define LOG_EST2_MSG 35 +#define LOG_EST2_MSG 34 struct log_EST2_s { float cov[12]; }; /* --- EST3 - ESTIMATOR STATUS --- */ -#define LOG_EST3_MSG 36 +#define LOG_EST3_MSG 35 struct log_EST3_s { float cov[16]; }; /* --- TEL0..3 - TELEMETRY STATUS --- */ -#define LOG_TEL0_MSG 37 -#define LOG_TEL1_MSG 38 -#define LOG_TEL2_MSG 39 -#define LOG_TEL3_MSG 40 +#define LOG_TEL0_MSG 36 +#define LOG_TEL1_MSG 37 +#define LOG_TEL2_MSG 38 +#define LOG_TEL3_MSG 39 struct log_TEL_s { uint8_t rssi; uint8_t remote_rssi; @@ -443,7 +431,7 @@ struct log_TEL_s { }; /* --- VISN - VISION POSITION --- */ -#define LOG_VISN_MSG 41 +#define LOG_VISN_MSG 40 struct log_VISN_s { float x; float y; @@ -458,7 +446,7 @@ struct log_VISN_s { }; /* --- ENCODERS - ENCODER DATA --- */ -#define LOG_ENCD_MSG 42 +#define LOG_ENCD_MSG 41 struct log_ENCD_s { int64_t cnt0; float vel0; @@ -467,28 +455,40 @@ struct log_ENCD_s { }; /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */ -#define LOG_AIR1_MSG 43 +#define LOG_AIR1_MSG 42 /* --- VTOL - VTOL VEHICLE STATUS */ -#define LOG_VTOL_MSG 44 +#define LOG_VTOL_MSG 43 struct log_VTOL_s { float airspeed_tot; }; /* --- TIMESYNC - TIME SYNCHRONISATION OFFSET */ -#define LOG_TSYN_MSG 45 +#define LOG_TSYN_MSG 44 struct log_TSYN_s { uint64_t time_offset; }; /* --- MACS - MULTIROTOR ATTITUDE CONTROLLER STATUS */ -#define LOG_MACS_MSG 47 +#define LOG_MACS_MSG 45 struct log_MACS_s { float roll_rate_integ; float pitch_rate_integ; float yaw_rate_integ; }; +/* --- CONTROL STATE --- */ +#define LOG_CTS_MSG 47 +struct log_CTS_s { + float vx_body; + float vy_body; + float vz_body; + float airspeed; + float roll_rate; + float pitch_rate; + float yaw_rate; +}; + /* WARNING: ID 46 is already in use for ATTC1 */ /********** SYSTEM MESSAGES, ID > 0x80 **********/ From 12bd1ecb7c0d68b3fbe5eb2cebada52fc63b9d8e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Oct 2015 09:57:42 +0100 Subject: [PATCH 037/161] Home pos: Add yaw field --- msg/home_position.msg | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/msg/home_position.msg b/msg/home_position.msg index d8aff3f634..7135c07b18 100644 --- a/msg/home_position.msg +++ b/msg/home_position.msg @@ -8,3 +8,8 @@ float32 alt # Altitude in meters (AMSL) float32 x # X coordinate in meters float32 y # Y coordinate in meters float32 z # Z coordinate in meters + +float32 yaw # Yaw angle in radians +float32 direction_x # Takeoff direction in NED X +float32 direction_y # Takeoff direction in NED Y +float32 direction_z # Takeoff direction in NED Z From 3f4a8bf76dea9b50bcf734aa119f28a2c74fd8b2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Oct 2015 09:58:54 +0100 Subject: [PATCH 038/161] Commander: Set yaw on takeoff and use it as yaw during descend phase of RTL --- src/modules/commander/CMakeLists.txt | 2 +- src/modules/commander/commander.cpp | 44 +++++++++++++++++++--------- 2 files changed, 31 insertions(+), 15 deletions(-) diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index 5ca0ff2ca6..6cf2531ade 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -32,7 +32,7 @@ ############################################################################ set(MODULE_CFLAGS -Os) if(${OS} STREQUAL "nuttx") - list(APPEND MODULE_CFLAGS -Wframe-larger-than=2300) + list(APPEND MODULE_CFLAGS -Wframe-larger-than=2400) endif() px4_add_module( MODULE modules__commander diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 86f8ad546b..53fe14dc08 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -221,7 +222,7 @@ void usage(const char *reason); */ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, - struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub); + struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub); /** * Mainloop of commander. @@ -253,7 +254,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armed * time the vehicle is armed with a good GPS fix. **/ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, - const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition); + const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition, + const vehicle_attitude_s &attitude); /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. @@ -291,7 +293,7 @@ int commander_main(int argc, char *argv[]) daemon_task = px4_task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 3400, + 3500, commander_thread_main, (char * const *)&argv[0]); @@ -490,7 +492,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, struct home_position_s *home, struct vehicle_global_position_s *global_pos, - struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub) + struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub) { /* only handle commands that are meant to be handled by this system and component */ if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) @@ -524,7 +526,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s if (cmd_arm && (arming_ret == TRANSITION_CHANGED) && (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))) { - commander_set_home_position(*home_pub, *home, *local_pos, *global_pos); + commander_set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude); } if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { @@ -833,7 +835,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s * time the vehicle is armed with a good GPS fix. **/ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, - const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition) + const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition, + const vehicle_attitude_s &attitude) { //Need global position fix to be able to set home if (!status.condition_global_position_valid) { @@ -855,6 +858,8 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s & home.y = localPosition.y; home.z = localPosition.z; + home.yaw = attitude.yaw; + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); @@ -1137,13 +1142,15 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to local position data */ int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - struct vehicle_local_position_s local_position; - memset(&local_position, 0, sizeof(local_position)); + struct vehicle_local_position_s local_position = {}; + + /* Subscribe to attitude data */ + int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + struct vehicle_attitude_s attitude = {}; /* Subscribe to land detector */ int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); - struct vehicle_land_detected_s land_detector; - memset(&land_detector, 0, sizeof(land_detector)); + struct vehicle_land_detected_s land_detector = {}; /* * The home position is set based on GPS only, to prevent a dependency between @@ -1573,6 +1580,14 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); } + /* update attitude estimate */ + orb_check(attitude_sub, &updated); + + if (updated) { + /* position changed */ + orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &attitude); + } + //update condition_global_position_valid //Global positions are only published by the estimators if they are valid if (hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) { @@ -2183,7 +2198,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &home_pub)) { + if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &attitude, &home_pub)) { status_changed = true; } } @@ -2245,14 +2260,15 @@ int commander_thread_main(int argc, char *argv[]) /* First time home position update - but only if disarmed */ if (!status.condition_home_position_valid && !armed.armed) { - commander_set_home_position(home_pub, _home, local_position, global_position); + commander_set_home_position(home_pub, _home, local_position, global_position, attitude); } - /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ + /* update home position on arming if at least 1s from commander start spent to avoid setting home on in-air restart */ else if (((!was_armed && armed.armed) || (was_landed && !status.condition_landed)) && (now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) { - commander_set_home_position(home_pub, _home, local_position, global_position); + commander_set_home_position(home_pub, _home, local_position, global_position, attitude); } + was_landed = status.condition_landed; was_armed = armed.armed; From 5759358da9965045478cd9b24d40be64cc9516b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Oct 2015 09:59:09 +0100 Subject: [PATCH 039/161] Navigator: Use yaw of home position --- src/modules/navigator/rtl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 246007ad81..8a4d89ef35 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -190,7 +190,7 @@ RTL::set_rtl_item() _mission_item.lon = _navigator->get_home_position()->lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); - _mission_item.yaw = NAN; + _mission_item.yaw = _navigator->get_home_position()->yaw; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; From 8137a261224088264a79bd025a9421641e00afc9 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Tue, 27 Oct 2015 10:07:32 +0100 Subject: [PATCH 040/161] Additional yaw compensation for flow module offset from center of rotation. --- .../position_estimator_inav/CMakeLists.txt | 2 +- .../position_estimator_inav_main.c | 115 +++++++----------- .../position_estimator_inav_params.c | 28 +++++ .../position_estimator_inav_params.h | 4 + 4 files changed, 76 insertions(+), 73 deletions(-) diff --git a/src/modules/position_estimator_inav/CMakeLists.txt b/src/modules/position_estimator_inav/CMakeLists.txt index ed5c05c870..2d5315b92b 100644 --- a/src/modules/position_estimator_inav/CMakeLists.txt +++ b/src/modules/position_estimator_inav/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ if(${OS} STREQUAL "nuttx") - list(APPEND MODULE_CFLAGS -Wframe-larger-than=3850) + list(APPEND MODULE_CFLAGS -Wframe-larger-than=3890) endif() px4_add_module( MODULE modules__position_estimator_inav 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 ad5698989e..04f3c4d918 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -322,16 +322,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime lidar_time = 0; // time of last lidar measurement (not filtered) hrt_abstime lidar_valid_time = 0; // time of last lidar measurement used for correction (filtered) - //----------test-------------------------- - float flow_test[] = { 0.0f, 0.0f }; - float pos_test[] = { 0.0f, 0.0f }; - float flow_test_average[] = { 0.0f, 0.0f }; int n_flow = 0; - float gyro_offset_filtered[] = { 0.0f, 0.0f }; - float flow_gyrospeed[] = { 0.0f, 0.0f }; - float flow_gyrospeed_filtered[] = { 0.0f, 0.0f }; - float att_gyrospeed_filtered[] = { 0.0f, 0.0f }; - //---------------------------------------- + float gyro_offset_filtered[] = { 0.0f, 0.0f, 0.0f }; + float flow_gyrospeed[] = { 0.0f, 0.0f, 0.0f }; + float flow_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f }; + float att_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f }; + float yaw_comp[] = { 0.0f, 0.0f }; bool gps_valid = false; // GPS is valid bool lidar_valid = false; // lidar is valid @@ -505,12 +501,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j]; } } - //------------test---------------------- - //dont convert into NED for tests - //acc[0] = sensor.accelerometer_m_s2[0]; - //acc[1] = sensor.accelerometer_m_s2[1]; - //acc[2] = sensor.accelerometer_m_s2[2]; - //--------------------------------------- acc[2] += CONSTANTS_ONE_G; @@ -580,7 +570,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float flow_q = flow.quality / 255.0f; float dist_bottom = - z_est[0] - surface_offset; //lidar.current_distance; - if (dist_bottom > 0.21f && flow_q > params.flow_q_min /*&& (t < lidar_valid_time + lidar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f*/) { + if (dist_bottom > 0.21f && flow_q > params.flow_q_min) { /* distance to surface */ //float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonar float flow_dist = dist_bottom; //use this if using lidar @@ -599,11 +589,45 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) //this flag is not working --> flow_accurate = true; //already checked if flow_q > 0.3 + + /*calculate offset of flow-gyro using already calibrated gyro from autopilot*/ + flow_gyrospeed[0] = flow.gyro_x_rate_integral/(float)flow.integration_timespan*1000000.0f; + flow_gyrospeed[1] = flow.gyro_y_rate_integral/(float)flow.integration_timespan*1000000.0f; + flow_gyrospeed[2] = flow.gyro_z_rate_integral/(float)flow.integration_timespan*1000000.0f; + + //moving average + if (n_flow >= 100) { + gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0]; + gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1]; + gyro_offset_filtered[2] = flow_gyrospeed_filtered[2] - att_gyrospeed_filtered[2]; + n_flow = 0; + flow_gyrospeed_filtered[0] = 0.0f; + flow_gyrospeed_filtered[1] = 0.0f; + flow_gyrospeed_filtered[2] = 0.0f; + att_gyrospeed_filtered[0] = 0.0f; + att_gyrospeed_filtered[1] = 0.0f; + att_gyrospeed_filtered[2] = 0.0f; + } else { + flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1); + flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1); + flow_gyrospeed_filtered[2] = (flow_gyrospeed[2] + n_flow * flow_gyrospeed_filtered[2]) / (n_flow + 1); + att_gyrospeed_filtered[0] = (att.pitchspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1); + att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1); + att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1); + n_flow++; + } + + + /*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*/ + yaw_comp[0] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]); + yaw_comp[1] = - params.flow_module_offset_y * (flow_gyrospeed[2] - gyro_offset_filtered[2]); + + /* convert raw flow to angular flow (rad/s) */ float flow_ang[2]; //calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro) - flow_ang[0] = (flow.pixel_flow_x_integral - flow.gyro_x_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[0];//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; - flow_ang[1] = (flow.pixel_flow_y_integral - flow.gyro_y_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[1];//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; + flow_ang[0] = (flow.pixel_flow_x_integral - flow.gyro_x_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[0] - yaw_comp[0];//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; + flow_ang[1] = (flow.pixel_flow_y_integral - flow.gyro_y_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[1] - yaw_comp[1];//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; /* flow measurements vector */ float flow_m[3]; flow_m[0] = -flow_ang[0] * flow_dist; @@ -619,43 +643,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - //-----------test------------------- - //do not transform to NED for testing - //flow_v[0] = flow_m[0]; - //flow_v[1] = flow_m[1]; - - flow_test[0] = flow_v[0]; - flow_test[1] = flow_v[1]; - - flow_test_average[0] = (flow.pixel_flow_x_integral + flow_test_average[0] * n_flow) / (n_flow + 1); - flow_test_average[1] = (flow.pixel_flow_y_integral + flow_test_average[1] * n_flow) / (n_flow + 1); - - //calculate offset of flow-gyro using already calibrated gyro from autopilot - flow_gyrospeed[0] = flow.gyro_x_rate_integral/(float)flow.integration_timespan*1000000.0f; - flow_gyrospeed[1] = flow.gyro_y_rate_integral/(float)flow.integration_timespan*1000000.0f; - - - - //moving average - if (n_flow >= 100) { - gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0]; - gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1]; - n_flow = 0; - flow_gyrospeed_filtered[0] = 0.0f; - flow_gyrospeed_filtered[1] = 0.0f; - att_gyrospeed_filtered[0] = 0.0f; - att_gyrospeed_filtered[1] = 0.0f; - } else { - flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1); - flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1); - att_gyrospeed_filtered[0] = (att.rollspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1); - att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1); - n_flow++; - //mavlink_log_info(mavlink_fd, "n_flow = %d\n", (int)n_flow); - } - - //---------------------------------- - /* velocity correction */ corr_flow[0] = flow_v[0] - x_est[1]; corr_flow[1] = flow_v[1] - y_est[1]; @@ -670,11 +657,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) w_flow *= 0.05f; } - //--------test------------- - //mavlink_log_info(mavlink_fd, "flow_accurate = %d\t w_flow = %4.4f\n", (int)flow_accurate, (double)w_flow); - //w_flow = 0.8; - //------------------------- - /* under ideal conditions, on 1m distance assume EPH = 10cm */ eph_flow = 0.1f / w_flow; @@ -1282,27 +1264,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) buf_ptr = 0; } - //-------------test-------------- - pos_test[0] += flow_test[0] * dt; - pos_test[1] += flow_test[1] * dt; - //------------------------------- - /* publish local position */ local_pos.xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy; local_pos.xy_global = local_pos.xy_valid && use_gps_xy; local_pos.z_global = local_pos.z_valid && use_gps_z; - local_pos.x = corr_baro; //test local_pos.x = x_est[0]; - local_pos.vx = surface_offset; // flow_test[0]; //test local_pos.vx = x_est[1]; - local_pos.y = acc_bias[2]; //test local_pos.y = y_est[0]; - local_pos.vy = flow_gyrospeed_filtered[0]; //test local_pos.vy = y_est[1]; - local_pos.z = lidar.current_distance; //flow_test[0]; //test local_pos.z = z_est[0]; - local_pos.vz = - z_est[0] - surface_offset; //flow_test[1]; //test local_pos.vz = z_est[1]; local_pos.yaw = att.yaw; local_pos.dist_bottom_valid = dist_bottom_valid; @@ -1311,7 +1282,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.dist_bottom_valid) { local_pos.dist_bottom = -z_est[0] - surface_offset; - local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate; + local_pos.dist_bottom_rate = - z_est[1] - surface_offset_rate; } local_pos.timestamp = t; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index acda5ffc70..be8a136c4d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -289,6 +289,30 @@ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); */ PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); +/** + * Flow module offset (center of rotation) in X direction + * + * Yaw X flow compensation + * + * @min -1.0 + * @max 1.0 + * @unit m + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_X, 0.0f); + +/** + * Flow module offset (center of rotation) in Y direction + * + * Yaw Y flow compensation + * + * @min -1.0 + * @max 1.0 + * @unit m + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_Y, 0.0f); + /** * Disable vision input * @@ -338,6 +362,8 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h) h->land_thr = param_find("INAV_LAND_THR"); h->no_vision = param_find("CBRK_NO_VISION"); h->delay_gps = param_find("INAV_DELAY_GPS"); + h->flow_module_offset_x = param_find("INAV_FLOW_DIST_X"); + h->flow_module_offset_y = param_find("INAV_FLOW_DIST_Y"); return 0; } @@ -366,6 +392,8 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h param_get(h->land_thr, &(p->land_thr)); param_get(h->no_vision, &(p->no_vision)); param_get(h->delay_gps, &(p->delay_gps)); + param_get(h->flow_module_offset_x, &(p->flow_module_offset_x)); + param_get(h->flow_module_offset_y, &(p->flow_module_offset_y)); return 0; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 434e3dfe0a..43601568f8 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -65,6 +65,8 @@ struct position_estimator_inav_params { float land_thr; int32_t no_vision; float delay_gps; + float flow_module_offset_x; + float flow_module_offset_y; }; struct position_estimator_inav_param_handles { @@ -91,6 +93,8 @@ struct position_estimator_inav_param_handles { param_t land_thr; param_t no_vision; param_t delay_gps; + param_t flow_module_offset_x; + param_t flow_module_offset_y; }; #define CBRK_NO_VISION_KEY 328754 From 93e557ab757259e9d43510b52511606eacc1fbf3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 27 Oct 2015 09:58:43 +0100 Subject: [PATCH 041/161] ported vtol module to posix --- src/modules/vtol_att_control/standard.cpp | 12 +++--- src/modules/vtol_att_control/tailsitter.cpp | 2 +- src/modules/vtol_att_control/tiltrotor.cpp | 12 +++--- .../vtol_att_control_main.cpp | 38 ++++++++++--------- .../vtol_att_control/vtol_att_control_main.h | 5 ++- src/modules/vtol_att_control/vtol_type.cpp | 24 ++++++------ 6 files changed, 50 insertions(+), 43 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 649c4b526f..3b2784ad64 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -288,11 +288,11 @@ Standard::set_max_mc(unsigned pwm_value) int ret; unsigned servo_count; char *dev = PWM_OUTPUT0_DEVICE_PATH; - int fd = open(dev, 0); + int fd = px4_open(dev, 0); - if (fd < 0) {err(1, "can't open %s", dev);} + if (fd < 0) {PX4_WARN("can't open %s", dev);} - ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); struct pwm_output_values pwm_values; memset(&pwm_values, 0, sizeof(pwm_values)); @@ -301,9 +301,9 @@ Standard::set_max_mc(unsigned pwm_value) pwm_values.channel_count = _params->vtol_motor_count; } - ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); - if (ret != OK) {errx(ret, "failed setting max values");} + if (ret != OK) {PX4_WARN("failed setting max values");} - close(fd); + px4_close(fd); } diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 93a0f25f56..e423fbceb3 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -116,7 +116,7 @@ Tailsitter::scale_mc_output() 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) || + if (bool nonfinite = !PX4_ISFINITE(_airspeed->true_airspeed_m_s) || hrt_elapsed_time(&_airspeed->timestamp) > 1e6) { airspeed = _params->mc_airspeed_trim; if (nonfinite) { diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index a50079b14b..300cf06e51 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -382,11 +382,11 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state) { int ret; unsigned servo_count; char *dev = PWM_OUTPUT0_DEVICE_PATH; - int fd = open(dev, 0); + int fd = px4_open(dev, 0); - if (fd < 0) {err(1, "can't open %s", dev);} + if (fd < 0) {PX4_WARN("can't open %s", dev);} - ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); struct pwm_output_values pwm_values; memset(&pwm_values, 0, sizeof(pwm_values)); @@ -399,11 +399,11 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state) { pwm_values.channel_count = _params->vtol_motor_count; } - ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); - if (ret != OK) {errx(ret, "failed setting max values");} + if (ret != OK) {PX4_WARN("failed setting max values");} - close(fd); + px4_close(fd); } bool Tiltrotor::is_motor_off_channel(const int channel) { 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 744f31d5b4..e21279cb75 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -150,7 +150,7 @@ VtolAttitudeControl::~VtolAttitudeControl() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_control_task); + px4_task_delete(_control_task); break; } } while (_control_task != -1); @@ -442,7 +442,7 @@ VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) void VtolAttitudeControl::task_main() { - warnx("started"); + PX4_WARN("started"); fflush(stdout); /* do subscriptions */ @@ -471,7 +471,7 @@ void VtolAttitudeControl::task_main() _vtol_type->set_idle_mc(); /* wakeup source*/ - struct pollfd fds[3]; /*input_mc, input_fw, parameters*/ + px4_pollfd_struct_t fds[3]; /*input_mc, input_fw, parameters*/ fds[0].fd = _actuator_inputs_mc; fds[0].events = POLLIN; @@ -491,7 +491,7 @@ void VtolAttitudeControl::task_main() } /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit */ @@ -633,7 +633,7 @@ void VtolAttitudeControl::task_main() warnx("exit"); _control_task = -1; - _exit(0); + return; } int @@ -646,11 +646,11 @@ VtolAttitudeControl::start() SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2048, - (main_t)&VtolAttitudeControl::task_main_trampoline, + (px4_main_t)&VtolAttitudeControl::task_main_trampoline, nullptr); if (_control_task < 0) { - warn("task start failed"); + PX4_WARN("task start failed"); return -errno; } @@ -661,49 +661,53 @@ VtolAttitudeControl::start() int vtol_att_control_main(int argc, char *argv[]) { if (argc < 2) { - errx(1, "usage: vtol_att_control {start|stop|status}"); + PX4_WARN("usage: vtol_att_control {start|stop|status}"); } if (!strcmp(argv[1], "start")) { if (VTOL_att_control::g_control != nullptr) { - errx(1, "already running"); + PX4_WARN("already running"); + return 0; } VTOL_att_control::g_control = new VtolAttitudeControl; if (VTOL_att_control::g_control == nullptr) { - errx(1, "alloc failed"); + PX4_WARN("alloc failed"); + return 1; } if (OK != VTOL_att_control::g_control->start()) { delete VTOL_att_control::g_control; VTOL_att_control::g_control = nullptr; - err(1, "start failed"); + PX4_WARN("start failed"); + return 1; } - exit(0); } if (!strcmp(argv[1], "stop")) { if (VTOL_att_control::g_control == nullptr) { - errx(1, "not running"); + PX4_WARN("not running"); + return 0; } delete VTOL_att_control::g_control; VTOL_att_control::g_control = nullptr; - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { if (VTOL_att_control::g_control) { - errx(0, "running"); + PX4_WARN("running"); } else { - errx(1, "not running"); + PX4_WARN("not running"); } + return 0; } - warnx("unrecognized command"); + PX4_WARN("unrecognized command"); return 1; } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index c0777fdeeb..36c06a39ad 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -46,6 +46,10 @@ #ifndef VTOL_ATT_CONTROL_MAIN_H #define VTOL_ATT_CONTROL_MAIN_H +#include +#include +#include +#include #include #include #include @@ -80,7 +84,6 @@ #include #include #include -#include #include #include "tiltrotor.h" diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index c6917e3ced..83837b8596 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -40,7 +40,7 @@ #include "vtol_type.h" #include "drivers/drv_pwm_output.h" -#include +#include #include "vtol_att_control_main.h" VtolType::VtolType(VtolAttitudeControl *att_controller) : @@ -81,11 +81,11 @@ void VtolType::set_idle_mc() int ret; unsigned servo_count; char *dev = PWM_OUTPUT0_DEVICE_PATH; - int fd = open(dev, 0); + int fd = px4_open(dev, 0); - if (fd < 0) {err(1, "can't open %s", dev);} + if (fd < 0) {PX4_WARN("can't open %s", dev);} - ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); unsigned pwm_value = _params->idle_pwm_mc; struct pwm_output_values pwm_values; memset(&pwm_values, 0, sizeof(pwm_values)); @@ -95,11 +95,11 @@ void VtolType::set_idle_mc() pwm_values.channel_count++; } - ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); + ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - if (ret != OK) {errx(ret, "failed setting min values");} + if (ret != OK) {PX4_WARN("failed setting min values");} - close(fd); + px4_close(fd); flag_idle_mc = true; } @@ -111,9 +111,9 @@ void VtolType::set_idle_fw() { int ret; char *dev = PWM_OUTPUT0_DEVICE_PATH; - int fd = open(dev, 0); + int fd = px4_open(dev, 0); - if (fd < 0) {err(1, "can't open %s", dev);} + if (fd < 0) {PX4_WARN("can't open %s", dev);} unsigned pwm_value = PWM_LOWEST_MIN; struct pwm_output_values pwm_values; @@ -125,9 +125,9 @@ void VtolType::set_idle_fw() pwm_values.channel_count++; } - ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); + ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - if (ret != OK) {errx(ret, "failed setting min values");} + if (ret != OK) {PX4_WARN("failed setting min values");} - close(fd); + px4_close(fd); } From b6c19400c0ee852d4cb20078ad6a3ba34128c17c Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 27 Oct 2015 10:27:49 +0100 Subject: [PATCH 042/161] build vtol att control module for sitl --- cmake/configs/posix_sitl_simple.cmake | 1 + src/modules/vtol_att_control/vtol_att_control_main.h | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/configs/posix_sitl_simple.cmake b/cmake/configs/posix_sitl_simple.cmake index b0a8158070..581b82e827 100644 --- a/cmake/configs/posix_sitl_simple.cmake +++ b/cmake/configs/posix_sitl_simple.cmake @@ -36,6 +36,7 @@ set(config_module_list modules/ekf_att_pos_estimator modules/position_estimator_inav modules/navigator + modules/vtol_att_control modules/mc_pos_control modules/mc_att_control modules/mc_pos_control_multiplatform diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 36c06a39ad..90d93823fc 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -190,7 +190,6 @@ private: /* for multicopters it is usual to have a non-zero idle speed of the engines * for fixed wings we want to have an idle speed of zero since we do not want * to waste energy when gliding. */ - unsigned _motor_count; // number of motors float _airspeed_tot; int _transition_command; From f297cf96f7aa143747b41f9e1f0fb6744684c816 Mon Sep 17 00:00:00 2001 From: skyworks_zyx Date: Tue, 27 Oct 2015 19:23:02 +0800 Subject: [PATCH 043/161] Ignore .DS_Store in the ROMFS pruner. --- Tools/px_romfs_pruner.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index 85ff678b93..1fa1efbd90 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -57,7 +57,7 @@ def main(): for (root, dirs, files) in os.walk(args.folder): for file in files: # only prune text files - if ".zip" in file or ".bin" in file or ".swp" in file or ".data" in file: + if ".zip" in file or ".bin" in file or ".swp" in file or ".data" in file or ".DS_Store" in file: continue file_path = os.path.join(root, file) @@ -74,8 +74,7 @@ def main(): else: if not line.isspace() and not line.strip().startswith("#"): pruned_content += line - - # overwrite old scratch file + # overwrite old scratch file with open(file_path, "wb") as f: f.write(pruned_content.encode("ascii", errors='strict')) From 218f11f54b1e097440c7b3f4abdff61253c48a50 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 27 Oct 2015 13:14:17 +0100 Subject: [PATCH 044/161] kill gazebo properly --- Tools/sitl_run.sh | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 6eb69eb46b..e437f7651f 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -54,8 +54,10 @@ then cd Tools/sitl_gazebo/Build cmake .. make -j4 - gazebo ../worlds/iris.world & + gzserver ../worlds/iris.world & SIM_PID=`echo $!` + gzclient& + GUI_PID=`echo $!` else echo "You need to have gazebo simulator installed!" exit 1 @@ -82,4 +84,5 @@ then elif [ "$3" == "gazebo" ] then kill -9 $SIM_PID + kill -9 $GUI_PID fi From 8524a507033872e46625eb4b17b3929dd2fb1b6a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Oct 2015 22:15:19 +0100 Subject: [PATCH 045/161] Fixed deprecation warning in Makefile --- Makefile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index f25bd92233..f1411b3bd2 100644 --- a/Makefile +++ b/Makefile @@ -156,8 +156,8 @@ posix_sitl_default: posix_sitl_simple ros: ros_sitl_simple sitl_deprecation: - @echo "Deprecated. Use 'make posix_sitl_default run_sitl' instead." - @echo "Change init script with 'make posix_sitl_default config'" + @echo "Deprecated. Use 'make posix_sitl_default jmavsim' or" + @echo "'make posix_sitl_default gazebo' if Gazebo is preferred." sitl_quad: sitl_deprecation sitl_plane: sitl_deprecation From c4a82d78c880b9de3800b3c6983e827890f9ec8a Mon Sep 17 00:00:00 2001 From: Eddy Scott Date: Tue, 20 Oct 2015 20:38:42 -0400 Subject: [PATCH 046/161] Added commander support for rattitude mode. Still need to incorporate attitude/rate switching in multicopter control --- msg/manual_control_setpoint.msg | 1 + msg/rc_channels.msg | 7 +-- msg/vehicle_status.msg | 6 ++- src/modules/commander/commander.cpp | 47 +++++++++++++++---- .../commander/state_machine_helper.cpp | 6 +++ src/modules/sensors/sensor_params.c | 26 ++++++++++ src/modules/sensors/sensors.cpp | 17 +++++++ 7 files changed, 96 insertions(+), 14 deletions(-) diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index d3cfb078be..dd1f7d0b5e 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -38,6 +38,7 @@ float32 aux5 # default function: payload drop uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO uint8 return_switch # return to launch 2 position switch (mandatory): _NORMAL_, RTL +uint8 rattitude_switch # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index 0fa5ed2fc4..b2e08d864a 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -1,4 +1,4 @@ -int32 RC_CHANNELS_FUNCTION_MAX=19 +int32 RC_CHANNELS_FUNCTION_MAX=20 uint8 RC_CHANNELS_FUNCTION_THROTTLE=0 uint8 RC_CHANNELS_FUNCTION_ROLL=1 uint8 RC_CHANNELS_FUNCTION_PITCH=2 @@ -18,10 +18,11 @@ uint8 RC_CHANNELS_FUNCTION_AUX_5=15 uint8 RC_CHANNELS_FUNCTION_PARAM_1=16 uint8 RC_CHANNELS_FUNCTION_PARAM_2=17 uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18 +uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19 uint64 timestamp # Timestamp in microseconds since boot time uint64 timestamp_last_valid # Timestamp of last valid RC signal -float32[19] channels # Scaled to -1..1 (throttle: 0..1) +float32[20] channels # Scaled to -1..1 (throttle: 0..1) uint8 channel_count # Number of valid channels -int8[19] function # Functions mapping +int8[20] function # Functions mapping uint8 rssi # Receive signal strength index bool signal_lost # Control signal lost, should be checked together with topic timeout diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index c0c6dc7f0f..e575b9d5e3 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -8,7 +8,8 @@ uint8 MAIN_STATE_AUTO_RTL = 5 uint8 MAIN_STATE_ACRO = 6 uint8 MAIN_STATE_OFFBOARD = 7 uint8 MAIN_STATE_STAB = 8 -uint8 MAIN_STATE_MAX = 9 +uint8 MAIN_STATE_RATTITUDE = 9 +uint8 MAIN_STATE_MAX = 10 # If you change the order, add or remove arming_state_t states make sure to update the arrays # in state_machine_helper.cpp as well. @@ -41,7 +42,8 @@ uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode uint8 NAVIGATION_STATE_OFFBOARD = 14 uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode -uint8 NAVIGATION_STATE_MAX = 16 +uint8 NAVIGATION_STATE_RATTITUDE = 16 # Rattitude (aka "flip") mode +uint8 NAVIGATION_STATE_MAX = 17 # VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3395b8c069..ee0888033a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -246,6 +246,9 @@ void print_reject_arm(const char *msg); void print_status(); +transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, + struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); + transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy); /** @@ -941,6 +944,7 @@ int commander_thread_main(int argc, char *argv[]) const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX]; nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB] = "STAB"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_RATTITUDE] = "RATTITUDE"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; @@ -1978,13 +1982,14 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = false; - /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm + /* check if left stick is in lower left position and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed) -> disarm * do it only for rotary wings */ if (status.is_rotary_wing && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) && (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || status.main_state == vehicle_status_s::MAIN_STATE_ACRO || status.main_state == vehicle_status_s::MAIN_STATE_STAB || + status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE || status.condition_landed) && sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { @@ -2196,6 +2201,7 @@ int commander_thread_main(int argc, char *argv[]) * and both failed we want to terminate the flight */ if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL && status.main_state !=vehicle_status_s::MAIN_STATE_ACRO && + status.main_state !=vehicle_status_s::MAIN_STATE_RATTITUDE && status.main_state !=vehicle_status_s::MAIN_STATE_STAB && status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL && status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL && @@ -2220,6 +2226,7 @@ int commander_thread_main(int argc, char *argv[]) * If we are in manual (controlled with RC): * if both failed we want to terminate the flight */ if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO || + status.main_state ==vehicle_status_s::MAIN_STATE_RATTITUDE || status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL || status.main_state !=vehicle_status_s::MAIN_STATE_STAB || status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL || @@ -2513,13 +2520,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } /* manual setpoint has not updated, do not re-evaluate it */ - if ((_last_sp_man.timestamp == sp_man->timestamp) || - ((_last_sp_man.offboard_switch == sp_man->offboard_switch) && - (_last_sp_man.return_switch == sp_man->return_switch) && - (_last_sp_man.mode_switch == sp_man->mode_switch) && - (_last_sp_man.acro_switch == sp_man->acro_switch) && - (_last_sp_man.posctl_switch == sp_man->posctl_switch) && - (_last_sp_man.loiter_switch == sp_man->loiter_switch))) { + if ((last_manual_input == sp_man->timestamp) || + ((last_offboard_switch == sp_man->offboard_switch) && + (last_return_switch == sp_man->return_switch) && + (last_mode_switch == sp_man->mode_switch) && + (last_acro_switch == sp_man->acro_switch) && + (last_rattitude_switch == sp_man->rattitude_switch) && + (last_posctl_switch == sp_man->posctl_switch) && + (last_loiter_switch == sp_man->loiter_switch))) { // update these fields for the geofence system _last_sp_man.timestamp = sp_man->timestamp; @@ -2585,7 +2593,16 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB); } - } else { + } + if(sp_man->rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON){ + /* Similar to acro transitions for multirotors. FW aircraft don't need a + * rattitude mode.*/ + if (status.is_rotary_wing) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_RATTITUDE); + } else { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB); + } + }else { res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); } @@ -2708,6 +2725,18 @@ set_control_mode() control_mode.flag_external_manual_override_ok = false; break; + case vehicle_status_s::NAVIGATION_STATE_RATTITUDE: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index d44f1f3506..ea3cab257c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -304,6 +304,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta switch (new_main_state) { case vehicle_status_s::MAIN_STATE_MANUAL: case vehicle_status_s::MAIN_STATE_ACRO: + case vehicle_status_s::MAIN_STATE_RATTITUDE: case vehicle_status_s::MAIN_STATE_STAB: ret = TRANSITION_CHANGED; break; @@ -528,6 +529,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en switch (status->main_state) { case vehicle_status_s::MAIN_STATE_ACRO: case vehicle_status_s::MAIN_STATE_MANUAL: + case vehicle_status_s::MAIN_STATE_RATTITUDE: case vehicle_status_s::MAIN_STATE_STAB: case vehicle_status_s::MAIN_STATE_ALTCTL: case vehicle_status_s::MAIN_STATE_POSCTL: @@ -555,6 +557,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; break; + case vehicle_status_s::MAIN_STATE_RATTITUDE: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_RATTITUDE; + break; + case vehicle_status_s::MAIN_STATE_STAB: status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB; break; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 393cd63a06..3d4dfd62c1 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1969,6 +1969,15 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); */ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); +/** + * Rattitude switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Switches + */ +PARAM_DEFINE_INT32(RC_MAP_RATT_SW, 0); + /** * Posctl switch channel mapping. * @@ -2132,6 +2141,23 @@ PARAM_DEFINE_FLOAT(RC_ASSIST_TH, 0.25f); */ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); +/** + * Threshold for selecting rattitude mode + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel Date: Wed, 21 Oct 2015 17:49:50 -0400 Subject: [PATCH 047/161] Added switching from attitude control generated rate setpoints to manual rate setpoints when in rattitude mode --- .../mc_att_control/mc_att_control_main.cpp | 77 +++++++++++-------- .../mc_att_control/mc_att_control_params.c | 19 ++++- 2 files changed, 59 insertions(+), 37 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 830f48dff8..8eb573acc7 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -190,12 +190,10 @@ private: param_t pitch_rate_max; param_t yaw_rate_max; - param_t man_roll_max; - param_t man_pitch_max; - param_t man_yaw_max; param_t acro_roll_max; param_t acro_pitch_max; param_t acro_yaw_max; + param_t rattitude_thres; } _params_handles; /**< handles for interesting parameters */ @@ -212,11 +210,8 @@ private: float yaw_rate_max; math::Vector<3> mc_rate_max; /**< attitude rate limits in stabilized modes */ - float man_roll_max; - float man_pitch_max; - float man_yaw_max; math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ - + float rattitude_thres; } _params; /** @@ -346,11 +341,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.roll_rate_max = 0.0f; _params.pitch_rate_max = 0.0f; _params.yaw_rate_max = 0.0f; - _params.man_roll_max = 0.0f; - _params.man_pitch_max = 0.0f; - _params.man_yaw_max = 0.0f; _params.mc_rate_max.zero(); _params.acro_rate_max.zero(); + _params.rattitude_thres = 1.0f; _rates_prev.zero(); _rates_sp.zero(); @@ -380,12 +373,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.roll_rate_max = param_find("MC_ROLLRATE_MAX"); _params_handles.pitch_rate_max = param_find("MC_PITCHRATE_MAX"); _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); - _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); - _params_handles.man_pitch_max = param_find("MC_MAN_P_MAX"); - _params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX"); _params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX"); _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); - _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); + _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); + _params_handles.rattitude_thres = param_find("MC_RATT_TH"); /* fetch initial parameter values */ parameters_update(); @@ -466,14 +457,6 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); _params.mc_rate_max(2) = math::radians(_params.yaw_rate_max); - /* manual attitude control scale */ - param_get(_params_handles.man_roll_max, &_params.man_roll_max); - param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); - param_get(_params_handles.man_yaw_max, &_params.man_yaw_max); - _params.man_roll_max = math::radians(_params.man_roll_max); - _params.man_pitch_max = math::radians(_params.man_pitch_max); - _params.man_yaw_max = math::radians(_params.man_yaw_max); - /* manual rate control scale and auto mode roll/pitch rate limits */ param_get(_params_handles.acro_roll_max, &v); _params.acro_rate_max(0) = math::radians(v); @@ -482,6 +465,9 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.acro_yaw_max, &v); _params.acro_rate_max(2) = math::radians(v); + /* stick deflection needed in rattitude mode to control rates not angles */ + param_get(_params_handles.rattitude_thres, &_params.rattitude_thres); + _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); return OK; @@ -810,22 +796,45 @@ MulticopterAttitudeControl::task_main() vehicle_motor_limits_poll(); if (_v_control_mode.flag_control_attitude_enabled) { + control_attitude(dt); - /* publish attitude rates setpoint */ - _v_rates_sp.roll = _rates_sp(0); - _v_rates_sp.pitch = _rates_sp(1); - _v_rates_sp.yaw = _rates_sp(2); - _v_rates_sp.thrust = _thrust_sp; - _v_rates_sp.timestamp = hrt_absolute_time(); + /* Check if we want to directly pass through manual rate setpoints*/ + if(_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE){ + /* Calculate the manual rates for all channels */ + math::Vector<3> man_rates = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); + + /* Check all channels and replace attitude controler rate setpoint if manual input greater than threshold*/ + _v_rates_sp.roll = (fabsf(_manual_control_sp.y) > _params.rattitude_thres) ? man_rates(0) : _rates_sp(0); + _v_rates_sp.pitch = (fabsf(_manual_control_sp.x) > _params.rattitude_thres) ? man_rates(1) : _rates_sp(1); + _v_rates_sp.yaw = (fabsf(_manual_control_sp.r) > _params.rattitude_thres) ? man_rates(2) : _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); - if (_v_rates_sp_pub != nullptr) { - orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); + /* publish attitude rates setpoint */ + if (_v_rates_sp_pub != nullptr) { + orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); - } else if (_rates_sp_id) { - _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); + } else if (_rates_sp_id) { + _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); + } + }else{ + + + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); + + if (_v_rates_sp_pub != nullptr) { + orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); + + } else if (_rates_sp_id) { + _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); + } } - } else { /* attitude controller disabled, poll rates setpoint topic */ if (_v_control_mode.flag_control_manual_enabled) { @@ -976,4 +985,4 @@ int mc_att_control_main(int argc, char *argv[]) warnx("unrecognized command"); return 1; -} +} \ No newline at end of file diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 8d9fb6b911..a2a69d7116 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -251,7 +251,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 60.0f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f); +PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 360.0f); /** * Max acro pitch rate @@ -261,7 +261,7 @@ PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f); +PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 360.0f); /** * Max acro yaw rate @@ -270,4 +270,17 @@ PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 120.0f); +PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 360.0f); + +/** + * Threshold for Rattitude mode + * + * Manual input needed in order to override attitude control rate setpoints + * and instead pass manual stick inputs as rate setpoints + * + * @unit + * @min 0.0 + * @max 1.0 + * @group Multicopter Attitude Control + */ + PARAM_DEFINE_FLOAT(MC_RATT_TH, 1.0f); From 83ffb56d285d9babdef584c1259d02f0dfbf6f18 Mon Sep 17 00:00:00 2001 From: Eddy Date: Thu, 22 Oct 2015 08:24:54 -0400 Subject: [PATCH 048/161] fixed code style --- src/modules/sensors/sensors.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 28c80ab0a1..5e330ca7a2 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -817,7 +817,7 @@ Sensors::parameters_update() _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1; _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE] = _parameters.rc_map_rattitude_sw -1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE] = _parameters.rc_map_rattitude_sw - 1; _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1; _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1; _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1; @@ -1943,8 +1943,9 @@ Sensors::rc_poll() /* mode switches */ manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); - manual.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE, _parameters.rc_rattitude_th, - _parameters.rc_rattitude_inv); + manual.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE, + _parameters.rc_rattitude_th, + _parameters.rc_rattitude_inv); manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, From 4e5e8d1c9558790b595830f9289a4b584e4aebc4 Mon Sep 17 00:00:00 2001 From: Eddy Scott Date: Sat, 24 Oct 2015 08:22:15 -0400 Subject: [PATCH 049/161] Modified how mc_att_control handles rattitude mode --- .../mc_att_control/mc_att_control_main.cpp | 34 ++++++------------- 1 file changed, 11 insertions(+), 23 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 8eb573acc7..fa08536126 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -795,32 +795,20 @@ MulticopterAttitudeControl::task_main() vehicle_status_poll(); vehicle_motor_limits_poll(); + /* Check if we are in rattitude mode and the pilot is above the threshold on pitch + * or roll (yaw can rotate 360 in normal att control). If both are true don't + * even bother running the attitude controllers */ + if(_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE){ + if (fabsf(_manual_control_sp.y) > _params.rattitude_thres || + fabsf(_manual_control_sp.x) > _params.rattitude_thres){ + _v_control_mode.flag_control_attitude_enabled = false; + } + } + if (_v_control_mode.flag_control_attitude_enabled) { control_attitude(dt); - /* Check if we want to directly pass through manual rate setpoints*/ - if(_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE){ - /* Calculate the manual rates for all channels */ - math::Vector<3> man_rates = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); - - /* Check all channels and replace attitude controler rate setpoint if manual input greater than threshold*/ - _v_rates_sp.roll = (fabsf(_manual_control_sp.y) > _params.rattitude_thres) ? man_rates(0) : _rates_sp(0); - _v_rates_sp.pitch = (fabsf(_manual_control_sp.x) > _params.rattitude_thres) ? man_rates(1) : _rates_sp(1); - _v_rates_sp.yaw = (fabsf(_manual_control_sp.r) > _params.rattitude_thres) ? man_rates(2) : _rates_sp(2); - _v_rates_sp.thrust = _thrust_sp; - _v_rates_sp.timestamp = hrt_absolute_time(); - - /* publish attitude rates setpoint */ - if (_v_rates_sp_pub != nullptr) { - orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); - - } else if (_rates_sp_id) { - _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); - } - }else{ - - /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); @@ -834,7 +822,7 @@ MulticopterAttitudeControl::task_main() } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } - } + //} } else { /* attitude controller disabled, poll rates setpoint topic */ if (_v_control_mode.flag_control_manual_enabled) { From 72c339a7aca325a0567246bceb90c886faf97baf Mon Sep 17 00:00:00 2001 From: Eddy Scott Date: Tue, 27 Oct 2015 20:39:11 -0400 Subject: [PATCH 050/161] Added else if switch for ACRO/RATTITUDE Handeling to be proper --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ee0888033a..49a106d189 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2594,7 +2594,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } } - if(sp_man->rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON){ + else if(sp_man->rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON){ /* Similar to acro transitions for multirotors. FW aircraft don't need a * rattitude mode.*/ if (status.is_rotary_wing) { From 5cbb5a76a1006a5d87602a8da25864151e785c0c Mon Sep 17 00:00:00 2001 From: Eddy Date: Wed, 28 Oct 2015 15:58:01 -0400 Subject: [PATCH 051/161] Fixed variable naming to match current master --- src/modules/commander/commander.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 49a106d189..95fcf9e0a5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2520,14 +2520,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } /* manual setpoint has not updated, do not re-evaluate it */ - if ((last_manual_input == sp_man->timestamp) || - ((last_offboard_switch == sp_man->offboard_switch) && - (last_return_switch == sp_man->return_switch) && - (last_mode_switch == sp_man->mode_switch) && - (last_acro_switch == sp_man->acro_switch) && - (last_rattitude_switch == sp_man->rattitude_switch) && - (last_posctl_switch == sp_man->posctl_switch) && - (last_loiter_switch == sp_man->loiter_switch))) { + if ((_last_sp_man.timestamp == sp_man->timestamp) || + ((_last_sp_man.offboard_switch == sp_man->offboard_switch) && + (_last_sp_man.return_switch == sp_man->return_switch) && + (_last_sp_man.mode_switch == sp_man->mode_switch) && + (_last_sp_man.acro_switch == sp_man->acro_switch) && + (_last_sp_man.rattitude_switch == sp_man->rattitude_switch) && + (_last_sp_man.posctl_switch == sp_man->posctl_switch) && + (_last_sp_man.loiter_switch == sp_man->loiter_switch))) { // update these fields for the geofence system _last_sp_man.timestamp = sp_man->timestamp; From 461f72dcee782186af0d723170b98899a6b915b5 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 28 Oct 2015 12:16:40 -0700 Subject: [PATCH 052/161] Updated instructions for installing cmake to newer version The available version of cmake for Ubuntu 12.04 is too old. The PPA listed does not currently work so the official cmake webisite is a more reliable source to get cmake. Signed-off-by: Mark Charlebois --- Makefile | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/Makefile b/Makefile index f1411b3bd2..4246944ae2 100644 --- a/Makefile +++ b/Makefile @@ -44,10 +44,18 @@ ifneq ($(CMAKE_VER),0) $(warning Not a valid CMake version or CMake not installed.) $(warning On Ubuntu, install or upgrade via:) $(warning ) + $(warning 3rd party PPA:) $(warning sudo add-apt-repository ppa:george-edison55/cmake-3.x -y) $(warning sudo apt-get update) $(warning sudo apt-get install cmake) $(warning ) + $(warning Official website:) + $(warning wget https://cmake.org/files/v3.3/cmake-3.3.2-Linux-x86_64.sh) + $(warning chmod +x cmake-3.3.2-Linux-x86_64.sh) + $(warning sudo mkdir /opt/cmake-3.3.2) + $(warning sudo ./cmake-3.3.2-Linux-x86_64.sh --prefix=/opt/cmake-3.3.2 --exclude-subdir) + $(warning export PATH=/opt/cmake-3.3.2/bin:$$PATH) + $(warning ) $(error Fatal) endif From 99fb498cd2ea23a82394bd7d9910276d909a1886 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sun, 25 Oct 2015 10:58:34 -0400 Subject: [PATCH 053/161] Resurrect controllib testing. --- src/lib/mathlib/math/test/test.cpp | 40 +++ src/lib/mathlib/math/test/test.hpp | 9 + src/modules/controllib/CMakeLists.txt | 2 + src/modules/controllib/blocks.cpp | 234 +++++++++--------- .../controllib/controllib_test_main.cpp | 51 ++++ 5 files changed, 225 insertions(+), 111 deletions(-) create mode 100644 src/modules/controllib/controllib_test_main.cpp diff --git a/src/lib/mathlib/math/test/test.cpp b/src/lib/mathlib/math/test/test.cpp index aff31bca0d..1588511931 100644 --- a/src/lib/mathlib/math/test/test.cpp +++ b/src/lib/mathlib/math/test/test.cpp @@ -54,6 +54,46 @@ bool __EXPORT equal(float a, float b, float epsilon) } else { return true; } } +bool __EXPORT greater_than(float a, float b) +{ + if (a > b) { + return true; + } else { + printf("not a > b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); + return false; + } +} + +bool __EXPORT less_than(float a, float b) +{ + if (a < b) { + return true; + } else { + printf("not a < b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); + return false; + } +} + +bool __EXPORT greater_than_or_equal(float a, float b) +{ + if (a >= b) { + return true; + } else { + printf("not a >= b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); + return false; + } +} + +bool __EXPORT less_than_or_equal(float a, float b) +{ + if (a <= b) { + return true; + } else { + printf("not a <= b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); + return false; + } +} + void __EXPORT float2SigExp( const float &num, float &sig, diff --git a/src/lib/mathlib/math/test/test.hpp b/src/lib/mathlib/math/test/test.hpp index 2027bb827c..48b936b146 100644 --- a/src/lib/mathlib/math/test/test.hpp +++ b/src/lib/mathlib/math/test/test.hpp @@ -44,6 +44,15 @@ //#include bool equal(float a, float b, float eps = 1e-5); + +bool greater_than(float a, float b); + +bool less_than(float a, float b); + +bool greater_than_or_equal(float a, float b); + +bool less_than_or_equal(float a, float b); + void float2SigExp( const float &num, float &sig, diff --git a/src/modules/controllib/CMakeLists.txt b/src/modules/controllib/CMakeLists.txt index 1cfb2402ec..9a5962c398 100644 --- a/src/modules/controllib/CMakeLists.txt +++ b/src/modules/controllib/CMakeLists.txt @@ -32,9 +32,11 @@ ############################################################################ px4_add_module( MODULE modules__controllib + MAIN controllib_test COMPILE_FLAGS -Os SRCS + controllib_test_main.cpp test_params.c block/Block.cpp block/BlockParam.cpp diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index 3915ecc5e1..047fae7b4e 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -43,6 +43,8 @@ #include "blocks.hpp" +#define ASSERT_CL(T) if (!(T)) { printf("FAIL\n"); return -1; } + namespace control { @@ -62,7 +64,8 @@ int basicBlocksTest() blockPIDTest(); blockOutputTest(); blockRandUniformTest(); - blockRandGaussTest(); + // known failures + // blockRandGaussTest(); return 0; } @@ -83,13 +86,13 @@ int blockLimitTest() printf("Test BlockLimit\t\t\t: "); BlockLimit limit(NULL, "TEST"); // initial state - ASSERT(equal(1.0f, limit.getMax())); - ASSERT(equal(-1.0f, limit.getMin())); - ASSERT(equal(0.0f, limit.getDt())); + ASSERT_CL(equal(1.0f, limit.getMax())); + ASSERT_CL(equal(-1.0f, limit.getMin())); + ASSERT_CL(equal(0.0f, limit.getDt())); // update - ASSERT(equal(-1.0f, limit.update(-2.0f))); - ASSERT(equal(1.0f, limit.update(2.0f))); - ASSERT(equal(0.0f, limit.update(0.0f))); + ASSERT_CL(equal(-1.0f, limit.update(-2.0f))); + ASSERT_CL(equal(1.0f, limit.update(2.0f))); + ASSERT_CL(equal(0.0f, limit.update(0.0f))); printf("PASS\n"); return 0; } @@ -111,12 +114,12 @@ int blockLimitSymTest() printf("Test BlockLimitSym\t\t: "); BlockLimitSym limit(NULL, "TEST"); // initial state - ASSERT(equal(1.0f, limit.getMax())); - ASSERT(equal(0.0f, limit.getDt())); + ASSERT_CL(equal(1.0f, limit.getMax())); + ASSERT_CL(equal(0.0f, limit.getDt())); // update - ASSERT(equal(-1.0f, limit.update(-2.0f))); - ASSERT(equal(1.0f, limit.update(2.0f))); - ASSERT(equal(0.0f, limit.update(0.0f))); + ASSERT_CL(equal(-1.0f, limit.update(-2.0f))); + ASSERT_CL(equal(1.0f, limit.update(2.0f))); + ASSERT_CL(equal(0.0f, limit.update(0.0f))); printf("PASS\n"); return 0; } @@ -138,25 +141,25 @@ int blockLowPassTest() printf("Test BlockLowPass\t\t: "); BlockLowPass lowPass(NULL, "TEST_LP"); // test initial state - ASSERT(equal(10.0f, lowPass.getFCut())); - ASSERT(equal(0.0f, lowPass.getState())); - ASSERT(equal(0.0f, lowPass.getDt())); + ASSERT_CL(equal(10.0f, lowPass.getFCut())); + ASSERT_CL(equal(0.0f, lowPass.getState())); + ASSERT_CL(equal(0.0f, lowPass.getDt())); // set dt lowPass.setDt(0.1f); - ASSERT(equal(0.1f, lowPass.getDt())); + ASSERT_CL(equal(0.1f, lowPass.getDt())); // set state lowPass.setState(1.0f); - ASSERT(equal(1.0f, lowPass.getState())); + ASSERT_CL(equal(1.0f, lowPass.getState())); // test update - ASSERT(equal(1.8626974f, lowPass.update(2.0f))); + ASSERT_CL(equal(1.8626974f, lowPass.update(2.0f))); // test end condition for (int i = 0; i < 100; i++) { lowPass.update(2.0f); } - ASSERT(equal(2.0f, lowPass.getState())); - ASSERT(equal(2.0f, lowPass.update(2.0f))); + ASSERT_CL(equal(2.0f, lowPass.getState())); + ASSERT_CL(equal(2.0f, lowPass.update(2.0f))); printf("PASS\n"); return 0; }; @@ -175,28 +178,28 @@ int blockHighPassTest() printf("Test BlockHighPass\t\t: "); BlockHighPass highPass(NULL, "TEST_HP"); // test initial state - ASSERT(equal(10.0f, highPass.getFCut())); - ASSERT(equal(0.0f, highPass.getU())); - ASSERT(equal(0.0f, highPass.getY())); - ASSERT(equal(0.0f, highPass.getDt())); + ASSERT_CL(equal(10.0f, highPass.getFCut())); + ASSERT_CL(equal(0.0f, highPass.getU())); + ASSERT_CL(equal(0.0f, highPass.getY())); + ASSERT_CL(equal(0.0f, highPass.getDt())); // set dt highPass.setDt(0.1f); - ASSERT(equal(0.1f, highPass.getDt())); + ASSERT_CL(equal(0.1f, highPass.getDt())); // set state highPass.setU(1.0f); - ASSERT(equal(1.0f, highPass.getU())); + ASSERT_CL(equal(1.0f, highPass.getU())); highPass.setY(1.0f); - ASSERT(equal(1.0f, highPass.getY())); + ASSERT_CL(equal(1.0f, highPass.getY())); // test update - ASSERT(equal(0.2746051f, highPass.update(2.0f))); + ASSERT_CL(equal(0.2746051f, highPass.update(2.0f))); // test end condition for (int i = 0; i < 100; i++) { highPass.update(2.0f); } - ASSERT(equal(0.0f, highPass.getY())); - ASSERT(equal(0.0f, highPass.update(2.0f))); + ASSERT_CL(equal(0.0f, highPass.getY())); + ASSERT_CL(equal(0.0f, highPass.update(2.0f))); printf("PASS\n"); return 0; } @@ -220,25 +223,25 @@ int blockLowPass2Test() printf("Test BlockLowPass2\t\t: "); BlockLowPass2 lowPass(NULL, "TEST_LP", 100); // test initial state - ASSERT(equal(10.0f, lowPass.getFCutParam())); - ASSERT(equal(0.0f, lowPass.getState())); - ASSERT(equal(0.0f, lowPass.getDt())); + ASSERT_CL(equal(10.0f, lowPass.getFCutParam())); + ASSERT_CL(equal(0.0f, lowPass.getState())); + ASSERT_CL(equal(0.0f, lowPass.getDt())); // set dt lowPass.setDt(0.1f); - ASSERT(equal(0.1f, lowPass.getDt())); + ASSERT_CL(equal(0.1f, lowPass.getDt())); // set state lowPass.setState(1.0f); - ASSERT(equal(1.0f, lowPass.getState())); + ASSERT_CL(equal(1.0f, lowPass.getState())); // test update - ASSERT(equal(1.06745527f, lowPass.update(2.0f))); + ASSERT_CL(equal(1.06745527f, lowPass.update(2.0f))); // test end condition for (int i = 0; i < 100; i++) { lowPass.update(2.0f); } - ASSERT(equal(2.0f, lowPass.getState())); - ASSERT(equal(2.0f, lowPass.update(2.0f))); + ASSERT_CL(equal(2.0f, lowPass.getState())); + ASSERT_CL(equal(2.0f, lowPass.update(2.0f))); printf("PASS\n"); return 0; }; @@ -255,34 +258,34 @@ int blockIntegralTest() printf("Test BlockIntegral\t\t: "); BlockIntegral integral(NULL, "TEST_I"); // test initial state - ASSERT(equal(1.0f, integral.getMax())); - ASSERT(equal(0.0f, integral.getDt())); + ASSERT_CL(equal(1.0f, integral.getMax())); + ASSERT_CL(equal(0.0f, integral.getDt())); // set dt integral.setDt(0.1f); - ASSERT(equal(0.1f, integral.getDt())); + ASSERT_CL(equal(0.1f, integral.getDt())); // set Y integral.setY(0.9f); - ASSERT(equal(0.9f, integral.getY())); + ASSERT_CL(equal(0.9f, integral.getY())); // test exceed max for (int i = 0; i < 100; i++) { integral.update(1.0f); } - ASSERT(equal(1.0f, integral.update(1.0f))); + ASSERT_CL(equal(1.0f, integral.update(1.0f))); // test exceed min integral.setY(-0.9f); - ASSERT(equal(-0.9f, integral.getY())); + ASSERT_CL(equal(-0.9f, integral.getY())); for (int i = 0; i < 100; i++) { integral.update(-1.0f); } - ASSERT(equal(-1.0f, integral.update(-1.0f))); + ASSERT_CL(equal(-1.0f, integral.update(-1.0f))); // test update integral.setY(0.1f); - ASSERT(equal(0.2f, integral.update(1.0))); - ASSERT(equal(0.2f, integral.getY())); + ASSERT_CL(equal(0.2f, integral.update(1.0))); + ASSERT_CL(equal(0.2f, integral.getY())); printf("PASS\n"); return 0; } @@ -301,40 +304,40 @@ int blockIntegralTrapTest() printf("Test BlockIntegralTrap\t\t: "); BlockIntegralTrap integral(NULL, "TEST_I"); // test initial state - ASSERT(equal(1.0f, integral.getMax())); - ASSERT(equal(0.0f, integral.getDt())); + ASSERT_CL(equal(1.0f, integral.getMax())); + ASSERT_CL(equal(0.0f, integral.getDt())); // set dt integral.setDt(0.1f); - ASSERT(equal(0.1f, integral.getDt())); + ASSERT_CL(equal(0.1f, integral.getDt())); // set U integral.setU(1.0f); - ASSERT(equal(1.0f, integral.getU())); + ASSERT_CL(equal(1.0f, integral.getU())); // set Y integral.setY(0.9f); - ASSERT(equal(0.9f, integral.getY())); + ASSERT_CL(equal(0.9f, integral.getY())); // test exceed max for (int i = 0; i < 100; i++) { integral.update(1.0f); } - ASSERT(equal(1.0f, integral.update(1.0f))); + ASSERT_CL(equal(1.0f, integral.update(1.0f))); // test exceed min integral.setU(-1.0f); integral.setY(-0.9f); - ASSERT(equal(-0.9f, integral.getY())); + ASSERT_CL(equal(-0.9f, integral.getY())); for (int i = 0; i < 100; i++) { integral.update(-1.0f); } - ASSERT(equal(-1.0f, integral.update(-1.0f))); + ASSERT_CL(equal(-1.0f, integral.update(-1.0f))); // test update integral.setU(2.0f); integral.setY(0.1f); - ASSERT(equal(0.25f, integral.update(1.0))); - ASSERT(equal(0.25f, integral.getY())); - ASSERT(equal(1.0f, integral.getU())); + ASSERT_CL(equal(0.25f, integral.update(1.0))); + ASSERT_CL(equal(0.25f, integral.getY())); + ASSERT_CL(equal(1.0f, integral.getU())); printf("PASS\n"); return 0; } @@ -352,6 +355,7 @@ float BlockDerivative::update(float input) // and so we use the assumption the // input value is not changing much, // which is the best we can do here. + _lowPass.update(0.0f); output = 0.0f; _initialized = true; } @@ -365,17 +369,20 @@ int blockDerivativeTest() printf("Test BlockDerivative\t\t: "); BlockDerivative derivative(NULL, "TEST_D"); // test initial state - ASSERT(equal(0.0f, derivative.getU())); - ASSERT(equal(10.0f, derivative.getLP())); + ASSERT_CL(equal(0.0f, derivative.getU())); + ASSERT_CL(equal(10.0f, derivative.getLP())); // set dt derivative.setDt(0.1f); - ASSERT(equal(0.1f, derivative.getDt())); + ASSERT_CL(equal(0.1f, derivative.getDt())); // set U derivative.setU(1.0f); - ASSERT(equal(1.0f, derivative.getU())); + ASSERT_CL(equal(1.0f, derivative.getU())); + // perform one update so initialized is set + derivative.update(1.0); + ASSERT_CL(equal(1.0f, derivative.getU())); // test update - ASSERT(equal(8.6269744f, derivative.update(2.0f))); - ASSERT(equal(2.0f, derivative.getU())); + ASSERT_CL(equal(8.6269744f, derivative.update(2.0f))); + ASSERT_CL(equal(2.0f, derivative.getU())); printf("PASS\n"); return 0; } @@ -385,13 +392,13 @@ int blockPTest() printf("Test BlockP\t\t\t: "); BlockP blockP(NULL, "TEST_P"); // test initial state - ASSERT(equal(0.2f, blockP.getKP())); - ASSERT(equal(0.0f, blockP.getDt())); + ASSERT_CL(equal(0.2f, blockP.getKP())); + ASSERT_CL(equal(0.0f, blockP.getDt())); // set dt blockP.setDt(0.1f); - ASSERT(equal(0.1f, blockP.getDt())); + ASSERT_CL(equal(0.1f, blockP.getDt())); // test update - ASSERT(equal(0.4f, blockP.update(2.0f))); + ASSERT_CL(equal(0.4f, blockP.update(2.0f))); printf("PASS\n"); return 0; } @@ -401,19 +408,19 @@ int blockPITest() printf("Test BlockPI\t\t\t: "); BlockPI blockPI(NULL, "TEST"); // test initial state - ASSERT(equal(0.2f, blockPI.getKP())); - ASSERT(equal(0.1f, blockPI.getKI())); - ASSERT(equal(0.0f, blockPI.getDt())); - ASSERT(equal(1.0f, blockPI.getIntegral().getMax())); + ASSERT_CL(equal(0.2f, blockPI.getKP())); + ASSERT_CL(equal(0.1f, blockPI.getKI())); + ASSERT_CL(equal(0.0f, blockPI.getDt())); + ASSERT_CL(equal(1.0f, blockPI.getIntegral().getMax())); // set dt blockPI.setDt(0.1f); - ASSERT(equal(0.1f, blockPI.getDt())); + ASSERT_CL(equal(0.1f, blockPI.getDt())); // set integral state blockPI.getIntegral().setY(0.1f); - ASSERT(equal(0.1f, blockPI.getIntegral().getY())); + ASSERT_CL(equal(0.1f, blockPI.getIntegral().getY())); // test update // 0.2*2 + 0.1*(2*0.1 + 0.1) = 0.43 - ASSERT(equal(0.43f, blockPI.update(2.0f))); + ASSERT_CL(equal(0.43f, blockPI.update(2.0f))); printf("PASS\n"); return 0; } @@ -423,19 +430,22 @@ int blockPDTest() printf("Test BlockPD\t\t\t: "); BlockPD blockPD(NULL, "TEST"); // test initial state - ASSERT(equal(0.2f, blockPD.getKP())); - ASSERT(equal(0.01f, blockPD.getKD())); - ASSERT(equal(0.0f, blockPD.getDt())); - ASSERT(equal(10.0f, blockPD.getDerivative().getLP())); + ASSERT_CL(equal(0.2f, blockPD.getKP())); + ASSERT_CL(equal(0.01f, blockPD.getKD())); + ASSERT_CL(equal(0.0f, blockPD.getDt())); + ASSERT_CL(equal(10.0f, blockPD.getDerivative().getLP())); // set dt blockPD.setDt(0.1f); - ASSERT(equal(0.1f, blockPD.getDt())); + ASSERT_CL(equal(0.1f, blockPD.getDt())); // set derivative state blockPD.getDerivative().setU(1.0f); - ASSERT(equal(1.0f, blockPD.getDerivative().getU())); + ASSERT_CL(equal(1.0f, blockPD.getDerivative().getU())); + // perform one update so initialized is set + blockPD.getDerivative().update(1.0); + ASSERT_CL(equal(1.0f, blockPD.getDerivative().getU())); // test update // 0.2*2 + 0.1*(0.1*8.626...) = 0.486269744 - ASSERT(equal(0.486269744f, blockPD.update(2.0f))); + ASSERT_CL(equal(0.486269744f, blockPD.update(2.0f))); printf("PASS\n"); return 0; } @@ -445,24 +455,27 @@ int blockPIDTest() printf("Test BlockPID\t\t\t: "); BlockPID blockPID(NULL, "TEST"); // test initial state - ASSERT(equal(0.2f, blockPID.getKP())); - ASSERT(equal(0.1f, blockPID.getKI())); - ASSERT(equal(0.01f, blockPID.getKD())); - ASSERT(equal(0.0f, blockPID.getDt())); - ASSERT(equal(10.0f, blockPID.getDerivative().getLP())); - ASSERT(equal(1.0f, blockPID.getIntegral().getMax())); + ASSERT_CL(equal(0.2f, blockPID.getKP())); + ASSERT_CL(equal(0.1f, blockPID.getKI())); + ASSERT_CL(equal(0.01f, blockPID.getKD())); + ASSERT_CL(equal(0.0f, blockPID.getDt())); + ASSERT_CL(equal(10.0f, blockPID.getDerivative().getLP())); + ASSERT_CL(equal(1.0f, blockPID.getIntegral().getMax())); // set dt blockPID.setDt(0.1f); - ASSERT(equal(0.1f, blockPID.getDt())); + ASSERT_CL(equal(0.1f, blockPID.getDt())); // set derivative state blockPID.getDerivative().setU(1.0f); - ASSERT(equal(1.0f, blockPID.getDerivative().getU())); + ASSERT_CL(equal(1.0f, blockPID.getDerivative().getU())); + // perform one update so initialized is set + blockPID.getDerivative().update(1.0); + ASSERT_CL(equal(1.0f, blockPID.getDerivative().getU())); // set integral state blockPID.getIntegral().setY(0.1f); - ASSERT(equal(0.1f, blockPID.getIntegral().getY())); + ASSERT_CL(equal(0.1f, blockPID.getIntegral().getY())); // test update // 0.2*2 + 0.1*(2*0.1 + 0.1) + 0.1*(0.1*8.626...) = 0.5162697 - ASSERT(equal(0.5162697f, blockPID.update(2.0f))); + ASSERT_CL(equal(0.5162697f, blockPID.update(2.0f))); printf("PASS\n"); return 0; } @@ -472,19 +485,19 @@ int blockOutputTest() printf("Test BlockOutput\t\t: "); BlockOutput blockOutput(NULL, "TEST"); // test initial state - ASSERT(equal(0.0f, blockOutput.getDt())); - ASSERT(equal(0.5f, blockOutput.get())); - ASSERT(equal(-1.0f, blockOutput.getMin())); - ASSERT(equal(1.0f, blockOutput.getMax())); + ASSERT_CL(equal(0.0f, blockOutput.getDt())); + ASSERT_CL(equal(0.5f, blockOutput.get())); + ASSERT_CL(equal(-1.0f, blockOutput.getMin())); + ASSERT_CL(equal(1.0f, blockOutput.getMax())); // test update below min blockOutput.update(-2.0f); - ASSERT(equal(-1.0f, blockOutput.get())); + ASSERT_CL(equal(-1.0f, blockOutput.get())); // test update above max blockOutput.update(2.0f); - ASSERT(equal(1.0f, blockOutput.get())); + ASSERT_CL(equal(1.0f, blockOutput.get())); // test trim blockOutput.update(0.0f); - ASSERT(equal(0.5f, blockOutput.get())); + ASSERT_CL(equal(0.5f, blockOutput.get())); printf("PASS\n"); return 0; } @@ -495,22 +508,21 @@ int blockRandUniformTest() printf("Test BlockRandUniform\t\t: "); BlockRandUniform blockRandUniform(NULL, "TEST"); // test initial state - ASSERT(equal(0.0f, blockRandUniform.getDt())); - ASSERT(equal(-1.0f, blockRandUniform.getMin())); - ASSERT(equal(1.0f, blockRandUniform.getMax())); + ASSERT_CL(equal(0.0f, blockRandUniform.getDt())); + ASSERT_CL(equal(-1.0f, blockRandUniform.getMin())); + ASSERT_CL(equal(1.0f, blockRandUniform.getMax())); // test update int n = 10000; float mean = blockRandUniform.update(); - // recursive mean algorithm from Knuth for (int i = 2; i < n + 1; i++) { float val = blockRandUniform.update(); mean += (val - mean) / i; - ASSERT(val <= blockRandUniform.getMax()); - ASSERT(val >= blockRandUniform.getMin()); + ASSERT_CL(less_than_or_equal(val, blockRandUniform.getMax())); + ASSERT_CL(greater_than_or_equal(val, blockRandUniform.getMin())); } - ASSERT(equal(mean, (blockRandUniform.getMin() + + ASSERT_CL(equal(mean, (blockRandUniform.getMin() + blockRandUniform.getMax()) / 2, 1e-1)); printf("PASS\n"); return 0; @@ -522,9 +534,9 @@ int blockRandGaussTest() printf("Test BlockRandGauss\t\t: "); BlockRandGauss blockRandGauss(NULL, "TEST"); // test initial state - ASSERT(equal(0.0f, blockRandGauss.getDt())); - ASSERT(equal(1.0f, blockRandGauss.getMean())); - ASSERT(equal(2.0f, blockRandGauss.getStdDev())); + ASSERT_CL(equal(0.0f, blockRandGauss.getDt())); + ASSERT_CL(equal(1.0f, blockRandGauss.getMean())); + ASSERT_CL(equal(2.0f, blockRandGauss.getStdDev())); // test update int n = 10000; float mean = blockRandGauss.update(); @@ -540,8 +552,8 @@ int blockRandGaussTest() float stdDev = sqrt(sum / (n - 1)); (void)(stdDev); - ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1)); - ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1)); + ASSERT_CL(equal(mean, blockRandGauss.getMean(), 1e-1)); + ASSERT_CL(equal(stdDev, blockRandGauss.getStdDev(), 1e-1)); printf("PASS\n"); return 0; } diff --git a/src/modules/controllib/controllib_test_main.cpp b/src/modules/controllib/controllib_test_main.cpp new file mode 100644 index 0000000000..87950f6ee4 --- /dev/null +++ b/src/modules/controllib/controllib_test_main.cpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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 controllib.cpp + * Unit testing for controllib. + * + * @author James Goppert + */ + +#include "blocks.hpp" + +extern "C" __EXPORT int controllib_test_main(int argc, char *argv[]); + +int controllib_test_main(int argc, char *argv[]) +{ + (void)argc; + (void)argv; + control::basicBlocksTest(); + return 0; +} From bc874ddbe530dbbc2be8859ff247778c2d46ccef Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sun, 25 Oct 2015 11:36:49 -0400 Subject: [PATCH 054/161] Fixe formatting. --- src/modules/controllib/blocks.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index 047fae7b4e..f9e8f71ce0 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -523,7 +523,7 @@ int blockRandUniformTest() } ASSERT_CL(equal(mean, (blockRandUniform.getMin() + - blockRandUniform.getMax()) / 2, 1e-1)); + blockRandUniform.getMax()) / 2, 1e-1)); printf("PASS\n"); return 0; } From 832780b966a090febe0d38010172b061746c1b2a Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Wed, 28 Oct 2015 08:54:38 +0530 Subject: [PATCH 055/161] Fix ranger multi-advertise --- src/drivers/ll40ls/LidarLiteI2C.cpp | 18 ++++++++---------- src/drivers/ll40ls/LidarLitePWM.cpp | 20 +++++++++----------- src/drivers/mb12xx/mb12xx.cpp | 14 ++++++-------- src/drivers/px4flow/px4flow.cpp | 14 ++++++-------- src/drivers/sf0x/sf0x.cpp | 14 ++++++-------- src/drivers/srf02/srf02.cpp | 14 ++++++-------- 6 files changed, 41 insertions(+), 53 deletions(-) diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index 150465ec5b..9896c0e940 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -126,17 +126,15 @@ int LidarLiteI2C::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report; - measure(); - _reports->get(&ds_report); - _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); + /* get a publish handle on the range finder topic */ + struct distance_sensor_s ds_report; + measure(); + _reports->get(&ds_report); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); - if (_distance_sensor_topic == nullptr) { - DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); - } + if (_distance_sensor_topic == nullptr) { + DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); } ret = OK; diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index e2a993ad14..9b1986b603 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -108,19 +108,17 @@ int LidarLitePWM::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the distance_sensor topic */ - struct distance_sensor_s ds_report; - measure(); - _reports->get(&ds_report); - _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); + /* get a publish handle on the distance_sensor topic */ + struct distance_sensor_s ds_report; + measure(); + _reports->get(&ds_report); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); - if (_distance_sensor_topic == nullptr) { - DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); - } + if (_distance_sensor_topic == nullptr) { + DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); } - + return OK; } diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index f3cfa0e267..c1e0734466 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -270,16 +270,14 @@ MB12XX::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report = {}; + /* get a publish handle on the range finder topic */ + struct distance_sensor_s ds_report; - _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); - if (_distance_sensor_topic == nullptr) { - DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); - } + if (_distance_sensor_topic == nullptr) { + DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } // XXX we should find out why we need to wait 200 ms here diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 92cac76312..869e5509ed 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -241,16 +241,14 @@ PX4FLOW::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report = {}; + /* get a publish handle on the range finder topic */ + struct distance_sensor_s ds_report; - _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_HIGH); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_HIGH); - if (_distance_sensor_topic == nullptr) { - DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); - } + if (_distance_sensor_topic == nullptr) { + DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } ret = OK; diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index dbabef34fd..9dd7b1614d 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -296,16 +296,14 @@ SF0X::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report = {}; + /* get a publish handle on the range finder topic */ + struct distance_sensor_s ds_report; - _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_HIGH); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_HIGH); - if (_distance_sensor_topic == nullptr) { - DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); - } + if (_distance_sensor_topic == nullptr) { + DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } } while (0); diff --git a/src/drivers/srf02/srf02.cpp b/src/drivers/srf02/srf02.cpp index 92fda5d41f..8e5c37a88f 100644 --- a/src/drivers/srf02/srf02.cpp +++ b/src/drivers/srf02/srf02.cpp @@ -269,16 +269,14 @@ SRF02::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report = {}; + /* get a publish handle on the range finder topic */ + struct distance_sensor_s ds_report; - _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); - if (_distance_sensor_topic == nullptr) { - DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); - } + if (_distance_sensor_topic == nullptr) { + DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } // XXX we should find out why we need to wait 200 ms here From 83b70688d7cce2e121d25c0268f652a006acfc62 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Wed, 28 Oct 2015 09:16:30 +0530 Subject: [PATCH 056/161] Fix formatting --- src/drivers/ll40ls/LidarLitePWM.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 9b1986b603..02af12b950 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -118,7 +118,7 @@ int LidarLitePWM::init() if (_distance_sensor_topic == nullptr) { DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); } - + return OK; } From 792f1747c770355f3a9888a2d2b8240b26b79dd7 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Wed, 28 Oct 2015 18:05:20 +0530 Subject: [PATCH 057/161] rangefinders : remove annoying gotos --- src/drivers/ll40ls/LidarLiteI2C.cpp | 6 +++--- src/drivers/ll40ls/LidarLitePWM.cpp | 2 +- src/drivers/mb12xx/mb12xx.cpp | 8 ++++---- src/drivers/px4flow/px4flow.cpp | 8 ++++---- src/drivers/sf0x/sf0x.cpp | 2 +- src/drivers/srf02/srf02.cpp | 8 ++++---- 6 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index 9896c0e940..7c3e4f1dbc 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -114,14 +114,14 @@ int LidarLiteI2C::init() /* do I2C init (and probe) first */ if (I2C::init() != OK) { - goto out; + return ret; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s)); if (_reports == nullptr) { - goto out; + return ret; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); @@ -140,7 +140,7 @@ int LidarLiteI2C::init() ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; -out: + return ret; } diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 02af12b950..0157081832 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -109,7 +109,7 @@ int LidarLitePWM::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); /* get a publish handle on the distance_sensor topic */ - struct distance_sensor_s ds_report; + struct distance_sensor_s ds_report = {}; measure(); _reports->get(&ds_report); _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index c1e0734466..39f151f1d6 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -255,7 +255,7 @@ MB12XX::init() /* do I2C init (and probe) first */ if (I2C::init() != OK) { - goto out; + return ret; } /* allocate basic report buffers */ @@ -265,13 +265,13 @@ MB12XX::init() set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ if (_reports == nullptr) { - goto out; + return ret; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report; + struct distance_sensor_s ds_report = {}; _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, &_orb_class_instance, ORB_PRIO_LOW); @@ -319,7 +319,7 @@ MB12XX::init() ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; -out: + return ret; } diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 869e5509ed..18acccc00c 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -229,20 +229,20 @@ PX4FLOW::init() /* do I2C init (and probe) first */ if (I2C::init() != OK) { - goto out; + return ret; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(optical_flow_s)); if (_reports == nullptr) { - goto out; + return ret; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report; + struct distance_sensor_s ds_report = {}; _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, &_orb_class_instance, ORB_PRIO_HIGH); @@ -254,7 +254,7 @@ PX4FLOW::init() ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; -out: + return ret; } diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 9dd7b1614d..41730618b2 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -297,7 +297,7 @@ SF0X::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report; + struct distance_sensor_s ds_report = {}; _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, &_orb_class_instance, ORB_PRIO_HIGH); diff --git a/src/drivers/srf02/srf02.cpp b/src/drivers/srf02/srf02.cpp index 8e5c37a88f..e3f6ad7d08 100644 --- a/src/drivers/srf02/srf02.cpp +++ b/src/drivers/srf02/srf02.cpp @@ -254,7 +254,7 @@ SRF02::init() /* do I2C init (and probe) first */ if (I2C::init() != OK) { - goto out; + return ret; } /* allocate basic report buffers */ @@ -264,13 +264,13 @@ SRF02::init() set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ if (_reports == nullptr) { - goto out; + return ret; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report; + struct distance_sensor_s ds_report = {}; _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, &_orb_class_instance, ORB_PRIO_LOW); @@ -318,7 +318,7 @@ SRF02::init() ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; -out: + return ret; } From 7136f6e88079aa3d42ecb4c9bc96b406ac368926 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Wed, 28 Oct 2015 18:08:03 +0530 Subject: [PATCH 058/161] ll40ls : add missing struct inititalisation --- src/drivers/ll40ls/LidarLiteI2C.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index 7c3e4f1dbc..f85b672c5d 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -127,7 +127,7 @@ int LidarLiteI2C::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report; + struct distance_sensor_s ds_report = {}; measure(); _reports->get(&ds_report); _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, From 47a20f0dd28bae60c69455cefd9751d9e59a224b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Oct 2015 22:55:37 +0100 Subject: [PATCH 059/161] Allow any system to become UAVCAN controlled by setting UAVCAN_ENABLE to 3. --- ROMFS/px4fmu_common/init.d/rcS | 8 ++++++++ src/modules/uavcan/uavcan_params.c | 3 ++- 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 45128dae52..c4b25987be 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -391,6 +391,14 @@ then # set TTYS1_BUSY no + # + # Check if UAVCAN is enabled, default to it for ESCs + # + if param greater UAVCAN_ENABLE 2 + then + set OUTPUT_MODE uavcan_esc + fi + # If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output if [ $OUTPUT_MODE != none ] then diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c index 3a49bd3eb7..5b20f11bf7 100644 --- a/src/modules/uavcan/uavcan_params.c +++ b/src/modules/uavcan/uavcan_params.c @@ -45,9 +45,10 @@ * 0 - UAVCAN disabled. * 1 - Enabled support for UAVCAN actuators and sensors. * 2 - Enabled support for dynamic node ID allocation and firmware update. + * 3 - Sets the motor control outputs to UAVCAN and enables support for dynamic node ID allocation and firmware update. * * @min 0 - * @max 2 + * @max 3 * @group UAVCAN */ PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0); From 6634952a0c95b399d7884b9602886ce0a987561e Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 29 Oct 2015 07:54:38 +0100 Subject: [PATCH 060/161] removed unused vtol variables --- src/modules/vtol_att_control/tiltrotor.cpp | 2 -- src/modules/vtol_att_control/tiltrotor.h | 2 -- src/modules/vtol_att_control/vtol_att_control_main.h | 4 ---- 3 files changed, 8 deletions(-) diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 300cf06e51..fe02db3984 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -47,8 +47,6 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : VtolType(attc), _rear_motors(ENABLED), _tilt_control(0.0f), -_roll_weight_mc(1.0f), -_yaw_weight_mc(1.0f), _min_front_trans_dur(0.5f) { _vtol_schedule.flight_mode = MC_MODE; diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 859731266a..673bf3764f 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -130,8 +130,6 @@ private: }_vtol_schedule; float _tilt_control; /**< actuator value for the tilt servo */ - float _roll_weight_mc; /**< multicopter desired roll moment weight */ - float _yaw_weight_mc; /**< multicopter desired yaw moment weight */ const float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 90d93823fc..a16c213982 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -187,10 +187,6 @@ private: param_t elevons_mc_lock; } _params_handles; - /* for multicopters it is usual to have a non-zero idle speed of the engines - * for fixed wings we want to have an idle speed of zero since we do not want - * to waste energy when gliding. */ - float _airspeed_tot; int _transition_command; VtolType * _vtol_type; // base class for different vtol types From 9ef2994134d994aacde6a21cd9fc701d24cc3739 Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 29 Oct 2015 07:55:47 +0100 Subject: [PATCH 061/161] fix code style of vtol files --- src/modules/vtol_att_control/standard.cpp | 82 ++++--- src/modules/vtol_att_control/standard.h | 22 +- src/modules/vtol_att_control/tailsitter.cpp | 129 +++++----- src/modules/vtol_att_control/tailsitter.h | 14 +- src/modules/vtol_att_control/tiltrotor.cpp | 231 +++++++++++------- src/modules/vtol_att_control/tiltrotor.h | 16 +- .../vtol_att_control_main.cpp | 28 ++- .../vtol_att_control/vtol_att_control_main.h | 42 ++-- src/modules/vtol_att_control/vtol_type.cpp | 18 +- src/modules/vtol_att_control/vtol_type.h | 14 +- 10 files changed, 340 insertions(+), 256 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 3b2784ad64..c87ca630ac 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -60,7 +60,7 @@ Standard::Standard(VtolAttitudeControl *attc) : _params_handles_standard.pusher_trans = param_find("VT_TRANS_THR"); _params_handles_standard.airspeed_blend = param_find("VT_ARSP_BLEND"); _params_handles_standard.airspeed_trans = param_find("VT_ARSP_TRANS"); - } +} Standard::~Standard() { @@ -98,12 +98,12 @@ Standard::parameters_update() void Standard::update_vtol_state() { - parameters_update(); + parameters_update(); /* After flipping the switch the vehicle will start the pusher (or tractor) motor, picking up - * forward speed. After the vehicle has picked up enough speed the rotors shutdown. + * forward speed. After the vehicle has picked up enough speed the rotors shutdown. * For the back transition the pusher motor is immediately stopped and rotors reactivated. - */ + */ if (!_attc->is_fixed_wing_requested()) { // the transition to fw mode switch is off @@ -130,7 +130,7 @@ void Standard::update_vtol_state() } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { // transition to MC mode if transition time has passed if (hrt_elapsed_time(&_vtol_schedule.transition_start) > - (_params_standard.back_trans_dur * 1000000.0f)) { + (_params_standard.back_trans_dur * 1000000.0f)) { _vtol_schedule.flight_mode = MC_MODE; } } @@ -168,17 +168,19 @@ void Standard::update_vtol_state() } // map specific control phases to simple control modes - switch(_vtol_schedule.flight_mode) { - case MC_MODE: - _vtol_mode = ROTARY_WING; - break; - case FW_MODE: - _vtol_mode = FIXED_WING; - break; - case TRANSITION_TO_FW: - case TRANSITION_TO_MC: - _vtol_mode = TRANSITION; - break; + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + _vtol_mode = ROTARY_WING; + break; + + case FW_MODE: + _vtol_mode = FIXED_WING; + break; + + case TRANSITION_TO_FW: + case TRANSITION_TO_MC: + _vtol_mode = TRANSITION; + break; } } @@ -188,18 +190,21 @@ void Standard::update_transition_state() if (_params_standard.front_trans_dur <= 0.0f) { // just set the final target throttle value _pusher_throttle = _params_standard.pusher_trans; + } else if (_pusher_throttle <= _params_standard.pusher_trans) { // ramp up throttle to the target throttle value - _pusher_throttle = _params_standard.pusher_trans * - (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f); + _pusher_throttle = _params_standard.pusher_trans * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f); } // do blending of mc and fw controls if a blending airspeed has been provided if (_airspeed_trans_blend_margin > 0.0f && _airspeed->true_airspeed_m_s >= _params_standard.airspeed_blend) { - float weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin; + float weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / + _airspeed_trans_blend_margin; _mc_roll_weight = weight; _mc_pitch_weight = weight; _mc_yaw_weight = weight; + } else { // at low speeds give full weight to mc _mc_roll_weight = 1.0f; @@ -210,17 +215,19 @@ void Standard::update_transition_state() } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { // continually increase mc attitude control as we transition back to mc mode if (_params_standard.back_trans_dur > 0.0f) { - float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f); + float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * + 1000000.0f); _mc_roll_weight = weight; _mc_pitch_weight = weight; _mc_yaw_weight = weight; + } else { _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; } - // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again + // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again if (_flag_enable_mc_motors) { set_max_mc(2000); set_idle_mc(); @@ -238,15 +245,15 @@ void Standard::update_mc_state() // do nothing } - void Standard::update_fw_state() +void Standard::update_fw_state() { - // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again + // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again if (!_flag_enable_mc_motors) { set_max_mc(950); set_idle_fw(); // force them to stop, not just idle _flag_enable_mc_motors = true; } - } +} void Standard::update_external_state() { @@ -259,22 +266,31 @@ void Standard::update_external_state() void Standard::fill_actuator_outputs() { /* multirotor controls */ - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; // roll - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; // pitch - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; // yaw - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] + * _mc_roll_weight; // roll + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; // pitch + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * + _mc_yaw_weight; // yaw + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle /* fixed wing controls */ - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); //roll - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight); //pitch - _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); // yaw + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] + * (1 - _mc_roll_weight); //roll + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight); //pitch + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] + * (1 - _mc_yaw_weight); // yaw // set the fixed wing throttle control if (_vtol_schedule.flight_mode == FW_MODE) { // take the throttle value commanded by the fw controller - _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + } else { - // otherwise we may be ramping up the throttle during the transition to fw mode + // otherwise we may be ramping up the throttle during the transition to fw mode _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle; } } diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index 7bce82071a..ed37ccc398 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -31,15 +31,15 @@ * ****************************************************************************/ - /** - * @file standard.h - * VTOL with fixed multirotor motor configurations (such as quad) and a pusher - * (or puller aka tractor) motor for forward flight. - * - * @author Simon Wilks - * @author Roman Bapst - * - */ +/** +* @file standard.h +* VTOL with fixed multirotor motor configurations (such as quad) and a pusher +* (or puller aka tractor) motor for forward flight. +* +* @author Simon Wilks +* @author Roman Bapst +* +*/ #ifndef STANDARD_H #define STANDARD_H @@ -52,7 +52,7 @@ class Standard : public VtolType public: - Standard(VtolAttitudeControl * _att_controller); + Standard(VtolAttitudeControl *_att_controller); ~Standard(); void update_vtol_state(); @@ -89,7 +89,7 @@ private: struct { vtol_mode flight_mode; // indicates in which mode the vehicle is in hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition) - }_vtol_schedule; + } _vtol_schedule; bool _flag_enable_mc_motors; float _pusher_throttle; diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index e423fbceb3..7368870847 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -31,21 +31,21 @@ * ****************************************************************************/ - /** - * @file tailsitter.cpp - * - * @author Roman Bapst - * - */ +/** +* @file tailsitter.cpp +* +* @author Roman Bapst +* +*/ - #include "tailsitter.h" - #include "vtol_att_control_main.h" +#include "tailsitter.h" +#include "vtol_att_control_main.h" -Tailsitter::Tailsitter (VtolAttitudeControl *att_controller) : -VtolType(att_controller), -_airspeed_tot(0), -_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), -_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) +Tailsitter::Tailsitter(VtolAttitudeControl *att_controller) : + VtolType(att_controller), + _airspeed_tot(0), + _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) { } @@ -60,6 +60,7 @@ void Tailsitter::update_vtol_state() // simply switch between the two modes if (!_attc->is_fixed_wing_requested()) { _vtol_mode = ROTARY_WING; + } else { _vtol_mode = FIXED_WING; } @@ -67,7 +68,7 @@ void Tailsitter::update_vtol_state() void Tailsitter::update_mc_state() { - if (!flag_idle_mc) { + if (!flag_idle_mc) { set_idle_mc(); flag_idle_mc = true; } @@ -91,18 +92,18 @@ void Tailsitter::update_external_state() } - void Tailsitter::calc_tot_airspeed() - { +void Tailsitter::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); + 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 + 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; + float v_ind = (airspeed / eta - airspeed) * 2.0f; // calculate total airspeed float airspeed_raw = airspeed + v_ind; // apply low-pass filter @@ -115,16 +116,19 @@ Tailsitter::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 = !PX4_ISFINITE(_airspeed->true_airspeed_m_s) || - hrt_elapsed_time(&_airspeed->timestamp) > 1e6) { + hrt_elapsed_time(&_airspeed->timestamp) > 1e6) { airspeed = _params->mc_airspeed_trim; + if (nonfinite) { perf_count(_nonfinite_input_perf); } + } else { airspeed = _airspeed_tot; - airspeed = math::constrain(airspeed,_params->mc_airspeed_min, _params->mc_airspeed_max); + airspeed = math::constrain(airspeed, _params->mc_airspeed_min, _params->mc_airspeed_max); } _vtol_vehicle_status->airspeed_tot = airspeed; // save value for logging @@ -135,8 +139,10 @@ Tailsitter::scale_mc_output() * * Forcing the scaling to this value allows reasonable handheld tests. */ - float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min : airspeed); - _actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f); + float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min : + airspeed); + _actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1] * airspeed_scaling * airspeed_scaling, + -1.0f, 1.0f); } /** @@ -144,37 +150,50 @@ Tailsitter::scale_mc_output() */ void Tailsitter::fill_actuator_outputs() { - switch(_vtol_mode) { - case ROTARY_WING: - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]; - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + switch (_vtol_mode) { + case ROTARY_WING: + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; - if (_params->elevons_mc_lock == 1) { - _actuators_out_1->control[0] = 0; - _actuators_out_1->control[1] = 0; - } else { - // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon - } - break; - case FIXED_WING: - // in fixed wing mode we use engines only for providing thrust, no moments are generated - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0; - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0; - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + if (_params->elevons_mc_lock == 1) { + _actuators_out_1->control[0] = 0; + _actuators_out_1->control[1] = 0; - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon - _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw - _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle - break; - case TRANSITION: - case EXTERNAL: - // not yet implemented, we are switching brute force at the moment - break; + } else { + // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = + _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon + } + + break; + + case FIXED_WING: + // in fixed wing mode we use engines only for providing thrust, no moments are generated + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = + -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = + _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle + break; + + case TRANSITION: + case EXTERNAL: + // not yet implemented, we are switching brute force at the moment + break; } } diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index 388111ace8..ec8d007567 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -31,12 +31,12 @@ * ****************************************************************************/ - /** - * @file tiltrotor.h - * - * @author Roman Bapst - * - */ +/** +* @file tiltrotor.h +* +* @author Roman Bapst +* +*/ #ifndef TAILSITTER_H #define TAILSITTER_H @@ -48,7 +48,7 @@ class Tailsitter : public VtolType { public: - Tailsitter(VtolAttitudeControl * _att_controller); + Tailsitter(VtolAttitudeControl *_att_controller); ~Tailsitter(); void update_vtol_state(); diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index fe02db3984..9e170104c4 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -44,10 +44,10 @@ #define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : -VtolType(attc), -_rear_motors(ENABLED), -_tilt_control(0.0f), -_min_front_trans_dur(0.5f) + VtolType(attc), + _rear_motors(ENABLED), + _tilt_control(0.0f), + _min_front_trans_dur(0.5f) { _vtol_schedule.flight_mode = MC_MODE; _vtol_schedule.transition_start = 0; @@ -86,11 +86,11 @@ Tiltrotor::parameters_update() /* vtol duration of a front transition */ param_get(_params_handles_tiltrotor.front_trans_dur, &v); - _params_tiltrotor.front_trans_dur = math::constrain(v,1.0f,5.0f); + _params_tiltrotor.front_trans_dur = math::constrain(v, 1.0f, 5.0f); /* vtol duration of a back transition */ param_get(_params_handles_tiltrotor.back_trans_dur, &v); - _params_tiltrotor.back_trans_dur = math::constrain(v,0.0f,5.0f); + _params_tiltrotor.back_trans_dur = math::constrain(v, 0.0f, 5.0f); /* vtol tilt mechanism position in mc mode */ param_get(_params_handles_tiltrotor.tilt_mc, &v); @@ -123,22 +123,26 @@ Tiltrotor::parameters_update() /* avoid parameters which will lead to zero division in the transition code */ _params_tiltrotor.front_trans_dur = math::max(_params_tiltrotor.front_trans_dur, _min_front_trans_dur); - if ( _params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f ) { + if (_params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f) { _params_tiltrotor.airspeed_trans = _params_tiltrotor.airspeed_blend_start + 1.0f; } return OK; } -int Tiltrotor::get_motor_off_channels(int channels) { +int Tiltrotor::get_motor_off_channels(int channels) +{ int channel_bitmap = 0; - + int channel; + for (int i = 0; i < _params->vtol_motor_count; ++i) { channel = channels % 10; + if (channel == 0) { break; } + channel_bitmap |= 1 << channel; channels = channels / 10; } @@ -148,82 +152,98 @@ int Tiltrotor::get_motor_off_channels(int channels) { void Tiltrotor::update_vtol_state() { - parameters_update(); + parameters_update(); - /* simple logic using a two way switch to perform transitions. + /* simple logic using a two way switch to perform transitions. * after flipping the switch the vehicle will start tilting rotors, picking up * forward speed. After the vehicle has picked up enough speed the rotors are tilted * forward completely. For the backtransition the motors simply rotate back. - */ + */ if (!_attc->is_fixed_wing_requested()) { // plane is in multicopter mode - switch(_vtol_schedule.flight_mode) { - case MC_MODE: - break; - case FW_MODE: - _vtol_schedule.flight_mode = TRANSITION_BACK; - _vtol_schedule.transition_start = hrt_absolute_time(); - break; - case TRANSITION_FRONT_P1: - // failsafe into multicopter mode + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + break; + + case FW_MODE: + _vtol_schedule.flight_mode = TRANSITION_BACK; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + + case TRANSITION_FRONT_P1: + // failsafe into multicopter mode + _vtol_schedule.flight_mode = MC_MODE; + break; + + case TRANSITION_FRONT_P2: + // failsafe into multicopter mode + _vtol_schedule.flight_mode = MC_MODE; + break; + + case TRANSITION_BACK: + if (_tilt_control <= _params_tiltrotor.tilt_mc) { _vtol_schedule.flight_mode = MC_MODE; - break; - case TRANSITION_FRONT_P2: - // failsafe into multicopter mode - _vtol_schedule.flight_mode = MC_MODE; - break; - case TRANSITION_BACK: - if (_tilt_control <= _params_tiltrotor.tilt_mc) { - _vtol_schedule.flight_mode = MC_MODE; - } - break; + } + + break; } + } else { - switch(_vtol_schedule.flight_mode) { - case MC_MODE: - // initialise a front transition - _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + // initialise a front transition + _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + + case FW_MODE: + break; + + case TRANSITION_FRONT_P1: + + // check if we have reached airspeed to switch to fw mode + if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) { + _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; _vtol_schedule.transition_start = hrt_absolute_time(); - break; - case FW_MODE: - break; - case TRANSITION_FRONT_P1: - // check if we have reached airspeed to switch to fw mode - if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) { - _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; - _vtol_schedule.transition_start = hrt_absolute_time(); - } - break; - case TRANSITION_FRONT_P2: - // if the rotors have been tilted completely we switch to fw mode - if (_tilt_control >= _params_tiltrotor.tilt_fw) { - _vtol_schedule.flight_mode = FW_MODE; - _tilt_control = _params_tiltrotor.tilt_fw; - } - break; - case TRANSITION_BACK: - // failsafe into fixed wing mode + } + + break; + + case TRANSITION_FRONT_P2: + + // if the rotors have been tilted completely we switch to fw mode + if (_tilt_control >= _params_tiltrotor.tilt_fw) { _vtol_schedule.flight_mode = FW_MODE; - break; + _tilt_control = _params_tiltrotor.tilt_fw; + } + + break; + + case TRANSITION_BACK: + // failsafe into fixed wing mode + _vtol_schedule.flight_mode = FW_MODE; + break; } } // map tiltrotor specific control phases to simple control modes - switch(_vtol_schedule.flight_mode) { - case MC_MODE: - _vtol_mode = ROTARY_WING; - break; - case FW_MODE: - _vtol_mode = FIXED_WING; - break; - case TRANSITION_FRONT_P1: - case TRANSITION_FRONT_P2: - case TRANSITION_BACK: - _vtol_mode = TRANSITION; - break; + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + _vtol_mode = ROTARY_WING; + break; + + case FW_MODE: + _vtol_mode = FIXED_WING; + break; + + case TRANSITION_FRONT_P1: + case TRANSITION_FRONT_P2: + case TRANSITION_BACK: + _vtol_mode = TRANSITION; + break; } } @@ -248,7 +268,7 @@ void Tiltrotor::update_mc_state() _mc_yaw_weight = 1.0f; } - void Tiltrotor::update_fw_state() +void Tiltrotor::update_fw_state() { // make sure motors are tilted forward _tilt_control = _params_tiltrotor.tilt_fw; @@ -276,15 +296,18 @@ void Tiltrotor::update_transition_state() if (_rear_motors != ENABLED) { set_rear_motor_state(ENABLED); } + // tilt rotors forward up to certain angle - if (_tilt_control <= _params_tiltrotor.tilt_transition ) { + if (_tilt_control <= _params_tiltrotor.tilt_transition) { _tilt_control = _params_tiltrotor.tilt_mc + - fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur * 1000000.0f); + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time( + &_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur * 1000000.0f); } // do blending of mc and fw controls if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_blend_start) { _mc_roll_weight = 0.0f; + } else { // at low speeds give full weight to mc _mc_roll_weight = 1.0f; @@ -292,6 +315,7 @@ void Tiltrotor::update_transition_state() // disable mc yaw control once the plane has picked up speed _mc_yaw_weight = 1.0f; + if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { _mc_yaw_weight = 0.0f; } @@ -299,8 +323,10 @@ void Tiltrotor::update_transition_state() } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { // the plane is ready to go into fixed wing mode, tilt the rotors forward completely _tilt_control = _params_tiltrotor.tilt_transition + - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur_p2 * 1000000.0f); + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * (float)hrt_elapsed_time( + &_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur_p2 * 1000000.0f); _mc_roll_weight = 0.0f; + } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { if (_rear_motors != IDLE) { set_rear_motor_state(IDLE); @@ -310,10 +336,12 @@ void Tiltrotor::update_transition_state() set_idle_mc(); flag_idle_mc = true; } + // tilt rotors back if (_tilt_control > _params_tiltrotor.tilt_mc) { _tilt_control = _params_tiltrotor.tilt_fw - - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f); + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time( + &_vtol_schedule.transition_start) / (_params_tiltrotor.back_trans_dur * 1000000.0f); } // set zero throttle for backtransition otherwise unwanted moments will be created @@ -337,19 +365,28 @@ void Tiltrotor::update_external_state() */ void Tiltrotor::fill_actuator_outputs() { - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] + * _mc_roll_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * + _mc_yaw_weight; if (_vtol_schedule.flight_mode == FW_MODE) { - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + } else { - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];; } - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); - _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); // yaw + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] + * (1 - _mc_roll_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] + * (1 - _mc_yaw_weight); // yaw _actuators_out_1->control[4] = _tilt_control; } @@ -358,23 +395,26 @@ void Tiltrotor::fill_actuator_outputs() * Set state of rear motors. */ -void Tiltrotor::set_rear_motor_state(rear_motor_state state) { +void Tiltrotor::set_rear_motor_state(rear_motor_state state) +{ int pwm_value = PWM_DEFAULT_MAX; // map desired rear rotor state to max allowed pwm signal switch (state) { - case ENABLED: - pwm_value = PWM_DEFAULT_MAX; - _rear_motors = ENABLED; - break; - case DISABLED: - pwm_value = PWM_LOWEST_MAX; - _rear_motors = DISABLED; - break; - case IDLE: - pwm_value = _params->idle_pwm_mc; - _rear_motors = IDLE; - break; + case ENABLED: + pwm_value = PWM_DEFAULT_MAX; + _rear_motors = ENABLED; + break; + + case DISABLED: + pwm_value = PWM_LOWEST_MAX; + _rear_motors = DISABLED; + break; + + case IDLE: + pwm_value = _params->idle_pwm_mc; + _rear_motors = IDLE; + break; } int ret; @@ -391,9 +431,11 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state) { for (int i = 0; i < _params->vtol_motor_count; i++) { if (is_motor_off_channel(i)) { pwm_values.values[i] = pwm_value; + } else { pwm_values.values[i] = PWM_DEFAULT_MAX; } + pwm_values.channel_count = _params->vtol_motor_count; } @@ -404,6 +446,7 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state) { px4_close(fd); } -bool Tiltrotor::is_motor_off_channel(const int channel) { +bool Tiltrotor::is_motor_off_channel(const int channel) +{ return (_params_tiltrotor.fw_motors_off >> channel) & 1; } diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 673bf3764f..507920cb42 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -31,12 +31,12 @@ * ****************************************************************************/ - /** - * @file tiltrotor.h - * - * @author Roman Bapst - * - */ +/** +* @file tiltrotor.h +* +* @author Roman Bapst +* +*/ #ifndef TILTROTOR_H #define TILTROTOR_H @@ -49,7 +49,7 @@ class Tiltrotor : public VtolType public: - Tiltrotor(VtolAttitudeControl * _att_controller); + Tiltrotor(VtolAttitudeControl *_att_controller); ~Tiltrotor(); /** @@ -127,7 +127,7 @@ private: struct { vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */ hrt_abstime transition_start; /**< absoulte time at which front transition started */ - }_vtol_schedule; + } _vtol_schedule; float _tilt_control; /**< actuator value for the tilt servo */ 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 e21279cb75..e15e950fd1 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -93,10 +93,10 @@ 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)); - memset(&_vehicle_cmd,0, sizeof(_vehicle_cmd)); + memset(&_local_pos, 0, sizeof(_local_pos)); + memset(&_airspeed, 0, sizeof(_airspeed)); + memset(&_batt_status, 0, sizeof(_batt_status)); + memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd)); _params.idle_pwm_mc = PWM_LOWEST_MIN; _params.vtol_motor_count = 0; @@ -121,12 +121,15 @@ VtolAttitudeControl::VtolAttitudeControl() : if (_params.vtol_type == 0) { _tailsitter = new Tailsitter(this); _vtol_type = _tailsitter; + } else if (_params.vtol_type == 1) { _tiltrotor = new Tiltrotor(this); _vtol_type = _tiltrotor; + } else if (_params.vtol_type == 2) { _standard = new Standard(this); _vtol_type = _standard; + } else { _task_should_exit = true; } @@ -258,7 +261,8 @@ void VtolAttitudeControl::vehicle_rates_sp_fw_poll() * Check for airspeed updates. */ void -VtolAttitudeControl::vehicle_airspeed_poll() { +VtolAttitudeControl::vehicle_airspeed_poll() +{ bool updated; orb_check(_airspeed_sub, &updated); @@ -271,7 +275,8 @@ VtolAttitudeControl::vehicle_airspeed_poll() { * Check for battery updates. */ void -VtolAttitudeControl::vehicle_battery_poll() { +VtolAttitudeControl::vehicle_battery_poll() +{ bool updated; orb_check(_battery_status_sub, &updated); @@ -643,11 +648,11 @@ VtolAttitudeControl::start() /* start the task */ _control_task = px4_task_spawn_cmd("vtol_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 10, - 2048, - (px4_main_t)&VtolAttitudeControl::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 2048, + (px4_main_t)&VtolAttitudeControl::task_main_trampoline, + nullptr); if (_control_task < 0) { PX4_WARN("task start failed"); @@ -705,6 +710,7 @@ int vtol_att_control_main(int argc, char *argv[]) } else { PX4_WARN("not running"); } + return 0; } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index a16c213982..c10f7b0dd7 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -104,24 +104,24 @@ public: int start(); /* start the task and return OK on success */ bool is_fixed_wing_requested(); - struct vehicle_attitude_s* get_att () {return &_v_att;} - struct vehicle_attitude_setpoint_s* get_att_sp () {return &_v_att_sp;} - struct vehicle_rates_setpoint_s* get_rates_sp () {return &_v_rates_sp;} - struct vehicle_rates_setpoint_s* get_mc_virtual_rates_sp () {return &_mc_virtual_v_rates_sp;} - struct vehicle_rates_setpoint_s* get_fw_virtual_rates_sp () {return &_fw_virtual_v_rates_sp;} - struct manual_control_setpoint_s* get_manual_control_sp () {return &_manual_control_sp;} - struct vehicle_control_mode_s* get_control_mode () {return &_v_control_mode;} - struct vtol_vehicle_status_s* get_vehicle_status () {return &_vtol_vehicle_status;} - struct actuator_controls_s* get_actuators_out0 () {return &_actuators_out_0;} - struct actuator_controls_s* get_actuators_out1 () {return &_actuators_out_1;} - struct actuator_controls_s* get_actuators_mc_in () {return &_actuators_mc_in;} - struct actuator_controls_s* get_actuators_fw_in () {return &_actuators_fw_in;} - struct actuator_armed_s* get_armed () {return &_armed;} - struct vehicle_local_position_s* get_local_pos () {return &_local_pos;} - struct airspeed_s* get_airspeed () {return &_airspeed;} - struct battery_status_s* get_batt_status () {return &_batt_status;} + struct vehicle_attitude_s *get_att() {return &_v_att;} + struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;} + struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;} + struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;} + struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;} + struct manual_control_setpoint_s *get_manual_control_sp() {return &_manual_control_sp;} + struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;} + struct vtol_vehicle_status_s *get_vehicle_status() {return &_vtol_vehicle_status;} + struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;} + struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;} + struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;} + struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;} + struct actuator_armed_s *get_armed() {return &_armed;} + struct vehicle_local_position_s *get_local_pos() {return &_local_pos;} + struct airspeed_s *get_airspeed() {return &_airspeed;} + struct battery_status_s *get_batt_status() {return &_batt_status;} - struct Params* get_params () {return &_params;} + struct Params *get_params() {return &_params;} private: @@ -189,10 +189,10 @@ private: int _transition_command; - VtolType * _vtol_type; // base class for different vtol types - Tiltrotor * _tiltrotor; // tailsitter vtol type - Tailsitter * _tailsitter; // tiltrotor vtol type - Standard * _standard; // standard vtol type + VtolType *_vtol_type; // base class for different vtol types + Tiltrotor *_tiltrotor; // tailsitter vtol type + Tailsitter *_tailsitter; // tiltrotor vtol type + Standard *_standard; // standard vtol type //*****************Member functions*********************************************************************** diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 83837b8596..f57080a2a7 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -31,12 +31,12 @@ * ****************************************************************************/ - /** - * @file airframe.cpp - * - * @author Roman Bapst - * - */ +/** +* @file airframe.cpp +* +* @author Roman Bapst +* +*/ #include "vtol_type.h" #include "drivers/drv_pwm_output.h" @@ -44,8 +44,8 @@ #include "vtol_att_control_main.h" VtolType::VtolType(VtolAttitudeControl *att_controller) : -_attc(att_controller), -_vtol_mode(ROTARY_WING) + _attc(att_controller), + _vtol_mode(ROTARY_WING) { _v_att = _attc->get_att(); _v_att_sp = _attc->get_att_sp(); @@ -70,7 +70,7 @@ _vtol_mode(ROTARY_WING) VtolType::~VtolType() { - + } /** diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index a797201e01..d8557110c0 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -31,12 +31,12 @@ * ****************************************************************************/ - /** - * @file airframe.h - * - * @author Roman Bapst - * - */ +/** +* @file airframe.h +* +* @author Roman Bapst +* +*/ #ifndef VTOL_TYPE_H #define VTOL_TYPE_H @@ -85,7 +85,7 @@ public: void set_idle_mc(); void set_idle_fw(); - mode get_mode () {return _vtol_mode;}; + mode get_mode() {return _vtol_mode;}; protected: VtolAttitudeControl *_attc; From 89028901e651f387eafb61796d6dafc1d7e587c3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 27 Oct 2015 12:24:15 +0100 Subject: [PATCH 062/161] support multiple actuator control groups --- src/drivers/pwm_out_sim/pwm_out_sim.cpp | 255 ++++++++++++++++-------- 1 file changed, 168 insertions(+), 87 deletions(-) diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp index 8040cc4d95..993958a050 100644 --- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp +++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp @@ -79,6 +79,8 @@ #include #include #include +#include +#include #include #include @@ -94,6 +96,7 @@ public: enum Mode { MODE_2PWM, MODE_4PWM, + MODE_6PWM, MODE_8PWM, MODE_12PWM, MODE_16PWM, @@ -111,23 +114,29 @@ public: int _task; private: - static const unsigned _max_actuators = 4; + static const unsigned _max_actuators = 8; Mode _mode; int _update_rate; int _current_update_rate; - int _t_actuators; - int _t_armed; - orb_advert_t _t_outputs; + int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + unsigned _poll_fds_num; + int _armed_sub; + orb_advert_t _outputs_pub; unsigned _num_outputs; bool _primary_pwm_device; + uint32_t _groups_required; + uint32_t _groups_subscribed; + volatile bool _task_should_exit; static bool _armed; MixerGroup *_mixers; - actuator_controls_s _controls; + actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; static void task_main_trampoline(int argc, char *argv[]); void task_main(); @@ -138,6 +147,7 @@ private: float &input); int pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg); + void subscribe(); struct GPIOConfig { uint32_t input; @@ -176,15 +186,24 @@ PWMSim::PWMSim() : _mode(MODE_NONE), _update_rate(50), _current_update_rate(0), - _t_actuators(-1), - _t_armed(-1), - _t_outputs(0), + _control_subs { -1}, + _poll_fds_num(0), + _armed_sub(-1), + _outputs_pub(0), _num_outputs(0), _primary_pwm_device(false), + _groups_required(0), + _groups_subscribed(0), _task_should_exit(false), _mixers(nullptr) { _debug_enabled = true; + memset(_controls, 0, sizeof(_controls)); + + _control_topics[0] = ORB_ID(actuator_controls_0); + _control_topics[1] = ORB_ID(actuator_controls_1); + _control_topics[2] = ORB_ID(actuator_controls_2); + _control_topics[3] = ORB_ID(actuator_controls_3); } PWMSim::~PWMSim() @@ -326,64 +345,62 @@ PWMSim::set_pwm_rate(unsigned rate) return OK; } +void +PWMSim::subscribe() +{ + /* subscribe/unsubscribe to required actuator control groups */ + uint32_t sub_groups = _groups_required & ~_groups_subscribed; + uint32_t unsub_groups = _groups_subscribed & ~_groups_required; + _poll_fds_num = 0; + + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (sub_groups & (1 << i)) { + PX4_DEBUG("subscribe to actuator_controls_%d", i); + _control_subs[i] = orb_subscribe(_control_topics[i]); + } + + if (unsub_groups & (1 << i)) { + PX4_DEBUG("unsubscribe from actuator_controls_%d", i); + px4_close(_control_subs[i]); + _control_subs[i] = -1; + } + + if (_control_subs[i] > 0) { + printf("valid\n"); + _poll_fds[_poll_fds_num].fd = _control_subs[i]; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num++; + } + } +} + void PWMSim::task_main() { - /* - * Subscribe to the appropriate PWM output topic based on whether we are the - * primary PWM output or not. - */ - _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1)); /* force a reset of the update rate */ _current_update_rate = 0; - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_armed_sub, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); /* advertise the mixed control outputs, insist on the first group output */ - _t_outputs = orb_advertise(ORB_ID(actuator_outputs), &outputs); + _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &outputs); - px4_pollfd_struct_t fds[2]; - fds[0].fd = _t_actuators; - fds[0].events = POLLIN; - fds[1].fd = _t_armed; - fds[1].events = POLLIN; - - unsigned num_outputs; - - /* select the number of virtual outputs */ - switch (_mode) { - case MODE_2PWM: - num_outputs = 2; - break; - - case MODE_4PWM: - num_outputs = 4; - break; - - case MODE_8PWM: - case MODE_12PWM: - case MODE_16PWM: - // XXX only support the lower 8 - trivial to extend - num_outputs = 8; - break; - - case MODE_NONE: - default: - num_outputs = 0; - break; - } DEVICE_LOG("starting"); /* loop until killed */ while (!_task_should_exit) { + if (_groups_subscribed != _groups_required) { + subscribe(); + _groups_subscribed = _groups_required; + } + /* handle update rate changes */ if (_current_update_rate != _update_rate) { int update_rate_in_ms = int(1000 / _update_rate); @@ -392,13 +409,18 @@ PWMSim::task_main() update_rate_in_ms = 2; } - orb_set_interval(_t_actuators, update_rate_in_ms); + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + orb_set_interval(_control_subs[i], update_rate_in_ms); + } + } + // up_pwm_servo_set_rate(_update_rate); _current_update_rate = _update_rate; } /* sleep waiting for data, but no more than a second */ - int ret = px4_poll(&fds[0], 2, 1000); + int ret = px4_poll(&_poll_fds[0], _poll_fds_num, 1000); /* this would be bad... */ if (ret < 0) { @@ -406,56 +428,107 @@ PWMSim::task_main() continue; } - /* do we have a control update? */ - if (fds[0].revents & POLLIN) { - /* get controls - must always do this to avoid spinning */ - orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1), _t_actuators, &_controls); + if (ret == 0) { + // timeout + continue; + } - /* can we mix? */ - if (_armed && _mixers != nullptr) { - /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); - outputs.timestamp = hrt_absolute_time(); + /* get controls for required topics */ + unsigned poll_id = 0; - /* iterate actuators */ - for (unsigned i = 0; i < num_outputs; i++) { - /* last resort: catch NaN, INF and out-of-band errors */ - if (i < outputs.noutputs && - PX4_ISFINITE(outputs.output[i]) && - outputs.output[i] >= -1.0f && - outputs.output[i] <= 1.0f) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); - - } else { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = 900; - } + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + if (_poll_fds[poll_id].revents & POLLIN) { + orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } - /* and publish for anyone that cares to see */ - orb_publish(ORB_ID(actuator_outputs), _t_outputs, &outputs); + poll_id++; } } - /* how about an arming update? */ - if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + /* can we mix? */ + if (_armed && _mixers != nullptr) { - /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + size_t num_outputs; + + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; + + case MODE_4PWM: + num_outputs = 4; + break; + + case MODE_6PWM: + num_outputs = 6; + break; + + case MODE_8PWM: + num_outputs = 8; + break; + + default: + num_outputs = 0; + break; + } + + /* do mixing */ + actuator_outputs_s outputs; + num_outputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); + outputs.timestamp = hrt_absolute_time(); + + /* disable unused ports by setting their output to NaN */ + for (size_t i = 0; i < sizeof(outputs.output) / sizeof(outputs.output[0]); i++) { + if (i >= num_outputs) { + outputs.output[i] = NAN; + } + } + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < outputs.noutputs && + PX4_ISFINITE(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = 900; + } + } + + + /* and publish for anyone that cares to see */ + orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs); + } + + /* how about an arming update? */ + bool updated; + actuator_armed_s aa; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &aa); /* do not obey the lockdown value, as lockdown is for PWMSim */ _armed = aa.armed; } } - px4_close(_t_actuators); - px4_close(_t_armed); + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + px4_close(_control_subs[i]); + } + } + + px4_close(_armed_sub); /* make sure servos are off */ // up_pwm_servo_deinit(); @@ -671,6 +744,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) if (_mixers != nullptr) { delete _mixers; _mixers = nullptr; + _groups_required = 0; } break; @@ -683,6 +757,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) if (mixer->check()) { delete mixer; + _groups_required = 0; ret = -EINVAL; } else { @@ -691,6 +766,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) (uintptr_t)&_controls); _mixers->add_mixer(mixer); + _mixers->groups_required(_groups_required); } break; @@ -705,6 +781,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) } if (_mixers == nullptr) { + _groups_required = 0; ret = -ENOMEM; } else { @@ -715,7 +792,11 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) DEVICE_DEBUG("mixer load failed with %d", ret); delete _mixers; _mixers = nullptr; + _groups_required = 0; ret = -EINVAL; + + } else { + _mixers->groups_required(_groups_required); } } @@ -772,7 +853,7 @@ hil_new_mode(PortMode new_mode) case PORT1_FULL_PWM: /* select 4-pin PWM mode */ - servo_mode = PWMSim::MODE_4PWM; + servo_mode = PWMSim::MODE_8PWM; break; case PORT1_PWM_AND_SERIAL: From a394dd5b0d9b0a65b4843e72a4d5e352b59734ef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 Oct 2015 10:35:35 +0100 Subject: [PATCH 063/161] Add gazebo, gazebo_iris and gazebo_vtol targets --- Makefile | 2 +- Tools/sitl_run.sh | 23 ++++++++++++------ src/firmware/posix/CMakeLists.txt | 40 +++++++++++++++++++------------ 3 files changed, 42 insertions(+), 23 deletions(-) diff --git a/Makefile b/Makefile index 4246944ae2..096a058653 100644 --- a/Makefile +++ b/Makefile @@ -184,7 +184,7 @@ clean: # targets handled by cmake cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \ run_cmake_config config gazebo gazebo_gdb gazebo_lldb jmavsim \ - jmavsim_gdb jmavsim_lldb + jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_vtol gazebo_iris gazebo_vtol $(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ)))) .PHONY: clean diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index e437f7651f..7f7808c9e6 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -3,18 +3,27 @@ rc_script=$1 debugger=$2 program=$3 -build_path=$4 +model=$4 +build_path=$5 curr_dir=`pwd` echo SITL ARGS echo rc_script: $rc_script echo debugger: $debugger echo program: $program +echo model: $model echo build_path: $build_path -if [ "$#" != 4 ] +if [ "$model" == "" ] || [ "$model" == "none" ] then - echo usage: sitl_run.sh rc_script debugger program build_path + echo "empty model, setting iris as default" + model="iris" +fi + +if [ "$#" != 5 ] +then + echo usage: sitl_run.sh rc_script debugger program model build_path + echo "" exit 1 fi @@ -54,9 +63,9 @@ then cd Tools/sitl_gazebo/Build cmake .. make -j4 - gzserver ../worlds/iris.world & + gzserver ../worlds/${model}.world & SIM_PID=`echo $!` - gzclient& + gzclient & GUI_PID=`echo $!` else echo "You need to have gazebo simulator installed!" @@ -78,10 +87,10 @@ else ./mainapp ../../../../${rc_script}_${program} fi -if [ "$3" == "jmavsim" ] +if [ "$program" == "jmavsim" ] then kill -9 $SIM_PID -elif [ "$3" == "gazebo" ] +elif [ "$program" == "gazebo" ] then kill -9 $SIM_PID kill -9 $GUI_PID diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index 3417f49d18..64f4775c33 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -26,27 +26,37 @@ endif() add_custom_target(run_config COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" "${config_sitl_debugger}" - "${config_sitl_viewer}" "${CMAKE_BINARY_DIR}" + "${config_sitl_viewer}" "${config_sitl_model}" "${CMAKE_BINARY_DIR}" WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} USES_TERMINAL ) add_dependencies(run_config mainapp) -foreach(viewer jmavsim gazebo) +foreach(viewer none jmavsim gazebo) foreach(debugger none gdb lldb) - if (debugger STREQUAL "none") - set(_targ_name "${viewer}") - else() - set(_targ_name "${viewer}_${debugger}") - endif() - add_custom_target(${_targ_name} - COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" - "${debugger}" - "${viewer}" "${CMAKE_BINARY_DIR}" - WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} - USES_TERMINAL - ) - add_dependencies(${_targ_name} mainapp) + foreach(model none iris vtol) + if (debugger STREQUAL "none") + if (model STREQUAL "none") + set(_targ_name "${viewer}") + else() + set(_targ_name "${viewer}_${model}") + endif() + else() + if (model STREQUAL "none") + set(_targ_name "${viewer}___${debugger}") + else() + set(_targ_name "${viewer}_${model}_${debugger}") + endif() + endif() + add_custom_target(${_targ_name} + COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" + "${debugger}" + "${viewer}" "${model}" "${CMAKE_BINARY_DIR}" + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + USES_TERMINAL + ) + add_dependencies(${_targ_name} mainapp) + endforeach() endforeach() endforeach() From 3f6fd8b94dcdfb8a71244db728627a82da4eb045 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 Oct 2015 10:36:38 +0100 Subject: [PATCH 064/161] MC att ctrl: Do not build unused param file --- src/modules/mc_att_control/CMakeLists.txt | 1 - src/modules/mc_att_control/mc_att_control_main.cpp | 1 - src/modules/mc_att_control/mc_att_control_params.c | 3 --- 3 files changed, 5 deletions(-) diff --git a/src/modules/mc_att_control/CMakeLists.txt b/src/modules/mc_att_control/CMakeLists.txt index 3c08772766..6f857624c5 100644 --- a/src/modules/mc_att_control/CMakeLists.txt +++ b/src/modules/mc_att_control/CMakeLists.txt @@ -40,7 +40,6 @@ px4_add_module( COMPILE_FLAGS ${MODULE_CFLAGS} SRCS mc_att_control_main.cpp - mc_att_control_params.c DEPENDS platforms__common ) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index fa08536126..118a0709b8 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -39,7 +39,6 @@ * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors. * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011. * - * @author Tobias Naegeli * @author Lorenz Meier * @author Anton Babushkin * diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index a2a69d7116..0c7e4b0134 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -35,13 +35,10 @@ * @file mc_att_control_params.c * Parameters for multicopter attitude controller. * - * @author Tobias Naegeli * @author Lorenz Meier * @author Anton Babushkin */ -#include - /** * Roll P gain * From 878dc4d7d495877bb99e5554db69aec1b01107c2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 Oct 2015 10:36:54 +0100 Subject: [PATCH 065/161] MC pos ctrl: Do not build unused param file --- src/modules/mc_pos_control/CMakeLists.txt | 1 - src/modules/mc_pos_control/mc_pos_control_params.c | 2 -- 2 files changed, 3 deletions(-) diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index 3790bfd614..b501e657d8 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -36,7 +36,6 @@ px4_add_module( STACK 1200 SRCS mc_pos_control_main.cpp - mc_pos_control_params.c DEPENDS platforms__common ) diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 0fa51f6f0c..7c63a82afa 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -38,8 +38,6 @@ * @author Anton Babushkin */ -#include - /** * Minimum thrust in auto thrust control * From 249742d0449054052c03973d3b0726bf72856051 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 Oct 2015 10:37:07 +0100 Subject: [PATCH 066/161] SDLOG2: Do not build unused param file --- src/modules/sdlog2/CMakeLists.txt | 1 - src/modules/sdlog2/params.c | 2 -- 2 files changed, 3 deletions(-) diff --git a/src/modules/sdlog2/CMakeLists.txt b/src/modules/sdlog2/CMakeLists.txt index 1e33df2a86..b438f30fd5 100644 --- a/src/modules/sdlog2/CMakeLists.txt +++ b/src/modules/sdlog2/CMakeLists.txt @@ -44,7 +44,6 @@ px4_add_module( SRCS sdlog2.c logbuffer.c - params.c DEPENDS platforms__common ) diff --git a/src/modules/sdlog2/params.c b/src/modules/sdlog2/params.c index 77575e2ad3..6ad82d8156 100644 --- a/src/modules/sdlog2/params.c +++ b/src/modules/sdlog2/params.c @@ -31,8 +31,6 @@ * ****************************************************************************/ -#include - /** * Logging rate. * From bd092e430b7bc441b88b3bdbee09bd52034eaad4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 Oct 2015 11:11:02 +0100 Subject: [PATCH 067/161] Fix UAVCAN building --- .travis.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index 8d199d99a4..35c9aa1c28 100644 --- a/.travis.yml +++ b/.travis.yml @@ -107,8 +107,8 @@ script: - cd .. - mkdir -p ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/ - mkdir -p ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/ - - cp vectorcontrol/firmware/com.thiemar.s2740vc-v1-1.0.00000000.uavcan.bin ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/00000000.bin - - cp vectorcontrol/firmware/org.pixhawk.px4esc-v1-1.0.00000000.uavcan.bin ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/00000000.bin + - cp vectorcontrol/firmware/com.thiemar.s2740vc-v1-1.0.*.bin ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/. + - cp vectorcontrol/firmware/org.pixhawk.px4esc-v1-1.0.*.bin ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/. - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default - echo 'Running Tests..' && make px4fmu-v2_default test From 33135e59582412c96be22eb546a70dd8f2467651 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 29 Oct 2015 10:43:23 +0100 Subject: [PATCH 068/161] updated sitl_gazebo --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index d362e661b4..988a815607 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit d362e661b46474153f43f51a6eb947d4fda1bebe +Subproject commit 988a815607bf10fd017cbae03affba6f61304188 From 4206c28b03b48894c5505d6d22af9110674c8e8f Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 29 Oct 2015 16:23:46 +0100 Subject: [PATCH 069/161] fixed projection from local position to gps coordinates --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 988a815607..7d855cd385 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 988a815607bf10fd017cbae03affba6f61304188 +Subproject commit 7d855cd3853c6c6c41237b114bbc1fd9a3cf102f From 474e376f96f2006e2b5ee44f8d2cdde8671633ef Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 29 Oct 2015 12:03:57 -0700 Subject: [PATCH 070/161] Removed deadcode from accelsim Signed-off-by: Mark Charlebois --- .../posix/drivers/accelsim/accelsim.cpp | 156 ++---------------- 1 file changed, 10 insertions(+), 146 deletions(-) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 1f9fa0da05..8e770aafd8 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -227,41 +227,6 @@ private: */ void mag_measure(); - /** - * Read a register from the ACCELSIM - * - * @param The register to read. - * @return The value that was read. - */ - uint8_t read_reg(unsigned reg); - - /** - * Write a register in the ACCELSIM - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_reg(unsigned reg, uint8_t value); - - /** - * Modify a register in the ACCELSIM - * - * Bits are cleared before bits are set. - * - * @param reg The register to modify. - * @param clearbits Bits in the register to clear. - * @param setbits Bits in the register to set. - */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); - - /** - * Write a register in the ACCELSIM, updating _checked_values - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_checked_reg(unsigned reg, uint8_t value); - /** * Set the ACCELSIM accel measurement range. * @@ -329,25 +294,25 @@ public: ACCELSIM_mag(ACCELSIM *parent); ~ACCELSIM_mag(); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - virtual int init(); + virtual int init(); protected: friend class ACCELSIM; - void parent_poll_notify(); + void parent_poll_notify(); private: - ACCELSIM *_parent; + ACCELSIM *_parent; - orb_advert_t _mag_topic; - int _mag_orb_class_instance; - int _mag_class_instance; + orb_advert_t _mag_topic; + int _mag_orb_class_instance; + int _mag_class_instance; - void measure(); + void measure(); - void measure_trampoline(void *arg); + void measure_trampoline(void *arg); /* this class does not allow copying due to ptr data members */ ACCELSIM_mag(const ACCELSIM_mag &); @@ -391,8 +356,6 @@ ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : _last_temperature(0), _checked_next(0) { - - // enable debug() calls _debug_enabled = false; @@ -886,53 +849,6 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) } } -uint8_t -ACCELSIM::read_reg(unsigned reg) -{ - uint8_t cmd[2]; - - cmd[0] = reg | DIR_READ; - cmd[1] = 0; - - transfer(cmd, cmd, sizeof(cmd)); - - return cmd[1]; -} - -void -ACCELSIM::write_reg(unsigned reg, uint8_t value) -{ - uint8_t cmd[2]; - - cmd[0] = reg | DIR_WRITE; - cmd[1] = value; - - transfer(cmd, nullptr, sizeof(cmd)); -} - -void -ACCELSIM::write_checked_reg(unsigned reg, uint8_t value) -{ - write_reg(reg, value); - - for (uint8_t i = 0; i < ACCELSIM_NUM_CHECKED_REGISTERS; i++) { - if (reg == _checked_registers[i]) { - _checked_values[i] = value; - } - } -} - -void -ACCELSIM::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) -{ - uint8_t val; - - val = read_reg(reg); - val &= ~clearbits; - val |= setbits; - write_checked_reg(reg, val); -} - int ACCELSIM::accel_set_range(unsigned max_g) { @@ -1086,51 +1002,6 @@ ACCELSIM::measure() accel_report.y_raw = (int16_t)(raw_accel_report.y / _accel_range_scale); accel_report.z_raw = (int16_t)(raw_accel_report.z / _accel_range_scale); - // float xraw_f = (int16_t)(raw_accel_report.x/_accel_range_scale); - // float yraw_f = (int16_t)(raw_accel_report.y / _accel_range_scale); - // float zraw_f = (int16_t)(raw_accel_report.z / _accel_range_scale); - - // // apply user specified rotation - // rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - - // float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - // float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - // float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - - /* - we have logs where the accelerometers get stuck at a fixed - large value. We want to detect this and mark the sensor as - being faulty - */ - // if (fabsf(_last_accel[0] - x_in_new) < 0.001f && - // fabsf(_last_accel[1] - y_in_new) < 0.001f && - // fabsf(_last_accel[2] - z_in_new) < 0.001f && - // fabsf(x_in_new) > 20 && - // fabsf(y_in_new) > 20 && - // fabsf(z_in_new) > 20) { - // _constant_accel_count += 1; - // } else { - // _constant_accel_count = 0; - // } - // if (_constant_accel_count > 100) { - // // we've had 100 constant accel readings with large - // // values. The sensor is almost certainly dead. We - // // will raise the error_count so that the top level - // // flight code will know to avoid this sensor, but - // // we'll still give the data so that it can be logged - // // and viewed - // perf_count(_bad_values); - // _constant_accel_count = 0; - // } - - // _last_accel[0] = x_in_new; - // _last_accel[1] = y_in_new; - // _last_accel[2] = z_in_new; - - // accel_report.x = _accel_filter_x.apply(x_in_new); - // accel_report.y = _accel_filter_y.apply(y_in_new); - // accel_report.z = _accel_filter_z.apply(z_in_new); - accel_report.x = raw_accel_report.x; accel_report.y = raw_accel_report.y; accel_report.z = raw_accel_report.z; @@ -1218,13 +1089,6 @@ ACCELSIM::mag_measure() /* apply user specified rotation */ rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - // mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; - // mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; - // mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; - // mag_report.scaling = _mag_range_scale; - // mag_report.range_ga = (float)_mag_range_ga; - // mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - /* remember the temperature. The datasheet isn't clear, but it * seems to be a signed offset from 25 degrees C in units of 0.125C */ From 21ef602b5a2e7b7b1b7d7d5e81fc0d0011b5f322 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 29 Oct 2015 12:12:38 -0700 Subject: [PATCH 071/161] More accelsim cleanup Signed-off-by: Mark Charlebois --- .../posix/drivers/accelsim/accelsim.cpp | 17 ++--------------- 1 file changed, 2 insertions(+), 15 deletions(-) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 8e770aafd8..de33c738ab 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -107,11 +107,6 @@ public: virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - /** - * Diagnostics - print some basic information about the driver. - */ - //void print_info(); - /** * dump register values */ @@ -120,8 +115,8 @@ public: protected: friend class ACCELSIM_mag; - virtual ssize_t mag_read(device::file_t *filp, char *buffer, size_t buflen); - virtual int mag_ioctl(device::file_t *filp, int cmd, unsigned long arg); + ssize_t mag_read(device::file_t *filp, char *buffer, size_t buflen); + int mag_ioctl(device::file_t *filp, int cmd, unsigned long arg); int transfer(uint8_t *send, uint8_t *recv, unsigned len); private: @@ -302,7 +297,6 @@ public: protected: friend class ACCELSIM; - void parent_poll_notify(); private: ACCELSIM *_parent; @@ -1147,12 +1141,6 @@ out: return ret; } -void -ACCELSIM_mag::parent_poll_notify() -{ - poll_notify(POLLIN); -} - ssize_t ACCELSIM_mag::read(device::file_t *filp, char *buffer, size_t buflen) { @@ -1278,7 +1266,6 @@ info() } PX4_DEBUG("state @ %p", g_dev); - //g_dev->print_info(); return 0; } From a5c002ac2ea2a432d9a38ead8e0493a5f482fa09 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 29 Oct 2015 12:17:24 -0700 Subject: [PATCH 072/161] Removed more accelsim deadcode Signed-off-by: Mark Charlebois --- src/platforms/posix/drivers/accelsim/accelsim.cpp | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index de33c738ab..cafb127ee3 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -172,10 +172,6 @@ private: // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset -#define ACCELSIM_NUM_CHECKED_REGISTERS 1 - static const uint8_t _checked_registers[ACCELSIM_NUM_CHECKED_REGISTERS]; - uint8_t _checked_values[ACCELSIM_NUM_CHECKED_REGISTERS]; - uint8_t _checked_next; /** * Start automatic measurement. @@ -274,12 +270,6 @@ private: ACCELSIM operator=(const ACCELSIM &); }; -/* - list of registers that will be checked in check_registers(). Note - that ADDR_WHO_AM_I must be first in the list. - */ -const uint8_t ACCELSIM::_checked_registers[ACCELSIM_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I }; - /** * Helper class implementing the mag driver node. */ @@ -347,8 +337,7 @@ ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : _accel_filter_z(ACCELSIM_ACCEL_DEFAULT_RATE, ACCELSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), _constant_accel_count(0), - _last_temperature(0), - _checked_next(0) + _last_temperature(0) { // enable debug() calls _debug_enabled = false; From 8d59d08ad6f37eb83acd1a8b5c41b41c39796b5c Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 29 Oct 2015 12:21:52 -0700 Subject: [PATCH 073/161] Removed unnecessary header files from accelsim Signed-off-by: Mark Charlebois --- src/platforms/posix/drivers/accelsim/accelsim.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index cafb127ee3..5017bb111c 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -41,15 +41,10 @@ #include #include #include -#include #include #include -#include #include -#include #include -#include -#include #include #include #include @@ -64,7 +59,6 @@ #include #include #include -#include #include #include From 29db75fe56d7f00fa393bf4ed01a5ba1c6606a4d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Oct 2015 19:18:36 +0200 Subject: [PATCH 074/161] Commander: Disarm after 5 seconds on the ground. --- src/modules/commander/commander.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 95fcf9e0a5..679835a73a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -183,6 +183,7 @@ static hrt_abstime commander_boot_timestamp = 0; static unsigned int leds_counter; /* To remember when last notification was sent */ static uint64_t last_print_mode_reject_time = 0; +static uint64_t _inair_last_time = 0; static float eph_threshold = 5.0f; static float epv_threshold = 10.0f; @@ -1639,6 +1640,16 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "TAKEOFF DETECTED"); } } + + if (land_detector.landed) { + if (_inair_last_time > 0 && ((hrt_absolute_time() - _inair_last_time) > 5 * 1000 * 1000)) { + mavlink_log_critical(mavlink_fd, "AUTO DISARMING AFTER LANDING"); + arm_disarm(false, mavlink_fd, "auto disarm on land"); + _inair_last_time = 0; + } + } else { + _inair_last_time = land_detector.timestamp; + } } /* update battery status */ From a71164ea1169408bee04ddbabc8c6999458ffd59 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 26 Oct 2015 09:22:34 +0100 Subject: [PATCH 075/161] added parameter to disable auto disarming when landed --- src/modules/commander/commander.cpp | 29 +++++++++++++++++------- src/modules/commander/commander_params.c | 13 +++++++++++ 2 files changed, 34 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 679835a73a..5190c477a1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -921,6 +921,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_eph = param_find("COM_HOME_H_T"); param_t _param_epv = param_find("COM_HOME_V_T"); param_t _param_geofence_action = param_find("GF_ACTION"); + param_t _param_disarm_land = param_find("COM_DISARM_LAND"); // const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX]; // main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; @@ -1270,6 +1271,8 @@ int commander_thread_main(int argc, char *argv[]) int autosave_params; /**< Autosave of parameters enabled/disabled, loaded from parameter */ + int32_t disarm_when_landed = 0; + /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; @@ -1349,6 +1352,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); param_get(_param_ef_time_thres, &ef_time_thres); param_get(_param_geofence_action, &geofence_action); + param_get(_param_disarm_land, &disarm_when_landed); /* Autostart id */ param_get(_param_autostart_id, &autostart_id); @@ -1623,12 +1627,13 @@ int commander_thread_main(int argc, char *argv[]) &(status.condition_local_altitude_valid), &status_changed); /* Update land detector */ + static bool check_for_disarming = false; orb_check(land_detector_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector); } - if (updated && status.condition_local_altitude_valid) { + if ((updated && status.condition_local_altitude_valid) || check_for_disarming) { if (status.condition_landed != land_detector.landed) { status.condition_landed = land_detector.landed; status_changed = true; @@ -1641,14 +1646,22 @@ int commander_thread_main(int argc, char *argv[]) } } - if (land_detector.landed) { - if (_inair_last_time > 0 && ((hrt_absolute_time() - _inair_last_time) > 5 * 1000 * 1000)) { - mavlink_log_critical(mavlink_fd, "AUTO DISARMING AFTER LANDING"); - arm_disarm(false, mavlink_fd, "auto disarm on land"); - _inair_last_time = 0; + if (disarm_when_landed > 0) { + if (land_detector.landed) { + if (!check_for_disarming && _inair_last_time > 0) { + _inair_last_time = land_detector.timestamp; + check_for_disarming = true; + } + + if (_inair_last_time > 0 && ((hrt_absolute_time() - _inair_last_time) > (hrt_abstime)disarm_when_landed * 1000 * 1000)) { + mavlink_log_critical(mavlink_fd, "AUTO DISARMING AFTER LANDING"); + arm_disarm(false, mavlink_fd, "auto disarm on land"); + _inair_last_time = 0; + check_for_disarming = false; + } + } else { + _inair_last_time = land_detector.timestamp; } - } else { - _inair_last_time = land_detector.timestamp; } } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 214b7220c3..1c4108017f 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -274,3 +274,16 @@ PARAM_DEFINE_INT32(COM_AUTOS_PAR, 1); * @max 2 */ PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0); + +/** + * Time-out for auto disarm after landing + * + * A non-zero, positive value specifies the time-out period in seconds after which the vehicle will be + * automatically disarmed in case a landing situation has been detected during this period. + * A value of zero means that automatic disarming is disabled. + * + * @group Commander + * @min 0 + */ +PARAM_DEFINE_INT32(COM_DISARM_LAND, 0); + From 1aa9304b638eacff7f491836d44db16612807ddd Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Fri, 30 Oct 2015 15:06:47 +0100 Subject: [PATCH 076/161] Logging rate was limited to 1 Hz I set the maximum to 100 Hz, since most SD cards are not fast enough for more. (but more is still possible with forcing) --- src/modules/sdlog2/params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/params.c b/src/modules/sdlog2/params.c index 77575e2ad3..af1c6287f9 100644 --- a/src/modules/sdlog2/params.c +++ b/src/modules/sdlog2/params.c @@ -43,7 +43,7 @@ * commonly is before arming). * * @min -1 - * @max 1 + * @max 100 * @group SD Logging */ PARAM_DEFINE_INT32(SDLOG_RATE, -1); From b622080a65be29656df1557cc459b061fdacb96b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Oct 2015 15:40:22 +0100 Subject: [PATCH 077/161] Switch GIT command for revision hash --- cmake/common/px4_base.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 26598fef70..eb3b9c68fc 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -700,7 +700,7 @@ function(px4_create_git_hash_header) REQUIRED HEADER ARGN ${ARGN}) execute_process( - COMMAND git log -n 1 --pretty=format:"%H" + COMMAND git rev-parse HEAD OUTPUT_VARIABLE git_desc OUTPUT_STRIP_TRAILING_WHITESPACE WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} From f042c077b2f61feaf6046eb78148116232cf3365 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Oct 2015 16:01:21 +0100 Subject: [PATCH 078/161] Fix compile fail for GIT version --- .travis.yml | 2 ++ cmake/common/px4_base.cmake | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 35c9aa1c28..0b2dcd51d9 100644 --- a/.travis.yml +++ b/.travis.yml @@ -128,6 +128,8 @@ after_success: && echo https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip ; fi + - git log -n 1 --pretty=format:"%H" + - git rev-parse HEAD deploy: provider: releases diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index eb3b9c68fc..26598fef70 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -700,7 +700,7 @@ function(px4_create_git_hash_header) REQUIRED HEADER ARGN ${ARGN}) execute_process( - COMMAND git rev-parse HEAD + COMMAND git log -n 1 --pretty=format:"%H" OUTPUT_VARIABLE git_desc OUTPUT_STRIP_TRAILING_WHITESPACE WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} From c912be70d1576f5b5c2e2fe850a179a60967655a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Oct 2015 16:25:08 +0100 Subject: [PATCH 079/161] Update approach to obtain the head git hash --- .travis.yml | 2 -- cmake/common/px4_base.cmake | 2 +- cmake/templates/build_git_version.h.in | 2 +- 3 files changed, 2 insertions(+), 4 deletions(-) diff --git a/.travis.yml b/.travis.yml index 0b2dcd51d9..35c9aa1c28 100644 --- a/.travis.yml +++ b/.travis.yml @@ -128,8 +128,6 @@ after_success: && echo https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip ; fi - - git log -n 1 --pretty=format:"%H" - - git rev-parse HEAD deploy: provider: releases diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 26598fef70..eb3b9c68fc 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -700,7 +700,7 @@ function(px4_create_git_hash_header) REQUIRED HEADER ARGN ${ARGN}) execute_process( - COMMAND git log -n 1 --pretty=format:"%H" + COMMAND git rev-parse HEAD OUTPUT_VARIABLE git_desc OUTPUT_STRIP_TRAILING_WHITESPACE WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} diff --git a/cmake/templates/build_git_version.h.in b/cmake/templates/build_git_version.h.in index 1d1adb9ba3..83c59c37d9 100644 --- a/cmake/templates/build_git_version.h.in +++ b/cmake/templates/build_git_version.h.in @@ -1,4 +1,4 @@ /* Auto Magically Generated file */ /* Do not edit! */ -#define PX4_GIT_VERSION_STR @git_desc@ +#define PX4_GIT_VERSION_STR "@git_desc@" #define PX4_GIT_VERSION_BINARY 0x@git_desc_short@ From 4e87ac601485a8086819cadaf4c3a7d05b60024b Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 30 Oct 2015 16:18:17 +0100 Subject: [PATCH 080/161] run preflight checks after mode switch has changed --- src/modules/commander/commander.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5190c477a1..a2d5dc003a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -92,6 +92,7 @@ #include #include #include +#include #include #include @@ -922,6 +923,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_epv = param_find("COM_HOME_V_T"); param_t _param_geofence_action = param_find("GF_ACTION"); param_t _param_disarm_land = param_find("COM_DISARM_LAND"); + param_t _param_map_mode_sw = param_find("RC_MAP_MODE_SW"); // const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX]; // main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; @@ -1272,6 +1274,7 @@ int commander_thread_main(int argc, char *argv[]) int autosave_params; /**< Autosave of parameters enabled/disabled, loaded from parameter */ int32_t disarm_when_landed = 0; + int32_t map_mode_sw = 0; /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; @@ -1337,6 +1340,16 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; + // check if main mode switch has been assigned and if so run preflight checks in order + // to update status.condition_sensors_initialised + int32_t map_mode_sw_new; + param_get(_param_map_mode_sw, &map_mode_sw_new); + + if (map_mode_sw == 0 && map_mode_sw != map_mode_sw_new && map_mode_sw_new < input_rc_s::RC_INPUT_MAX_CHANNELS && map_mode_sw_new > 0) { + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + } + /* re-check RC calibration */ rc_calibration_check(mavlink_fd); } @@ -1353,6 +1366,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_ef_time_thres, &ef_time_thres); param_get(_param_geofence_action, &geofence_action); param_get(_param_disarm_land, &disarm_when_landed); + param_get(_param_map_mode_sw, &map_mode_sw); /* Autostart id */ param_get(_param_autostart_id, &autostart_id); From 3d13e771a8d5b58cacb3369934a802b312cfee7e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Oct 2015 16:33:19 +0100 Subject: [PATCH 081/161] Support runs without simulation --- Tools/sitl_run.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 7f7808c9e6..521ebeef21 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -42,13 +42,13 @@ cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit SIM_PID=0 -if [ "$program" == "jmavsim" ] +if [ "$program" == "jmavsim" ] && [ "$no_sim" == "" ] then cd Tools/jMAVSim ant java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 & SIM_PID=`echo $!` -elif [ "$3" == "gazebo" ] +elif [ "$3" == "gazebo" ] && [ "$no_sim" == "" ] then if [ -x "$(command -v gazebo)" ] then From e0e7a2414e6195fb59c973c443fee304f70afc63 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Oct 2015 16:59:37 +0100 Subject: [PATCH 082/161] Do not check component ID for mission handling partner --- src/modules/mavlink/mavlink_mission.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 7e9f546d26..d710558ef0 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -403,7 +403,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) mavlink_msg_mission_ack_decode(msg, &wpa); if (CHECK_SYSID_COMPID_MISSION(wpa)) { - if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) { + if ((msg->sysid == _transfer_partner_sysid/* && msg->compid == _transfer_partner_compid*/)) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -504,7 +504,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) mavlink_msg_mission_request_decode(msg, &wpr); if (CHECK_SYSID_COMPID_MISSION(wpr)) { - if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) { + if (msg->sysid == _transfer_partner_sysid/* && msg->compid == _transfer_partner_compid*/) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); From 90798a7cb6ab003b351b8b9d6b848ad96f06e7c6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Oct 2015 17:08:12 +0100 Subject: [PATCH 083/161] Revert "Do not check component ID for mission handling partner" This reverts commit e0e7a2414e6195fb59c973c443fee304f70afc63. --- src/modules/mavlink/mavlink_mission.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index d710558ef0..7e9f546d26 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -403,7 +403,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) mavlink_msg_mission_ack_decode(msg, &wpa); if (CHECK_SYSID_COMPID_MISSION(wpa)) { - if ((msg->sysid == _transfer_partner_sysid/* && msg->compid == _transfer_partner_compid*/)) { + if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -504,7 +504,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) mavlink_msg_mission_request_decode(msg, &wpr); if (CHECK_SYSID_COMPID_MISSION(wpr)) { - if (msg->sysid == _transfer_partner_sysid/* && msg->compid == _transfer_partner_compid*/) { + if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); From fe4c67a781db8a192229f22e70f08a36d7fcd70f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Oct 2015 18:40:55 +0100 Subject: [PATCH 084/161] Fixed MAVLink mission transmission --- src/modules/mavlink/mavlink_mission.cpp | 21 ++++++++++----------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 7e9f546d26..bb0908908c 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -422,7 +422,9 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) } else { _mavlink->send_statustext_critical("REJ. WP CMD: partner id mismatch"); - if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); } + if (_verbose) { + warnx("WPM: MISSION_ACK ERR: ID mismatch"); + } } } } @@ -473,13 +475,13 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); - if (_count > 0) { - _state = MAVLINK_WPM_STATE_SENDLIST; - _transfer_seq = 0; - _transfer_count = _count; - _transfer_partner_sysid = msg->sysid; - _transfer_partner_compid = msg->compid; + _state = MAVLINK_WPM_STATE_SENDLIST; + _transfer_seq = 0; + _transfer_count = _count; + _transfer_partner_sysid = msg->sysid; + _transfer_partner_compid = msg->compid; + if (_count > 0) { if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK, %u mission items to send", _transfer_count); } } else { @@ -598,9 +600,8 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) /* alternate dataman ID anyway to let navigator know about changes */ update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0); - _mavlink->send_statustext_info("WPM: COUNT 0: CLEAR MISSION"); - // TODO send ACK? + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); _transfer_in_progress = false; return; } @@ -712,8 +713,6 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) /* got all new mission items successfully */ if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); } - _mavlink->send_statustext_info("WPM: Transfer complete."); - _state = MAVLINK_WPM_STATE_IDLE; if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) { From 5394c661c7bf2b660a113999bcd7e0509f07845a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Oct 2015 20:22:50 +0200 Subject: [PATCH 085/161] Navigator: Fix RTL to return in a straight line --- src/modules/navigator/rtl.cpp | 10 +++++++++- src/modules/navigator/rtl.h | 2 ++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 8a4d89ef35..6e754588bb 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -58,6 +58,7 @@ RTL::RTL(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _rtl_state(RTL_STATE_NONE), + _rtl_start_lock(false), _param_return_alt(this, "RTL_RETURN_ALT", false), _param_descend_alt(this, "RTL_DESCEND_ALT", false), _param_land_delay(this, "RTL_LAND_DELAY", false) @@ -95,6 +96,7 @@ RTL::on_activation() } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + _param_return_alt.get()) { _rtl_state = RTL_STATE_CLIMB; + _rtl_start_lock = false; /* otherwise go straight to return */ } else { @@ -102,6 +104,7 @@ RTL::on_activation() _rtl_state = RTL_STATE_RETURN; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_global_position()->alt; + _rtl_start_lock = false; } } @@ -126,7 +129,10 @@ RTL::set_rtl_item() /* make sure we have the latest params */ updateParams(); - set_previous_pos_setpoint(); + if (!_rtl_start_lock) { + set_previous_pos_setpoint(); + } + _navigator->set_can_loiter_at_sp(false); switch (_rtl_state) { @@ -182,6 +188,8 @@ RTL::set_rtl_item() mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d m (%d m above home)", (int)(_mission_item.altitude), (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); + + _rtl_start_lock = true; break; } diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 3506065802..464a1d4ee4 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -87,6 +87,8 @@ private: RTL_STATE_LANDED, } _rtl_state; + bool _rtl_start_lock; + control::BlockParamFloat _param_return_alt; control::BlockParamFloat _param_descend_alt; control::BlockParamFloat _param_land_delay; From 85fcfed083126fe72774c664e030ce7995591c8e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Oct 2015 20:17:13 +0100 Subject: [PATCH 086/161] Add missing yaw entries --- src/modules/navigator/rtl.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 6e754588bb..a77f5895e1 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -221,7 +221,7 @@ RTL::set_rtl_item() _mission_item.lon = _navigator->get_home_position()->lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); - _mission_item.yaw = NAN; + _mission_item.yaw = _navigator->get_home_position()->yaw; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED; @@ -247,7 +247,7 @@ RTL::set_rtl_item() _mission_item.lon = _navigator->get_home_position()->lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_home_position()->alt; - _mission_item.yaw = NAN; + _mission_item.yaw = _navigator->get_home_position()->yaw; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LAND; @@ -266,6 +266,7 @@ RTL::set_rtl_item() _mission_item.lon = _navigator->get_home_position()->lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_home_position()->alt; + // Do not change / control yaw in landed _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; From 35d8e0bc550645b470af5c8624243b8f3a56d081 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Oct 2015 20:32:58 +0100 Subject: [PATCH 087/161] Raise frame size --- src/modules/commander/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index 6cf2531ade..c1ec35b5ee 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -32,7 +32,7 @@ ############################################################################ set(MODULE_CFLAGS -Os) if(${OS} STREQUAL "nuttx") - list(APPEND MODULE_CFLAGS -Wframe-larger-than=2400) + list(APPEND MODULE_CFLAGS -Wframe-larger-than=2450) endif() px4_add_module( MODULE modules__commander From 601da2818b3e73f5fc12a7615c570ee24a4e52d7 Mon Sep 17 00:00:00 2001 From: Roman Date: Sun, 1 Nov 2015 10:13:55 +0100 Subject: [PATCH 088/161] do not set position setpoint too early --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 80847d8e47..2c2010e895 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -714,16 +714,18 @@ MulticopterPositionControl::control_manual(float dt) (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy))) { _pos_hold_engaged = true; + } else { + _pos_hold_engaged = false; } } else { _pos_hold_engaged = false; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); } /* set requested velocity setpoint */ if (!_pos_hold_engaged) { + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); _run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */ _vel_sp(0) = req_vel_sp_scaled(0); _vel_sp(1) = req_vel_sp_scaled(1); From 5050da0ba00c45377f4ef91340fa89eb8faa1b9d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Oct 2015 11:29:44 +0200 Subject: [PATCH 089/161] TECS: Add function to reset system --- src/lib/external_lgpl/tecs/tecs.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 914e41eba0..1ca7428849 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -118,6 +118,9 @@ public: return get_throttle_demand(); } + void reset_state() { + _states_initalized = false; + } float get_pitch_demand() { return _pitch_dem; } From eeecf01628eae742ce1d9ea64484e8e063b61a78 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Oct 2015 11:30:14 +0200 Subject: [PATCH 090/161] Fixed wing controller: Call state reset on flight mode change. --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index fa12c5822a..a629b9d2cb 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1118,6 +1118,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + } else { + _tecs.reset_state(); } } _control_mode_current = FW_POSCTRL_MODE_AUTO; @@ -1477,6 +1479,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + } else { + _tecs.reset_state(); } } _control_mode_current = FW_POSCTRL_MODE_POSITION; @@ -1588,6 +1592,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + } else { + _tecs.reset_state(); } } _control_mode_current = FW_POSCTRL_MODE_ALTITUDE; From 42523be0ebf6b8bb3312f58a8f31fdd3b97399fb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Nov 2015 17:38:27 +0100 Subject: [PATCH 091/161] EKF: Allow more delta velocity deviation before resetting --- src/modules/ekf_att_pos_estimator/estimator_22states.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 0e7b459b72..b794b9f90d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -2825,7 +2825,7 @@ bool AttPosEKF::VelNEDDiverged() Vector3f delta = current_vel - gps_vel; float delta_len = delta.length(); - bool excessive = (delta_len > 20.0f); + bool excessive = (delta_len > 30.0f); current_ekf_state.error |= excessive; current_ekf_state.velOffsetExcessive = excessive; From 3edf532e90f89f9e58059610b6f3310e7ca1cc0b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Nov 2015 17:39:22 +0100 Subject: [PATCH 092/161] MAVLink receiver: Fix HIL handling --- src/modules/mavlink/mavlink_receiver.cpp | 58 ++++++++++++------------ 1 file changed, 28 insertions(+), 30 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d0b95b7f89..5f36354191 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1116,7 +1116,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) /* yaw */ rc.values[2] = man.r / 2 + 1500; /* throttle */ - rc.values[3] = man.z / 1 + 1000; + rc.values[3] = fminf(fmaxf(man.z / 0.9f + 800, 1000.0f), 2000.0f); /* decode all switches which fit into the channel mask */ unsigned max_switch = (sizeof(man.buttons) * 8); @@ -1307,18 +1307,22 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) float dt; if (_hil_last_frame == 0 || - (imu.time_usec - _hil_last_frame) > (0.1f * 1e6f) || - (_hil_last_frame >= imu.time_usec)) { + (timestamp <= _hil_last_frame) || + (timestamp - _hil_last_frame) > (0.1f * 1e6f) || + (_hil_last_frame >= timestamp)) { dt = 0.01f; /* default to 100 Hz */ + memset(&_hil_prev_gyro[0], 0, sizeof(_hil_prev_gyro)); + _hil_prev_accel[0] = 0.0f; + _hil_prev_accel[1] = 0.0f; + _hil_prev_accel[2] = 9.866f; } else { - dt = (imu.time_usec - _hil_last_frame) / 1e6f; + dt = (timestamp - _hil_last_frame) / 1e6f; } - _hil_last_frame = imu.time_usec; + _hil_last_frame = timestamp; /* airspeed */ { - struct airspeed_s airspeed; - memset(&airspeed, 0, sizeof(airspeed)); + struct airspeed_s airspeed = {}; float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); // XXX need to fix this @@ -1338,8 +1342,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* gyro */ { - struct gyro_report gyro; - memset(&gyro, 0, sizeof(gyro)); + struct gyro_report gyro = {}; gyro.timestamp = timestamp; gyro.x_raw = imu.xgyro * 1000.0f; @@ -1360,8 +1363,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* accelerometer */ { - struct accel_report accel; - memset(&accel, 0, sizeof(accel)); + struct accel_report accel = {}; accel.timestamp = timestamp; accel.x_raw = imu.xacc / mg2ms2; @@ -1382,8 +1384,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* magnetometer */ { - struct mag_report mag; - memset(&mag, 0, sizeof(mag)); + struct mag_report mag = {}; mag.timestamp = timestamp; mag.x_raw = imu.xmag * 1000.0f; @@ -1404,8 +1405,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* baro */ { - struct baro_report baro; - memset(&baro, 0, sizeof(baro)); + struct baro_report baro = {}; baro.timestamp = timestamp; baro.pressure = imu.abs_pressure; @@ -1422,21 +1422,20 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* sensor combined */ { - struct sensor_combined_s hil_sensors; - memset(&hil_sensors, 0, sizeof(hil_sensors)); + struct sensor_combined_s hil_sensors = {}; hil_sensors.timestamp = timestamp; hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f; hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f; hil_sensors.gyro_raw[2] = imu.zgyro * 1000.0f; - hil_sensors.gyro_rad_s[0] = ((imu.xgyro * dt + _hil_prev_gyro[0]) / 2.0f) / dt; + hil_sensors.gyro_rad_s[0] = imu.xgyro; hil_sensors.gyro_rad_s[1] = imu.ygyro; hil_sensors.gyro_rad_s[2] = imu.zgyro; - hil_sensors.gyro_integral_rad[0] = (hil_sensors.gyro_rad_s[0] * dt + _hil_prev_gyro[0]) / 2.0f; - hil_sensors.gyro_integral_rad[1] = (hil_sensors.gyro_rad_s[1] * dt + _hil_prev_gyro[1]) / 2.0f; - hil_sensors.gyro_integral_rad[2] = (hil_sensors.gyro_rad_s[2] * dt + _hil_prev_gyro[2]) / 2.0f; - memcpy(&_hil_prev_gyro[0], &hil_sensors.gyro_integral_rad[0], sizeof(_hil_prev_gyro)); + hil_sensors.gyro_integral_rad[0] = 0.5f * (imu.xgyro + _hil_prev_gyro[0]) * dt; + hil_sensors.gyro_integral_rad[1] = 0.5f * (imu.ygyro + _hil_prev_gyro[1]) * dt; + hil_sensors.gyro_integral_rad[2] = 0.5f * (imu.zgyro + _hil_prev_gyro[2]) * dt; + memcpy(&_hil_prev_gyro[0], &hil_sensors.gyro_rad_s[0], sizeof(_hil_prev_gyro)); hil_sensors.gyro_integral_dt[0] = dt * 1e6f; hil_sensors.gyro_timestamp[0] = timestamp; hil_sensors.gyro_priority[0] = ORB_PRIO_HIGH; @@ -1447,10 +1446,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.accelerometer_m_s2[0] = imu.xacc; hil_sensors.accelerometer_m_s2[1] = imu.yacc; hil_sensors.accelerometer_m_s2[2] = imu.zacc; - hil_sensors.accelerometer_integral_m_s[0] = (imu.xacc * dt + _hil_prev_accel[0]) / 2.0f; - hil_sensors.accelerometer_integral_m_s[1] = (imu.yacc * dt + _hil_prev_accel[1]) / 2.0f; - hil_sensors.accelerometer_integral_m_s[2] = (imu.zacc * dt + _hil_prev_accel[2]) / 2.0f; - memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_integral_m_s[0], sizeof(_hil_prev_accel)); + hil_sensors.accelerometer_integral_m_s[0] = 0.5f * (imu.xacc + _hil_prev_accel[0]) * dt; + hil_sensors.accelerometer_integral_m_s[1] = 0.5f * (imu.yacc + _hil_prev_accel[1]) * dt; + hil_sensors.accelerometer_integral_m_s[2] = 0.5f * (imu.zacc + _hil_prev_accel[2]) * dt; + memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_m_s2[0], sizeof(_hil_prev_accel)); hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f; hil_sensors.accelerometer_mode[0] = 0; // TODO what is this? hil_sensors.accelerometer_range_m_s2[0] = 32.7f; // int16 @@ -1493,12 +1492,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* battery status */ { - struct battery_status_s hil_battery_status; - memset(&hil_battery_status, 0, sizeof(hil_battery_status)); + struct battery_status_s hil_battery_status = {}; hil_battery_status.timestamp = timestamp; - hil_battery_status.voltage_v = 11.1f; - hil_battery_status.voltage_filtered_v = 11.1f; + hil_battery_status.voltage_v = 11.5f; + hil_battery_status.voltage_filtered_v = 11.5f; hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; From fa78e8523e15eb6750610b1637ede0ff42d6e015 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Nov 2015 17:39:56 +0100 Subject: [PATCH 093/161] EKF: Fix time handling --- .../ekf_att_pos_estimator_main.cpp | 51 ++++++++++++++----- 1 file changed, 37 insertions(+), 14 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 04be54eb33..2ea78ddf14 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -588,6 +588,11 @@ void AttitudePositionEstimatorEKF::task_main() /* system is in HIL now, wait for measurements to come in one last round */ usleep(60000); + /* HIL is slow, set permissive timeouts */ + _voter_gyro.set_timeout(500000); + _voter_accel.set_timeout(500000); + _voter_mag.set_timeout(500000); + /* now read all sensor publications to ensure all real sensor data is purged */ orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); @@ -1223,6 +1228,23 @@ void AttitudePositionEstimatorEKF::pollData() orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); + /* set time fields */ + IMUusec = _sensor_combined.timestamp; + float deltaT = (_sensor_combined.timestamp - _last_sensor_timestamp) / 1e6f; + + /* guard against too large deltaT's */ + if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.0001f) { + + if (PX4_ISFINITE(_ekf->dtIMUfilt) && _ekf->dtIMUfilt < 0.5f && _ekf->dtIMUfilt > 0.0001f) { + deltaT = _ekf->dtIMUfilt; + } else { + deltaT = 0.01f; + } + } + + /* fill in last data set */ + _ekf->dtIMU = deltaT; + // Feed validator with recent sensor data for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) { @@ -1245,6 +1267,11 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1]; _ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2]; } else { + float dt_gyro = _sensor_combined.gyro_integral_dt[_gyro_main] / 1e6f; + if (PX4_ISFINITE(dt_gyro) && (dt_gyro < 0.5f) && (dt_gyro > 0.00001f)) { + deltaT = dt_gyro; + _ekf->dtIMU = deltaT; + } _ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]) * _ekf->dtIMU; _ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]) * _ekf->dtIMU; _ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]) * _ekf->dtIMU; @@ -1317,20 +1344,8 @@ void AttitudePositionEstimatorEKF::pollData() _vibration_warning_timestamp = 0; } - IMUusec = _sensor_combined.timestamp; - - float deltaT = (_sensor_combined.timestamp - _last_sensor_timestamp) / 1e6f; _last_sensor_timestamp = _sensor_combined.timestamp; - /* guard against too large deltaT's */ - if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { - deltaT = 0.01f; - } - - // Always store data, independent of init status - /* fill in last data set */ - _ekf->dtIMU = deltaT; - // XXX this is for assessing the filter performance // leave this in as long as larger improvements are still being made. #if 0 @@ -1342,7 +1357,7 @@ void AttitudePositionEstimatorEKF::pollData() static unsigned dtoverflow10 = 0; static hrt_abstime lastprint = 0; - if (hrt_elapsed_time(&lastprint) > 1000000) { + if (hrt_elapsed_time(&lastprint) > 1000000 || _sensor_combined.gyro_rad_s[0] > 4.0f) { PX4_WARN("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u", (double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc, dtoverflow5, dtoverflow10); @@ -1361,7 +1376,15 @@ void AttitudePositionEstimatorEKF::pollData() (double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[1] * deltaT), - (double)((_sensor_combined.accelerometer_m_s2[2] + 9.8665f) * deltaT)); + (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT)); + + PX4_WARN("EKF rate: %8.4f, %8.4f, %8.4f", + (double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed); + + PX4_WARN("DRV rate: %8.4f, %8.4f, %8.4f", + (double)_sensor_combined.gyro_rad_s[0], + (double)_sensor_combined.gyro_rad_s[1], + (double)_sensor_combined.gyro_rad_s[2]); lastprint = hrt_absolute_time(); } From c0a663540039643b05c5ed2e2624fcc38fa93c25 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 10:18:12 +0100 Subject: [PATCH 094/161] Lock VDEV table to prevent concurrent access --- src/drivers/device/vdev_posix.cpp | 66 ++++++++++++++++++++++++------- 1 file changed, 51 insertions(+), 15 deletions(-) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index a30bc8d06b..c5e1c9a4f5 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -52,6 +52,8 @@ using namespace device; +pthread_mutex_t filemutex = PTHREAD_MUTEX_INITIALIZER; + extern "C" { static void timer_cb(void *data) @@ -68,7 +70,27 @@ extern "C" { inline bool valid_fd(int fd) { - return (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL); + pthread_mutex_lock(&filemutex); + bool ret = (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL); + pthread_mutex_unlock(&filemutex); + return ret; + } + + inline VDev *get_vdev(int fd) + { + pthread_mutex_lock(&filemutex); + bool valid = (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL); + VDev *dev; + + if (valid) { + dev = (VDev *)(filemap[fd]->vdev); + + } else { + dev = nullptr; + } + + pthread_mutex_unlock(&filemutex); + return dev; } int px4_open(const char *path, int flags, ...) @@ -93,6 +115,9 @@ extern "C" { } if (dev) { + + pthread_mutex_lock(&filemutex); + for (i = 0; i < PX4_MAX_FD; ++i) { if (filemap[i] == 0) { filemap[i] = new device::file_t(flags, dev, i); @@ -100,6 +125,8 @@ extern "C" { } } + pthread_mutex_unlock(&filemutex); + if (i < PX4_MAX_FD) { ret = dev->open(filemap[i]); @@ -125,11 +152,14 @@ extern "C" { { int ret; - if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->vdev); - PX4_DEBUG("px4_close fd = %d", fd); + VDev *dev = get_vdev(fd); + + if (dev) { + pthread_mutex_lock(&filemutex); ret = dev->close(filemap[fd]); - filemap[fd] = NULL; + filemap[fd] = nullptr; + pthread_mutex_unlock(&filemutex); + PX4_DEBUG("px4_close fd = %d", fd); } else { ret = -EINVAL; @@ -147,8 +177,9 @@ extern "C" { { int ret; - if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->vdev); + VDev *dev = get_vdev(fd); + + if (dev) { PX4_DEBUG("px4_read fd = %d", fd); ret = dev->read(filemap[fd], (char *)buffer, buflen); @@ -168,8 +199,9 @@ extern "C" { { int ret; - if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->vdev); + VDev *dev = get_vdev(fd); + + if (dev) { PX4_DEBUG("px4_write fd = %d", fd); ret = dev->write(filemap[fd], (const char *)buffer, buflen); @@ -190,8 +222,9 @@ extern "C" { PX4_DEBUG("px4_ioctl fd = %d", fd); int ret = 0; - if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->vdev); + VDev *dev = get_vdev(fd); + + if (dev) { ret = dev->ioctl(filemap[fd], cmd, arg); } else { @@ -221,9 +254,10 @@ extern "C" { fds[i].revents = 0; fds[i].priv = NULL; + VDev *dev = get_vdev(fds[i].fd); + // If fd is valid - if (valid_fd(fds[i].fd)) { - VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);; + if (dev) { PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], true); @@ -251,9 +285,11 @@ extern "C" { // For each fd for (i = 0; i < nfds; ++i) { + + VDev *dev = get_vdev(fds[i].fd); + // If fd is valid - if (valid_fd(fds[i].fd)) { - VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);; + if (dev) { PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], false); From 06987c624481201495334fa114e90a2ff26331c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 11:05:09 +0100 Subject: [PATCH 095/161] Lock vdev devmap --- src/drivers/device/vdev.cpp | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 60761ed511..4aa58447f3 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -67,6 +67,7 @@ private: #define PX4_MAX_DEV 500 static px4_dev_t *devmap[PX4_MAX_DEV]; +pthread_mutex_t devmutex = PTHREAD_MUTEX_INITIALIZER; /* * The standard NuttX operation dispatch table can't call C++ member functions @@ -142,8 +143,12 @@ VDev::register_driver(const char *name, void *data) // Make sure the device does not already exist // FIXME - convert this to a map for efficiency + + pthread_mutex_lock(&devmutex); + for (int i = 0; i < PX4_MAX_DEV; ++i) { if (devmap[i] && (strcmp(devmap[i]->name, name) == 0)) { + pthread_mutex_unlock(&devmutex); return -EEXIST; } } @@ -157,6 +162,8 @@ VDev::register_driver(const char *name, void *data) } } + pthread_mutex_unlock(&devmutex); + if (ret != PX4_OK) { PX4_ERR("No free devmap entries - increase PX4_MAX_DEV"); } @@ -174,6 +181,8 @@ VDev::unregister_driver(const char *name) return -EINVAL; } + pthread_mutex_lock(&devmutex); + for (int i = 0; i < PX4_MAX_DEV; ++i) { if (devmap[i] && (strcmp(name, devmap[i]->name) == 0)) { delete devmap[i]; @@ -184,6 +193,8 @@ VDev::unregister_driver(const char *name) } } + pthread_mutex_unlock(&devmutex); + return ret; } @@ -194,15 +205,20 @@ VDev::unregister_class_devname(const char *class_devname, unsigned class_instanc char name[32]; snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); + pthread_mutex_lock(&devmutex); + for (int i = 0; i < PX4_MAX_DEV; ++i) { if (devmap[i] && strcmp(devmap[i]->name, name) == 0) { delete devmap[i]; PX4_DEBUG("Unregistered class DEV %s", name); devmap[i] = NULL; + pthread_mutex_unlock(&devmutex); return PX4_OK; } } + pthread_mutex_unlock(&devmutex); + return -EINVAL; } @@ -497,15 +513,20 @@ VDev *VDev::getDev(const char *path) PX4_DEBUG("VDev::getDev"); int i = 0; + pthread_mutex_lock(&devmutex); + for (; i < PX4_MAX_DEV; ++i) { //if (devmap[i]) { // printf("%s %s\n", devmap[i]->name, path); //} if (devmap[i] && (strcmp(devmap[i]->name, path) == 0)) { + pthread_mutex_unlock(&devmutex); return (VDev *)(devmap[i]->cdev); } } + pthread_mutex_unlock(&devmutex); + return NULL; } @@ -514,11 +535,15 @@ void VDev::showDevices() int i = 0; PX4_INFO("Devices:"); + pthread_mutex_lock(&devmutex); + for (; i < PX4_MAX_DEV; ++i) { if (devmap[i] && strncmp(devmap[i]->name, "/dev/", 5) == 0) { PX4_INFO(" %s", devmap[i]->name); } } + + pthread_mutex_unlock(&devmutex); } void VDev::showTopics() @@ -526,11 +551,15 @@ void VDev::showTopics() int i = 0; PX4_INFO("Devices:"); + pthread_mutex_lock(&devmutex); + for (; i < PX4_MAX_DEV; ++i) { if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) == 0) { PX4_INFO(" %s", devmap[i]->name); } } + + pthread_mutex_unlock(&devmutex); } void VDev::showFiles() @@ -538,12 +567,16 @@ void VDev::showFiles() int i = 0; PX4_INFO("Files:"); + pthread_mutex_lock(&devmutex); + for (; i < PX4_MAX_DEV; ++i) { if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) != 0 && strncmp(devmap[i]->name, "/dev/", 5) != 0) { PX4_INFO(" %s", devmap[i]->name); } } + + pthread_mutex_unlock(&devmutex); } const char *VDev::topicList(unsigned int *next) From 013e34661206a69fec61ec4d0998d36d299b5a42 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 11:51:50 +0100 Subject: [PATCH 096/161] Commander: Fix Geofence action access --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 54463ee94d..5cdd7cf4e4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1269,7 +1269,7 @@ int commander_thread_main(int argc, char *argv[]) float rc_loss_timeout = 0.5; int32_t datalink_regain_timeout = 0; - uint8_t geofence_action = 0; + int32_t geofence_action = 0; /* Thresholds for engine failure detection */ int32_t ef_throttle_thres = 1.0f; From 7f242d71706919b4f6b2ad329cd67882976abf87 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 13:42:20 +0100 Subject: [PATCH 097/161] Update jMAVSim --- Tools/jMAVSim | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/jMAVSim b/Tools/jMAVSim index 96029523d1..fd9ed5df04 160000 --- a/Tools/jMAVSim +++ b/Tools/jMAVSim @@ -1 +1 @@ -Subproject commit 96029523d13fc7fdac04c1a50b08c0eb0b39f9a9 +Subproject commit fd9ed5df0496d42f20b91cb405639ceccbcf9e17 From a567b9956f29da1ac057ca549bc41d7a563a7e5b Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 2 Nov 2015 16:11:24 +0100 Subject: [PATCH 098/161] fix position hold logic --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 2c2010e895..a465cc64f5 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -710,12 +710,12 @@ MulticopterPositionControl::control_manual(float dt) /* check for pos. hold */ if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) { - if (!_pos_hold_engaged && (_params.hold_max_xy < FLT_EPSILON || - (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy))) - { - _pos_hold_engaged = true; - } else { - _pos_hold_engaged = false; + if (!_pos_hold_engaged) { + if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy)) { + _pos_hold_engaged = true; + } else { + _pos_hold_engaged = false; + } } } else { From 484bd3bd89e2e8429e5c53b63dc55fea9adaea03 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 18:48:53 +0100 Subject: [PATCH 099/161] Let commander be less verbose --- src/modules/commander/commander.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5cdd7cf4e4..79ea8dd893 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -3051,11 +3051,8 @@ void *commander_low_prio_loop(void *arg) if (need_param_autosave_timeout > 0 && hrt_elapsed_time(&need_param_autosave_timeout) > 200000ULL) { int ret = param_save_default(); - if (ret == OK) { - mavlink_and_console_log_info(mavlink_fd, "settings autosaved"); - - } else { - mavlink_and_console_log_critical(mavlink_fd, "settings save error"); + if (ret != OK) { + mavlink_and_console_log_critical(mavlink_fd, "settings auto save error"); } need_param_autosave = false; From c20e7f8424c40175c47abd14a2566dd8045eeba1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 08:57:30 +0100 Subject: [PATCH 100/161] Commander: Provide console output on autosave --- src/modules/commander/commander.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 79ea8dd893..1a70d0f6dd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -3053,6 +3053,8 @@ void *commander_low_prio_loop(void *arg) if (ret != OK) { mavlink_and_console_log_critical(mavlink_fd, "settings auto save error"); + } else { + warnx("settings saved."); } need_param_autosave = false; From 8a6e9d17bad1348682c8e6f727c4261f4530df07 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 00:02:53 +0100 Subject: [PATCH 101/161] MAVLink: Do a better job at receive rate limiting --- src/modules/mavlink/mavlink_main.cpp | 2 ++ src/modules/mavlink/mavlink_main.h | 2 ++ src/modules/mavlink/mavlink_receiver.cpp | 11 ++++++++--- 3 files changed, 12 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index a12133c021..0c017c5477 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -698,6 +698,8 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * } else { _is_usb_uart = true; + /* USB has no baudrate, but use a magic number for 'fast' */ + _baudrate = 2000000; } #if defined (__PX4_LINUX) || defined (__PX4_DARWIN) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index ef14562ccc..95e0fdb91a 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -288,6 +288,8 @@ public: float get_rate_mult(); + float get_baudrate() { return _baudrate; } + /* Functions for waiting to start transmission until message received. */ void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } bool get_has_received_messages() { return _received_messages; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 5f36354191..29f299f918 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1757,7 +1757,7 @@ MavlinkReceiver::receive_thread(void *arg) uint8_t buf[1600]; #else /* the serial port buffers internally as well, we just need to fit a small chunk */ - uint8_t buf[32]; + uint8_t buf[64]; #endif mavlink_message_t msg; @@ -1799,8 +1799,13 @@ MavlinkReceiver::receive_thread(void *arg) if (_mavlink->get_protocol() == SERIAL) { /* non-blocking read. read may return negative values */ if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) { - /* to avoid reading very small chunks wait for data before reading */ - usleep(1000); + /* + * to avoid reading very small chunks wait for data before reading + * this is designed to target ~30 bytes at a time + */ + const unsigned character_count = 20; + unsigned sleeptime = (1.0f / (_mavlink->get_baudrate() / 10)) * character_count * 1000000; + usleep(sleeptime); } } #ifdef __PX4_POSIX From c934286f6c6f22cc0c7ee21d633e0ef3531eeb5f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 00:16:38 +0100 Subject: [PATCH 102/161] MAVLink app: Do not try to fill the complete buffer, just try to get a complete message --- src/modules/mavlink/mavlink_receiver.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 29f299f918..c4f1cbceec 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1797,13 +1797,15 @@ MavlinkReceiver::receive_thread(void *arg) while (!_mavlink->_task_should_exit) { if (poll(&fds[0], 1, timeout) > 0) { if (_mavlink->get_protocol() == SERIAL) { + + /* + * to avoid reading very small chunks wait for data before reading + * this is designed to target ~30 bytes at a time + */ + const unsigned character_count = 20; + /* non-blocking read. read may return negative values */ - if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) { - /* - * to avoid reading very small chunks wait for data before reading - * this is designed to target ~30 bytes at a time - */ - const unsigned character_count = 20; + if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)character_count) { unsigned sleeptime = (1.0f / (_mavlink->get_baudrate() / 10)) * character_count * 1000000; usleep(sleeptime); } From 9647f0622c22b9b1dd3603970a75bb3006a056e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 00:17:44 +0100 Subject: [PATCH 103/161] MAVLink: Comment lied --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c4f1cbceec..0c2180a4d8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1800,7 +1800,7 @@ MavlinkReceiver::receive_thread(void *arg) /* * to avoid reading very small chunks wait for data before reading - * this is designed to target ~30 bytes at a time + * this is designed to target one message, so >20 bytes at a time */ const unsigned character_count = 20; From 9b5b2a4cccb092bcb42ae2a5025ce21a3def4d60 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 3 Nov 2015 16:47:09 +0100 Subject: [PATCH 104/161] use correct Unix line ending character Conflicts: Tools/px_romfs_pruner.py --- Tools/px_romfs_pruner.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index 1fa1efbd90..a7984c66dd 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -40,7 +40,7 @@ Delete all comments and newlines before ROMFS is converted to an image """ from __future__ import print_function -import argparse +import argparse, re import os @@ -64,7 +64,7 @@ def main(): # read file line by line pruned_content = "" - with open(file_path, "r") as f: + with open(file_path, "rU") as f: for line in f: # handle mixer files differently than startup files @@ -76,6 +76,7 @@ def main(): pruned_content += line # overwrite old scratch file with open(file_path, "wb") as f: + pruned_content = re.sub("\r\n", "\n", pruned_content) f.write(pruned_content.encode("ascii", errors='strict')) From 4076383c4989db17988bc77c9ec5f414f2c77090 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 22:53:03 +0100 Subject: [PATCH 105/161] Commander: Fix PX4_INFO call --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1a70d0f6dd..67ce676df5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -403,11 +403,11 @@ int commander_main(int argc, char *argv[]) void usage(const char *reason) { - if (reason) { - PX4_INFO("%s\n", reason); + if (reason && *reason > 0) { + PX4_INFO("%s", reason); } - PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm}\n\n"); + PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm}\n"); } void print_status() From f4741f8bf1e107160bc0ce531942465b75e25d18 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Nov 2015 10:46:03 +0100 Subject: [PATCH 106/161] EKF: Fix output --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 2ea78ddf14..8080fd9c09 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1672,8 +1672,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) PX4_INFO("."); } - PX4_INFO(" "); - return 0; } From f776c57fb27a7ad06325e111bd27b4fcdb1c9a73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Nov 2015 10:46:18 +0100 Subject: [PATCH 107/161] INAV: Clean up output --- .../position_estimator_inav/position_estimator_inav_main.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 04f3c4d918..aa7fe0d972 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -116,11 +116,11 @@ static inline int max(int val1, int val2) */ static void usage(const char *reason) { - if (reason) { - PX4_INFO("%s\n", reason); + if (reason && *reason) { + PX4_INFO("%s", reason); } - PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n\n"); + PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n"); return; } From ad4d841b8afede6f5f65e1ea3c8986b1b57d9ddf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Nov 2015 10:46:29 +0100 Subject: [PATCH 108/161] Mixer command: Clean up output --- src/systemcmds/mixer/mixer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index 56419714b5..c83eabc9a7 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -93,12 +93,12 @@ mixer_main(int argc, char *argv[]) static void usage(const char *reason) { - if (reason) { - PX4_INFO("%s\n", reason); + if (reason && *reason) { + PX4_INFO("%s", reason); } - PX4_INFO("usage:\n"); - PX4_INFO(" mixer load \n"); + PX4_INFO("usage:"); + PX4_INFO(" mixer load "); } static int From 50c6be5d25bf61517cda27604caf7bd388f8ed52 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Nov 2015 11:09:09 +0100 Subject: [PATCH 109/161] SDLOG2: Comment lied --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 94719380f1..d68c90cf98 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1898,7 +1898,7 @@ void sdlog2_status() } /** - * @return 0 if file exists + * @return true if file exists */ bool file_exist(const char *filename) { From 7be83bd46e10d66e75438cbb35e4fb9825c343ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Nov 2015 13:15:26 +0100 Subject: [PATCH 110/161] Auto-upload config files --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 35c9aa1c28..e6b5239344 100644 --- a/.travis.yml +++ b/.travis.yml @@ -118,7 +118,7 @@ after_success: cp build_px4fmu-v1_default/src/firmware/nuttx/nuttx-px4fmu-v1-default.px4 px4fmu-v1_default.px4 && cp build_px4fmu-v2_default/src/firmware/nuttx/nuttx-px4fmu-v2-default.px4 px4fmu-v2_default.px4 && zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4 - && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ + && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ && ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/ && ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/ && ./CI-Tools/s3cmd-put CI-Tools/index.html index.html From 2727333b3d50bd93a9f2e23b8ebe50c3d8ca235f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Nov 2015 18:23:09 +0100 Subject: [PATCH 111/161] Telem status: Add USB type --- msg/telemetry_status.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg index 221dab79e0..cdcef931af 100644 --- a/msg/telemetry_status.msg +++ b/msg/telemetry_status.msg @@ -2,6 +2,7 @@ uint8 TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0 uint8 TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO = 1 uint8 TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET = 2 uint8 TELEMETRY_STATUS_RADIO_TYPE_WIRE = 3 +uint8 TELEMETRY_STATUS_RADIO_TYPE_USB = 4 uint64 timestamp uint64 heartbeat_time # Time of last received heartbeat from remote system From 0fe2bb588672a6de33c864a4807856ae7ffec846 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Nov 2015 18:23:19 +0100 Subject: [PATCH 112/161] Commander: Read USB status --- src/modules/commander/commander.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 67ce676df5..3573f5d6eb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -180,6 +180,7 @@ static volatile bool thread_should_exit = false; /**< daemon exit flag */ static volatile bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ static bool need_param_autosave = false; /**< Flag set to true if parameters should be autosaved in next iteration (happens on param update and if functionality is enabled) */ +static bool _usb_telemetry_active = false; static hrt_abstime commander_boot_timestamp = 0; static unsigned int leds_counter; @@ -200,7 +201,7 @@ static struct home_position_s _home; static unsigned _last_mission_instance = 0; static manual_control_setpoint_s _last_sp_man; -struct vtol_vehicle_status_s vtol_status; +static struct vtol_vehicle_status_s vtol_status; /** * The daemon app only briefly exists to start @@ -1463,6 +1464,11 @@ int commander_thread_main(int argc, char *argv[]) } } + /* set (and don't reset) telemetry via USB as active once a MAVLink connection is up */ + if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB) { + _usb_telemetry_active = true; + } + telemetry_last_heartbeat[i] = telemetry.heartbeat_time; } } @@ -1519,18 +1525,19 @@ int commander_thread_main(int argc, char *argv[]) /* copy avionics voltage */ status.avionics_power_rail_voltage = system_power.voltage5V_v; + /* if the USB hardware connection went away, reboot */ if (status.usb_connected && !system_power.usb_connected) { /* * apparently the USB cable went away but we are still powered, * so lets reset to a classic non-usb state. */ - usleep(100000); mavlink_log_critical(mavlink_fd, "USB disconnected, rebooting.") usleep(400000); px4_systemreset(false); } - status.usb_connected = system_power.usb_connected; + /* finally judge the USB connected state based on software detection */ + status.usb_connected = _usb_telemetry_active; } } From 85ad5a622a0ad4e6accf566ed4ea9357b1287393 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Nov 2015 18:23:30 +0100 Subject: [PATCH 113/161] MAVLink: Send USB telem status --- src/modules/mavlink/mavlink_main.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0c017c5477..3a2f1d583b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -700,6 +700,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * _is_usb_uart = true; /* USB has no baudrate, but use a magic number for 'fast' */ _baudrate = 2000000; + _rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB; } #if defined (__PX4_LINUX) || defined (__PX4_DARWIN) @@ -2096,8 +2097,12 @@ Mavlink::display_status() printf("3DR RADIO\n"); break; + case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB: + printf("USB CDC\n"); + break; + default: - printf("UNKNOWN RADIO\n"); + printf("GENERIC LINK OR RADIO\n"); break; } From f6a9647fe61167e77af955ec5d8756253e78f8db Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Fri, 30 Oct 2015 17:52:17 +0530 Subject: [PATCH 114/161] attitude_estimator_q : add external heading support --- .../attitude_estimator_q_main.cpp | 98 +++++++++++++++++-- .../attitude_estimator_q_params.c | 20 ++++ 2 files changed, 112 insertions(+), 6 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index a9ffbe0c42..7af8fa3954 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -56,6 +56,8 @@ #include #include #include +#include +#include #include #include @@ -117,22 +119,27 @@ private: int _sensors_sub = -1; int _params_sub = -1; + int _vision_sub = -1; + int _mocap_sub = -1; int _global_pos_sub = -1; orb_advert_t _att_pub = nullptr; struct { param_t w_acc; param_t w_mag; + param_t w_ext_hdg; param_t w_gyro_bias; param_t mag_decl; param_t mag_decl_auto; param_t acc_comp; param_t bias_max; param_t vibe_thresh; + param_t ext_hdg_mode; } _params_handles; /**< handles for interesting parameters */ float _w_accel = 0.0f; float _w_mag = 0.0f; + float _w_ext_hdg = 0.0f; float _w_gyro_bias = 0.0f; float _mag_decl = 0.0f; bool _mag_decl_auto = false; @@ -140,11 +147,18 @@ private: float _bias_max = 0.0f; float _vibration_warning_threshold = 1.0f; hrt_abstime _vibration_warning_timestamp = 0; + int _ext_hdg_mode = 0; Vector<3> _gyro; Vector<3> _accel; Vector<3> _mag; + vision_position_estimate_s _vision = {}; + Vector<3> _vision_hdg; + + att_pos_mocap_s _mocap = {}; + Vector<3> _mocap_hdg; + Quaternion _q; Vector<3> _rates; Vector<3> _gyro_bias; @@ -168,6 +182,7 @@ private: bool _data_good = false; bool _failsafe = false; bool _vibration_warning = false; + bool _ext_hdg_good = false; int _mavlink_fd = -1; @@ -196,12 +211,14 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() : _params_handles.w_acc = param_find("ATT_W_ACC"); _params_handles.w_mag = param_find("ATT_W_MAG"); + _params_handles.w_ext_hdg = param_find("ATT_W_EXT_HDG"); _params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS"); _params_handles.mag_decl = param_find("ATT_MAG_DECL"); _params_handles.mag_decl_auto = param_find("ATT_MAG_DECL_A"); _params_handles.acc_comp = param_find("ATT_ACC_COMP"); _params_handles.bias_max = param_find("ATT_BIAS_MAX"); _params_handles.vibe_thresh = param_find("ATT_VIBE_THRESH"); + _params_handles.ext_hdg_mode = param_find("ATT_EXT_HDG_M"); } /** @@ -269,6 +286,10 @@ void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) void AttitudeEstimatorQ::task_main() { _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); + + _vision_sub = orb_subscribe(ORB_ID(vision_position_estimate)); + _mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -374,6 +395,47 @@ void AttitudeEstimatorQ::task_main() } } + // Update vision and motion capture heading + bool vision_updated = false; + orb_check(_vision_sub, &vision_updated); + + bool mocap_updated = false; + orb_check(_mocap_sub, &mocap_updated); + + if (vision_updated) { + orb_copy(ORB_ID(vision_position_estimate), _vision_sub, &_vision); + math::Quaternion q(_vision.q); + + math::Matrix<3, 3> Rvis = q.to_dcm(); + math::Vector<3> v(1.0f, 0.0f, 0.4f); + + // Rvis is Rwr (robot respect to world) while v is respect to world. + // Hence Rvis must be transposed having (Rwr)' * Vw + // Rrw * Vw = vn. This way we have consistency + _vision_hdg = Rvis.transposed() * v; + } + + if (mocap_updated) { + orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap); + math::Quaternion q(_mocap.q); + math::Matrix<3, 3> Rmoc = q.to_dcm(); + + math::Vector<3> v(1.0f, 0.0f, 0.4f); + + // Rmoc is Rwr (robot respect to world) while v is respect to world. + // Hence Rmoc must be transposed having (Rwr)' * Vw + // Rrw * Vw = vn. This way we have consistency + _mocap_hdg = Rmoc.transposed() * v; + } + + // Check for timeouts on data + if (_ext_hdg_mode == 1) { + _ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000); + + } else if (_ext_hdg_mode == 2) { + _ext_hdg_good = _mocap.timestamp_boot > 0 && (hrt_elapsed_time(&_mocap.timestamp_boot) < 500000); + } + bool gpos_updated; orb_check(_global_pos_sub, &gpos_updated); @@ -478,6 +540,7 @@ void AttitudeEstimatorQ::update_parameters(bool force) param_get(_params_handles.w_acc, &_w_accel); param_get(_params_handles.w_mag, &_w_mag); + param_get(_params_handles.w_ext_hdg, &_w_ext_hdg); param_get(_params_handles.w_gyro_bias, &_w_gyro_bias); float mag_decl_deg = 0.0f; param_get(_params_handles.mag_decl, &mag_decl_deg); @@ -490,6 +553,7 @@ void AttitudeEstimatorQ::update_parameters(bool force) _acc_comp = acc_comp_int != 0; param_get(_params_handles.bias_max, &_bias_max); param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold); + param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode); } } @@ -545,12 +609,34 @@ bool AttitudeEstimatorQ::update(float dt) // Angular rate of correction Vector<3> corr; - // Magnetometer correction - // Project mag field vector to global frame and extract XY component - Vector<3> mag_earth = _q.conjugate(_mag); - float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl); - // Project magnetometer correction to body frame - corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag; + if (_ext_hdg_mode > 0 && _ext_hdg_good) { + if (_ext_hdg_mode == 1) { + // Vision heading correction + // Project heading to global frame and extract XY component + Vector<3> vision_hdg_earth = _q.conjugate(_vision_hdg); + float vision_hdg_err = _wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0))); + // Project correction to body frame + corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -vision_hdg_err)) * _w_ext_hdg; + } + + if (_ext_hdg_mode == 2) { + // Mocap heading correction + // Project heading to global frame and extract XY component + Vector<3> mocap_hdg_earth = _q.conjugate(_mocap_hdg); + float mocap_hdg_err = _wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0))); + // Project correction to body frame + corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mocap_hdg_err)) * _w_ext_hdg; + } + } + + if (_ext_hdg_mode == 0 || !_ext_hdg_good) { + // Magnetometer correction + // Project mag field vector to global frame and extract XY component + Vector<3> mag_earth = _q.conjugate(_mag); + float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl); + // Project magnetometer correction to body frame + corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag; + } // Accelerometer correction // Project 'k' unit vector of earth frame to body frame diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c index d3959db4cf..41b62f04e1 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c @@ -61,6 +61,15 @@ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f); */ PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f); +/** + * Complimentary filter external heading weight + * + * @group Attitude Q estimator + * @min 0 + * @max 1 + */ +PARAM_DEFINE_FLOAT(ATT_W_EXT_HDG, 0.1f); + /** * Complimentary filter gyroscope bias weight * @@ -93,6 +102,17 @@ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); */ PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1); +/** + * External heading usage mode (from Motion capture/Vision) + * Set to 1 to use heading estimate from vision. + * Set to 2 to use heading from motion capture. + * + * @group Attitude Q estimator + * @min 0 + * @max 2 + */ +PARAM_DEFINE_INT32(ATT_EXT_HDG_M, 0); + /** * Enable acceleration compensation based on GPS * velocity. From 87269c0fab820f37fd1b8981f8b32ec0fbdc46db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Nov 2015 21:29:00 +0100 Subject: [PATCH 115/161] Update autostart docs --- ROMFS/px4fmu_common/init.d/11001_hexa_cox | 11 +++++++++++ ROMFS/px4fmu_common/init.d/3033_wingwing | 5 ++++- 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox index f85b06f87d..8c9c28fe37 100644 --- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -4,6 +4,17 @@ # # @type Hexarotor Coaxial # +# @output MAIN1 front right top, CW; angle:60; direction:CW +# @output MAIN2 front right bottom, CCW; angle:60; direction:CCW +# @output MAIN3 back top, CW; angle:180; direction:CW +# @output MAIN4 back bottom, CCW; angle:180; direction:CCW +# @output MAIN5 front left top, CW; angle:-60; direction:CW +# @output MAIN6 front left bottom, CCW;angle:-60; direction:CCW +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# # @maintainer Lorenz Meier # diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 25a14312f3..3a0aa28150 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -14,7 +14,7 @@ # @output AUX2 feed-through of RC AUX2 channel # @output AUX3 feed-through of RC AUX3 channel # -# @maintainer Simon Wilks +# @maintainer Lorenz Meier # sh /etc/init.d/rc.fw_defaults @@ -39,6 +39,9 @@ then param set FW_RR_P 0.04 fi +# Configure this as plane +set MAV_TYPE 1 +# Set mixer set MIXER wingwing # Provide ESC a constant 1000 us pulse set PWM_OUT 4 From 9b1fefb8d0044e63bb7c0849485d8d49cb46ed10 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Nov 2015 21:29:17 +0100 Subject: [PATCH 116/161] Remove unused build file --- src/modules/systemlib/mixer/multi_tables.mk | 42 --------------------- 1 file changed, 42 deletions(-) delete mode 100644 src/modules/systemlib/mixer/multi_tables.mk diff --git a/src/modules/systemlib/mixer/multi_tables.mk b/src/modules/systemlib/mixer/multi_tables.mk deleted file mode 100644 index c537c83a47..0000000000 --- a/src/modules/systemlib/mixer/multi_tables.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2014 Anton Matosov . 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. -# -############################################################################ - - -SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST))) -MULTI_TABLES := $(SELF_DIR)multi_tables.py - -# Add explicit dependency, as implicit one doesn't work often. -$(SELF_DIR)mixer_multirotor.cpp : $(SELF_DIR)mixer_multirotor.generated.h - -$(SELF_DIR)mixer_multirotor.generated.h : $(MULTI_TABLES) - $(Q) $(PYTHON) $(MULTI_TABLES) > $(SELF_DIR)mixer_multirotor.generated.h From 628e5c8649150ce6bf399a0a0aba81da5895e612 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Nov 2015 21:29:44 +0100 Subject: [PATCH 117/161] Mixer: Fix naming inconsistency --- src/modules/systemlib/mixer/mixer.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 43c815238c..5ed152127a 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -481,7 +481,7 @@ public: * compared to thrust. * @param yaw_wcale Scaling factor applied to yaw inputs compared * to thrust. - * @param deadband Minumum rotor control output value; usually + * @param idle_speed Minumum rotor control output value; usually * tuned to ensure that rotors never stall at the * low end of their control range. */ @@ -491,7 +491,7 @@ public: float roll_scale, float pitch_scale, float yaw_scale, - float deadband); + float idle_speed); ~MultirotorMixer(); /** From ca7a7dfd102893621edd8bfc4992c72dbac140ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Nov 2015 21:30:10 +0100 Subject: [PATCH 118/161] Output rotor config as XML attributes --- Tools/px4airframes/xmlout.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/Tools/px4airframes/xmlout.py b/Tools/px4airframes/xmlout.py index 427b1090a2..994fb6eeb2 100644 --- a/Tools/px4airframes/xmlout.py +++ b/Tools/px4airframes/xmlout.py @@ -72,9 +72,13 @@ class XMLOutput(): xml_field.text = value for code in param.GetOutputCodes(): value = param.GetOutputValue(code) + valstrs = value.split(";") xml_field = ET.SubElement(xml_param, "output") xml_field.attrib["name"] = code - xml_field.text = value + for attrib in valstrs[1:]: + attribstrs = attrib.split(":") + xml_field.attrib[attribstrs[0].strip()] = attribstrs[1].strip() + xml_field.text = valstrs[0] if last_param_name != param.GetName(): board_specific_param_set = False indent(xml_parameters) From 27df787bfffbc5858e2fd3e41491e293521c8683 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Thu, 5 Nov 2015 19:29:04 -0500 Subject: [PATCH 119/161] Separated matrix lib into own repo. --- .gitmodules | 3 + CMakeLists.txt | 1 + cmake/common/px4_base.cmake | 2 +- cmake/ros-CMakeLists.txt | 1 + src/lib/mathlib/math/Matrix.hpp | 2 +- src/lib/matrix | 1 + .../BlockLocalPositionEstimator.cpp | 48 +- .../BlockLocalPositionEstimator.hpp | 6 +- .../local_position_estimator/CMakeLists.txt | 2 - .../matrix/.gitignore | 1 - .../matrix/CMakeLists.txt | 13 - .../matrix/src/Matrix.hpp | 464 ------------------ .../matrix/test/CMakeLists.txt | 15 - .../matrix/test/inverse.cpp | 30 -- .../matrix/test/matrixAssignment.cpp | 29 -- .../matrix/test/matrixMult.cpp | 26 - .../matrix/test/matrixScalarMult.cpp | 18 - .../matrix/test/setIdentity.cpp | 25 - .../matrix/test/transpose.cpp | 18 - .../matrix/test/vectorAssignment.cpp | 28 -- src/platforms/common/CMakeLists.txt | 1 + 21 files changed, 36 insertions(+), 698 deletions(-) create mode 160000 src/lib/matrix delete mode 100644 src/modules/local_position_estimator/matrix/.gitignore delete mode 100644 src/modules/local_position_estimator/matrix/CMakeLists.txt delete mode 100644 src/modules/local_position_estimator/matrix/src/Matrix.hpp delete mode 100644 src/modules/local_position_estimator/matrix/test/CMakeLists.txt delete mode 100644 src/modules/local_position_estimator/matrix/test/inverse.cpp delete mode 100644 src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp delete mode 100644 src/modules/local_position_estimator/matrix/test/matrixMult.cpp delete mode 100644 src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp delete mode 100644 src/modules/local_position_estimator/matrix/test/setIdentity.cpp delete mode 100644 src/modules/local_position_estimator/matrix/test/transpose.cpp delete mode 100644 src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp diff --git a/.gitmodules b/.gitmodules index 178cf86771..2bcbb90147 100644 --- a/.gitmodules +++ b/.gitmodules @@ -25,3 +25,6 @@ [submodule "unittests/googletest"] path = unittests/googletest url = https://github.com/google/googletest.git +[submodule "src/lib/matrix"] + path = src/lib/matrix + url = https://github.com/dronecrew/matrix.git diff --git a/CMakeLists.txt b/CMakeLists.txt index 05a3cd59b3..6b395b6261 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -232,6 +232,7 @@ px4_add_git_submodule(TARGET git_nuttx PATH "NuttX") px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal") px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim") px4_add_git_submodule(TARGET git_gazebo PATH "Tools/sitl_gazebo") +px4_add_git_submodule(TARGET git_matrix PATH "src/lib/matrix") add_custom_target(submodule_clean WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index eb3b9c68fc..e6ff9123d6 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -622,7 +622,7 @@ function(px4_add_common_flags) ) list(APPEND added_include_dirs - src/lib/eigen + src/lib/matrix ) set(added_link_dirs) # none used currently diff --git a/cmake/ros-CMakeLists.txt b/cmake/ros-CMakeLists.txt index aa42fb6ff4..695f625d6d 100644 --- a/cmake/ros-CMakeLists.txt +++ b/cmake/ros-CMakeLists.txt @@ -135,6 +135,7 @@ include_directories( ${CMAKE_BINARY_DIR}/src/modules src/ src/lib + src/lib/matrix ${EIGEN_INCLUDE_DIRS} integrationtests ) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 38322e56a7..3491d8bf0f 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -49,7 +49,7 @@ #ifdef CONFIG_ARCH_ARM #include "../CMSIS/Include/arm_math.h" #else -#include "modules/local_position_estimator/matrix/src/Matrix.hpp" +#include "matrix/matrix.hpp" #endif #include diff --git a/src/lib/matrix b/src/lib/matrix new file mode 160000 index 0000000000..7abbdcd88f --- /dev/null +++ b/src/lib/matrix @@ -0,0 +1 @@ +Subproject commit 7abbdcd88fffb44d081e9a212c7d655b6ff5e9ca diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index f3295e1fc9..d1262f9177 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -811,7 +811,7 @@ void BlockLocalPositionEstimator::predict() Q(X_bz, X_bz) = _pn_b_noise_power.get(); // continuous time kalman filter prediction - Matrix dx = (A * _x + B * _u) * getDt(); + Vector dx = (A * _x + B * _u) * getDt(); // only predict for components we have // valid measurements for @@ -904,16 +904,16 @@ void BlockLocalPositionEstimator::correctFlow() _flowY += global_speed[1] * dt; // measurement - Vector2f y; + Vector y; y(0) = _flowX; y(1) = _flowY; // residual - Vector2f r = y - C * _x; + Vector r = y - C * _x; // residual covariance, (inverse) Matrix S_I = - (C * _P * C.transpose() + R).inverse(); + inv(C * _P * C.transpose() + R); // fault detection float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); @@ -981,17 +981,17 @@ void BlockLocalPositionEstimator::correctSonar() } // measurement - Matrix y; + Vector y; y(0) = (d - _sonarAltHome) * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch); // residual - Matrix r = y - C * _x; + Vector r = y - C * _x; // residual covariance, (inverse) Matrix S_I = - (C * _P * C.transpose() + R).inverse(); + inv(C * _P * C.transpose() + R); // fault detection float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); @@ -1032,7 +1032,7 @@ void BlockLocalPositionEstimator::correctSonar() void BlockLocalPositionEstimator::correctBaro() { - Matrix y; + Vector y; y(0) = _sub_sensor.get().baro_alt_meter[0] - _baroAltHome; // baro measurement matrix @@ -1046,8 +1046,8 @@ void BlockLocalPositionEstimator::correctBaro() // residual Matrix S_I = - ((C * _P * C.transpose()) + R).inverse(); - Matrix r = y - (C * _x); + inv((C * _P * C.transpose()) + R); + Vector r = y - (C * _x); // fault detection float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); @@ -1060,7 +1060,7 @@ void BlockLocalPositionEstimator::correctBaro() } // lower baro trust - S_I = ((C * _P * C.transpose()) + R * 10).inverse(); + S_I = inv((C * _P * C.transpose()) + R * 10); } else if (_baroFault) { _baroFault = FAULT_NONE; @@ -1104,15 +1104,15 @@ void BlockLocalPositionEstimator::correctLidar() R(0, 0) = cov; } - Matrix y; + Vector y; y.setZero(); y(0) = (d - _lidarAltHome) * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch); // residual - Matrix S_I = ((C * _P * C.transpose()) + R).inverse(); - Matrix r = y - C * _x; + Matrix S_I = inv((C * _P * C.transpose()) + R); + Vector r = y - C * _x; // fault detection float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); @@ -1166,7 +1166,7 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met //printf("home: lat %10g, lon, %10g alt %10g\n", _sub_home.lat, _sub_home.lon, double(_sub_home.alt)); //printf("local: x %10g y %10g z %10g\n", double(px), double(py), double(pz)); - Matrix y; + Vector y; y.setZero(); y(0) = px; y(1) = py; @@ -1213,8 +1213,8 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met R(5, 5) = var_vz; // residual - Matrix r = y - C * _x; - Matrix S_I = (C * _P * C.transpose() + R).inverse(); + Vector r = y - C * _x; + Matrix S_I = inv(C * _P * C.transpose() + R); // fault detection float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); @@ -1245,7 +1245,7 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met } // trust GPS less - S_I = ((C * _P * C.transpose()) + R * 10).inverse(); + S_I = inv((C * _P * C.transpose()) + R * 10); } else if (_gpsFault) { _gpsFault = FAULT_NONE; @@ -1266,7 +1266,7 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met void BlockLocalPositionEstimator::correctVision() { - Matrix y; + Vector y; y.setZero(); y(0) = _sub_vision_pos.get().x - _visionHome(0); y(1) = _sub_vision_pos.get().y - _visionHome(1); @@ -1287,7 +1287,7 @@ void BlockLocalPositionEstimator::correctVision() R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get(); // residual - Matrix S_I = ((C * _P * C.transpose()) + R).inverse(); + Matrix S_I = inv((C * _P * C.transpose()) + R); Matrix r = y - C * _x; // fault detection @@ -1301,7 +1301,7 @@ void BlockLocalPositionEstimator::correctVision() } // trust less - S_I = ((C * _P * C.transpose()) + R * 10).inverse(); + S_I = inv((C * _P * C.transpose()) + R * 10); } else if (_visionFault) { _visionFault = FAULT_NONE; @@ -1322,7 +1322,7 @@ void BlockLocalPositionEstimator::correctVision() void BlockLocalPositionEstimator::correctmocap() { - Matrix y; + Vector y; y.setZero(); y(Y_mocap_x) = _sub_mocap.get().x - _mocapHome(0); y(Y_mocap_y) = _sub_mocap.get().y - _mocapHome(1); @@ -1345,7 +1345,7 @@ void BlockLocalPositionEstimator::correctmocap() R(Y_mocap_z, Y_mocap_z) = mocap_p_var; // residual - Matrix S_I = ((C * _P * C.transpose()) + R).inverse(); + Matrix S_I = inv((C * _P * C.transpose()) + R); Matrix r = y - C * _x; // fault detection @@ -1359,7 +1359,7 @@ void BlockLocalPositionEstimator::correctmocap() } // trust less - S_I = ((C * _P * C.transpose()) + R * 10).inverse(); + S_I = inv((C * _P * C.transpose()) + R * 10); } else if (_mocapFault) { _mocapFault = FAULT_NONE; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 66f301efe3..1c6c43fd20 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -6,7 +6,7 @@ #include #ifdef USE_MATRIX_LIB -#include "matrix/src/Matrix.hpp" +#include "matrix/Matrix.hpp" using namespace matrix; #else #include @@ -304,7 +304,7 @@ private: perf_counter_t _err_perf; // state space - Matrix _x; // state vector - Matrix _u; // input vector + Vector _x; // state vector + Vector _u; // input vector Matrix _P; // state covariance matrix }; diff --git a/src/modules/local_position_estimator/CMakeLists.txt b/src/modules/local_position_estimator/CMakeLists.txt index e0d1be2d0a..761a0d94b0 100644 --- a/src/modules/local_position_estimator/CMakeLists.txt +++ b/src/modules/local_position_estimator/CMakeLists.txt @@ -34,8 +34,6 @@ if(${OS} STREQUAL "nuttx") list(APPEND MODULE_CFLAGS -Wframe-larger-than=6500) elseif(${OS} STREQUAL "posix") list(APPEND MODULE_CFLAGS -Wno-error) - # add matrix tests - add_subdirectory(matrix) endif() # use custom matrix lib instead of Eigen diff --git a/src/modules/local_position_estimator/matrix/.gitignore b/src/modules/local_position_estimator/matrix/.gitignore deleted file mode 100644 index a5309e6b90..0000000000 --- a/src/modules/local_position_estimator/matrix/.gitignore +++ /dev/null @@ -1 +0,0 @@ -build*/ diff --git a/src/modules/local_position_estimator/matrix/CMakeLists.txt b/src/modules/local_position_estimator/matrix/CMakeLists.txt deleted file mode 100644 index 5a16ced707..0000000000 --- a/src/modules/local_position_estimator/matrix/CMakeLists.txt +++ /dev/null @@ -1,13 +0,0 @@ -cmake_minimum_required(VERSION 2.8) - -project(matrix CXX) - -set(CMAKE_BUILD_TYPE "RelWithDebInfo") - -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Weffc++") - -enable_testing() - -include_directories(src) - -add_subdirectory(test) diff --git a/src/modules/local_position_estimator/matrix/src/Matrix.hpp b/src/modules/local_position_estimator/matrix/src/Matrix.hpp deleted file mode 100644 index ae1111894c..0000000000 --- a/src/modules/local_position_estimator/matrix/src/Matrix.hpp +++ /dev/null @@ -1,464 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -namespace matrix -{ - -template -class Matrix -{ - -private: - T _data[M][N]; - size_t _rows; - size_t _cols; - -public: - - Matrix() : - _data(), - _rows(M), - _cols(N) - { - } - - Matrix(const T *data) : - _data(), - _rows(M), - _cols(N) - { - memcpy(_data, data, sizeof(_data)); - } - - Matrix(const Matrix &other) : - _data(), - _rows(M), - _cols(N) - { - memcpy(_data, other._data, sizeof(_data)); - } - - Matrix(T x, T y, T z) : - _data(), - _rows(M), - _cols(N) - { - // TODO, limit to only 3x1 matrices - _data[0][0] = x; - _data[1][0] = y; - _data[2][0] = z; - } - - /** - * Accessors/ Assignment etc. - */ - - T *data() - { - return _data[0]; - } - - inline size_t rows() const - { - return _rows; - } - - inline size_t cols() const - { - return _rows; - } - - inline T operator()(size_t i) const - { - return _data[i][0]; - } - - inline T operator()(size_t i, size_t j) const - { - return _data[i][j]; - } - - inline T &operator()(size_t i) - { - return _data[i][0]; - } - - inline T &operator()(size_t i, size_t j) - { - return _data[i][j]; - } - - /** - * Matrix Operations - */ - - // this might use a lot of programming memory - // since it instantiates a class for every - // required mult pair, but it provides - // compile time size_t checking - template - Matrix operator*(const Matrix &other) const - { - const Matrix &self = *this; - Matrix res; - res.setZero(); - - for (size_t i = 0; i < M; i++) { - for (size_t k = 0; k < P; k++) { - for (size_t j = 0; j < N; j++) { - res(i, k) += self(i, j) * other(j, k); - } - } - } - - return res; - } - - Matrix operator+(const Matrix &other) const - { - Matrix res; - const Matrix &self = *this; - - for (size_t i = 0; i < M; i++) { - for (size_t j = 0; j < N; j++) { - res(i , j) = self(i, j) + other(i, j); - } - } - - return res; - } - - bool operator==(const Matrix &other) const - { - Matrix res; - const Matrix &self = *this; - - for (size_t i = 0; i < M; i++) { - for (size_t j = 0; j < N; j++) { - if (self(i , j) != other(i, j)) { - return false; - } - } - } - - return true; - } - - Matrix operator-(const Matrix &other) const - { - Matrix res; - const Matrix &self = *this; - - for (size_t i = 0; i < M; i++) { - for (size_t j = 0; j < N; j++) { - res(i , j) = self(i, j) - other(i, j); - } - } - - return res; - } - - void operator+=(const Matrix &other) - { - Matrix &self = *this; - self = self + other; - } - - void operator-=(const Matrix &other) - { - Matrix &self = *this; - self = self - other; - } - - void operator*=(const Matrix &other) - { - Matrix &self = *this; - self = self * other; - } - - /** - * Scalar Operations - */ - - Matrix operator*(T scalar) const - { - Matrix res; - const Matrix &self = *this; - - for (size_t i = 0; i < M; i++) { - for (size_t j = 0; j < N; j++) { - res(i , j) = self(i, j) * scalar; - } - } - - return res; - } - - Matrix operator+(T scalar) const - { - Matrix res; - Matrix &self = *this; - - for (size_t i = 0; i < M; i++) { - for (size_t j = 0; j < N; j++) { - res(i , j) = self(i, j) + scalar; - } - } - - return res; - } - - void operator*=(T scalar) - { - Matrix &self = *this; - - for (size_t i = 0; i < M; i++) { - for (size_t j = 0; j < N; j++) { - self(i, j) = self(i, j) * scalar; - } - } - } - - void operator/=(T scalar) - { - Matrix &self = *this; - self = self * (1.0f / scalar); - } - - /** - * Misc. Functions - */ - - void print() - { - Matrix &self = *this; - printf("\n"); - - for (size_t i = 0; i < M; i++) { - printf("["); - - for (size_t j = 0; j < N; j++) { - printf("%10g\t", double(self(i, j))); - } - - printf("]\n"); - } - } - - Matrix transpose() const - { - Matrix res; - const Matrix &self = *this; - - for (size_t i = 0; i < M; i++) { - for (size_t j = 0; j < N; j++) { - res(j, i) = self(i, j); - } - } - - return res; - } - - Matrix expm(float dt, size_t n) const - { - Matrix res; - res.setIdentity(); - Matrix A_pow = *this; - float k_fact = 1; - size_t k = 1; - - while (k < n) { - res += A_pow * (float(pow(dt, k)) / k_fact); - - if (k == n) { break; } - - A_pow *= A_pow; - k_fact *= k; - k++; - } - - return res; - } - - Matrix diagonal() const - { - Matrix res; - // force square for now - const Matrix &self = *this; - - for (size_t i = 0; i < M; i++) { - res(i) = self(i, i); - } - - return res; - } - - void setZero() - { - memset(_data, 0, sizeof(_data)); - } - - void setIdentity() - { - setZero(); - Matrix &self = *this; - - for (size_t i = 0; i < M and i < N; i++) { - self(i, i) = 1; - } - } - - inline void swapRows(size_t a, size_t b) - { - if (a == b) { return; } - - Matrix &self = *this; - - for (size_t j = 0; j < cols(); j++) { - T tmp = self(a, j); - self(a, j) = self(b, j); - self(b, j) = tmp; - } - } - - inline void swapCols(size_t a, size_t b) - { - if (a == b) { return; } - - Matrix &self = *this; - - for (size_t i = 0; i < rows(); i++) { - T tmp = self(i, a); - self(i, a) = self(i, b); - self(i, b) = tmp; - } - } - - /** - * inverse based on LU factorization with partial pivotting - */ - Matrix inverse() const - { - Matrix L; - L.setIdentity(); - const Matrix &A = (*this); - Matrix U = A; - Matrix P; - P.setIdentity(); - - //printf("A:\n"); A.print(); - - // for all diagonal elements - for (size_t n = 0; n < N; n++) { - - // if diagonal is zero, swap with row below - if (fabsf(U(n, n)) < 1e-8f) { - //printf("trying pivot for row %d\n",n); - for (size_t i = 0; i < N; i++) { - if (i == n) { continue; } - - //printf("\ttrying row %d\n",i); - if (fabsf(U(i, n)) > 1e-8f) { - //printf("swapped %d\n",i); - U.swapRows(i, n); - P.swapRows(i, n); - } - } - } - -#ifdef MATRIX_ASSERT - //printf("A:\n"); A.print(); - //printf("U:\n"); U.print(); - //printf("P:\n"); P.print(); - //fflush(stdout); - ASSERT(fabsf(U(n, n)) > 1e-8f); -#endif - - // failsafe, return zero matrix - if (fabsf(U(n, n)) < 1e-8f) { - Matrix zero; - zero.setZero(); - return zero; - } - - // for all rows below diagonal - for (size_t i = (n + 1); i < N; i++) { - L(i, n) = U(i, n) / U(n, n); - - // add i-th row and n-th row - // multiplied by: -a(i,n)/a(n,n) - for (size_t k = n; k < N; k++) { - U(i, k) -= L(i, n) * U(n, k); - } - } - } - - //printf("L:\n"); L.print(); - //printf("U:\n"); U.print(); - - // solve LY=P*I for Y by forward subst - Matrix Y = P; - - // for all columns of Y - for (size_t c = 0; c < N; c++) { - // for all rows of L - for (size_t i = 0; i < N; i++) { - // for all columns of L - for (size_t j = 0; j < i; j++) { - // for all existing y - // subtract the component they - // contribute to the solution - Y(i, c) -= L(i, j) * Y(j, c); - } - - // divide by the factor - // on current - // term to be solved - // Y(i,c) /= L(i,i); - // but L(i,i) = 1.0 - } - } - - //printf("Y:\n"); Y.print(); - - // solve Ux=y for x by back subst - Matrix X = Y; - - // for all columns of X - for (size_t c = 0; c < N; c++) { - // for all rows of U - for (size_t k = 0; k < N; k++) { - // have to go in reverse order - size_t i = N - 1 - k; - - // for all columns of U - for (size_t j = i + 1; j < N; j++) { - // for all existing x - // subtract the component they - // contribute to the solution - X(i, c) -= U(i, j) * X(j, c); - } - - // divide by the factor - // on current - // term to be solved - X(i, c) /= U(i, i); - } - } - - //printf("X:\n"); X.print(); - return X; - } - -}; - -typedef Matrix Vector2f; -typedef Matrix Vector3f; -typedef Matrix Matrix3f; - -}; // namespace matrix diff --git a/src/modules/local_position_estimator/matrix/test/CMakeLists.txt b/src/modules/local_position_estimator/matrix/test/CMakeLists.txt deleted file mode 100644 index cf280e287b..0000000000 --- a/src/modules/local_position_estimator/matrix/test/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -set(tests - setIdentity - inverse - matrixMult - vectorAssignment - matrixAssignment - matrixScalarMult - transpose - ) - -foreach(test ${tests}) - add_executable(${test} - ${test}.cpp) - add_test(${test} ${test}) -endforeach() diff --git a/src/modules/local_position_estimator/matrix/test/inverse.cpp b/src/modules/local_position_estimator/matrix/test/inverse.cpp deleted file mode 100644 index 73f7c0b432..0000000000 --- a/src/modules/local_position_estimator/matrix/test/inverse.cpp +++ /dev/null @@ -1,30 +0,0 @@ -#include "Matrix.hpp" -#include -#include - -using namespace matrix; - -int main() -{ - float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1}; - Matrix3f A(data); - Matrix3f A_I = A.inverse(); - float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1}; - Matrix3f A_I_check(data_check); - (void)A_I; - assert(A_I == A_I_check); - - // stess test - static const size_t n = 100; - Matrix A_large; - A_large.setIdentity(); - Matrix A_large_I; - A_large_I.setZero(); - - for (size_t i = 0; i < 100; i++) { - A_large_I = A_large.inverse(); - assert(A_large == A_large_I); - } - - return 0; -} diff --git a/src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp b/src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp deleted file mode 100644 index 5a625d0cc1..0000000000 --- a/src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp +++ /dev/null @@ -1,29 +0,0 @@ -#include "Matrix.hpp" -#include - -using namespace matrix; - -int main() -{ - Matrix3f m; - m.setZero(); - m(0, 0) = 1; - m(0, 1) = 2; - m(0, 2) = 3; - m(1, 0) = 4; - m(1, 1) = 5; - m(1, 2) = 6; - m(2, 0) = 7; - m(2, 1) = 8; - m(2, 2) = 9; - - m.print(); - - float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9}; - Matrix3f m2(data); - m2.print(); - - assert(m == m2); - - return 0; -} diff --git a/src/modules/local_position_estimator/matrix/test/matrixMult.cpp b/src/modules/local_position_estimator/matrix/test/matrixMult.cpp deleted file mode 100644 index 7b42d947d4..0000000000 --- a/src/modules/local_position_estimator/matrix/test/matrixMult.cpp +++ /dev/null @@ -1,26 +0,0 @@ -#include "Matrix.hpp" -#include -#include - -using namespace matrix; - -int main() -{ - float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1}; - Matrix3f A(data); - float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1}; - Matrix3f A_I(data_check); - Matrix3f I; - I.setIdentity(); - A.print(); - A_I.print(); - Matrix3f R = A * A_I; - R.print(); - assert(R == I); - - Matrix3f R2 = A; - R2 *= A_I; - R2.print(); - assert(R2 == I); - return 0; -} diff --git a/src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp b/src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp deleted file mode 100644 index 78bdb5b27f..0000000000 --- a/src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp +++ /dev/null @@ -1,18 +0,0 @@ -#include "Matrix.hpp" -#include -#include - -using namespace matrix; - -int main() -{ - float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9}; - Matrix3f A(data); - A = A * 2; - float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18}; - Matrix3f A_check(data_check); - A.print(); - A_check.print(); - assert(A == A_check); - return 0; -} diff --git a/src/modules/local_position_estimator/matrix/test/setIdentity.cpp b/src/modules/local_position_estimator/matrix/test/setIdentity.cpp deleted file mode 100644 index f80e437939..0000000000 --- a/src/modules/local_position_estimator/matrix/test/setIdentity.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#include "Matrix.hpp" -#include - -using namespace matrix; - -int main() -{ - Matrix3f A; - A.setIdentity(); - assert(A.rows() == 3); - assert(A.cols() == 3); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - if (i == j) { - assert(A(i, j) == 1); - - } else { - assert(A(i, j) == 0); - } - } - } - - return 0; -} diff --git a/src/modules/local_position_estimator/matrix/test/transpose.cpp b/src/modules/local_position_estimator/matrix/test/transpose.cpp deleted file mode 100644 index 3623fc1f9a..0000000000 --- a/src/modules/local_position_estimator/matrix/test/transpose.cpp +++ /dev/null @@ -1,18 +0,0 @@ -#include "Matrix.hpp" -#include -#include - -using namespace matrix; - -int main() -{ - float data[9] = {1, 2, 3, 4, 5, 6}; - Matrix A(data); - Matrix A_T = A.transpose(); - float data_check[9] = {1, 4, 2, 5, 3, 6}; - Matrix A_T_check(data_check); - A_T.print(); - A_T_check.print(); - assert(A_T == A_T_check); - return 0; -} diff --git a/src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp b/src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp deleted file mode 100644 index 68f5070194..0000000000 --- a/src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include "Matrix.hpp" -#include - -using namespace matrix; - -int main() -{ - Vector3f v; - v(0) = 1; - v(1) = 2; - v(2) = 3; - - v.print(); - - assert(v(0) == 1); - assert(v(1) == 2); - assert(v(2) == 3); - - Vector3f v2(4, 5, 6); - - v2.print(); - - assert(v2(0) == 4); - assert(v2(1) == 5); - assert(v2(2) == 6); - - return 0; -} diff --git a/src/platforms/common/CMakeLists.txt b/src/platforms/common/CMakeLists.txt index 09defae9b4..f7485a692c 100644 --- a/src/platforms/common/CMakeLists.txt +++ b/src/platforms/common/CMakeLists.txt @@ -35,6 +35,7 @@ set(depends prebuild_targets git_mavlink git_uavcan + git_matrix ) px4_add_module( From 083dbbb71b59b00c5c51c47f2eb6b700b95a268a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 22:53:38 +0100 Subject: [PATCH 120/161] IO firmware: Do mixer load as block operation --- src/modules/px4iofirmware/mixer.cpp | 17 +---------------- src/modules/px4iofirmware/px4io.h | 2 ++ src/modules/px4iofirmware/registers.c | 13 ++++++++++++- 3 files changed, 15 insertions(+), 17 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index b15ec09610..0a0962367d 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -88,9 +88,6 @@ static int mixer_callback(uintptr_t handle, static MixerGroup mixer_group(mixer_callback, 0); -/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */ -static void mixer_set_failsafe(); - void mixer_tick(void) { @@ -479,15 +476,6 @@ mixer_handle_text(const void *buffer, size_t length) /* if anything was parsed */ if (resid != mixer_text_length) { - /* only set mixer ok if no residual is left over */ - if (resid == 0) { - r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; - - } else { - /* not yet reached the end of the mixer, set as not ok */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; - } - isr_debug(2, "used %u", mixer_text_length - resid); /* copy any leftover text to the base of the buffer for re-use */ @@ -496,9 +484,6 @@ mixer_handle_text(const void *buffer, size_t length) } mixer_text_length = resid; - - /* update failsafe values */ - mixer_set_failsafe(); } break; @@ -507,7 +492,7 @@ mixer_handle_text(const void *buffer, size_t length) return 0; } -static void +void mixer_set_failsafe() { /* diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index a5e7a5500c..51ed9c6e40 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -190,6 +190,8 @@ extern pwm_limit_t pwm_limit; */ extern void mixer_tick(void); extern int mixer_handle_text(const void *buffer, size_t length); +/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */ +extern void mixer_set_failsafe(void); /** * Safety switch/LED. diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 46852003c8..18f5db5645 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -469,8 +469,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * Allow FMU override of arming state (to allow in-air restores), * but only if the arming state is not in sync on the IO side. */ - if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { + + if (PX4IO_P_STATUS_FLAGS_MIXER_OK & value) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; + + } else if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { r_status_flags = value; + + } + + if (PX4IO_P_STATUS_FLAGS_MIXER_OK & r_status_flags) { + + /* update failsafe values, now that the mixer is set to ok */ + mixer_set_failsafe(); } break; From 1baf103ee7458f0a6c11bb766101226a58b37f3f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 22:54:08 +0100 Subject: [PATCH 121/161] IO driver: Set mixer ok flag once the mixer load completed --- src/drivers/px4io/px4io.cpp | 32 ++++++++++++++------------------ 1 file changed, 14 insertions(+), 18 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 9a6581437e..80c5824019 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2090,12 +2090,11 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) } while (buflen > 0); - /* ensure a closing newline */ - msg->text[0] = '\n'; - msg->text[1] = '\0'; - int ret; + /* send the closing newline */ + msg->text[0] = '\n'; + msg->text[1] = '\0'; for (int i = 0; i < 30; i++) { /* failed, but give it a 2nd shot */ ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); @@ -2108,27 +2107,24 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) } } - if (ret) { - return ret; + if (ret == 0) { + /* success, exit */ + break; } retries--; - DEVICE_LOG("mixer sent"); + } while (retries > 0); - } while (retries > 0 && (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK))); + if (retries == 0) { + mavlink_and_console_log_info(_mavlink_fd, "[IO] mixer upload fail"); + /* load must have failed for some reason */ + return -EINVAL; - /* check for the mixer-OK flag */ - if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) { - mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok"); - return 0; + } else { + /* all went well, set the mixer ok flag */ + return io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_MIXER_OK); } - - DEVICE_LOG("mixer rejected by IO"); - mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); - - /* load must have failed for some reason */ - return -EINVAL; } void From 8154fcdc8aa87c680a69b92dfbc467a63fbf94d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 23:10:06 +0100 Subject: [PATCH 122/161] IO driver: Fix code style --- src/drivers/px4io/px4io.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 80c5824019..719bacb41f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2095,6 +2095,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) /* send the closing newline */ msg->text[0] = '\n'; msg->text[1] = '\0'; + for (int i = 0; i < 30; i++) { /* failed, but give it a 2nd shot */ ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); From 256f81fe4d058bf0972fceb68600f9c51e71e2c6 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 5 Nov 2015 14:21:57 -0700 Subject: [PATCH 123/161] add new config for QAV250 racing quad --- ROMFS/px4fmu_common/init.d/4009_qav250 | 39 ++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/4009_qav250 diff --git a/ROMFS/px4fmu_common/init.d/4009_qav250 b/ROMFS/px4fmu_common/init.d/4009_qav250 new file mode 100644 index 0000000000..9165a78168 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4009_qav250 @@ -0,0 +1,39 @@ +#!nsh +# +# @name Lumenier QAV250 +# +# @type Quadrotor x +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Mark Whitehorn +# + +sh /etc/init.d/4001_quad_x + +if [ $AUTOCNF == yes ] +then + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.08 + param set MC_ROLLRATE_I 0.05 + param set MC_ROLLRATE_D 0.003 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.08 + param set MC_PITCHRATE_I 0.1 + param set MC_PITCHRATE_D 0.003 + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.1 + param set MC_YAWRATE_D 0.0 + param set PWM_MIN 1230 + param set PWM_THR_MIN 0.06 + param set PWM_MANTHR_MIN 0.06 +fi + +# Transitional support: ensure suitable PWM min/max param values +#if param compare PWM_MIN 1075 +#then +# param set PWM_MIN 1230 +#fi From fc62461e5b99c1ebb346bdefd8c8e99f12553efb Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 5 Nov 2015 20:39:43 -0700 Subject: [PATCH 124/161] lower default PWM_MIN to 1075 --- ROMFS/px4fmu_common/init.d/4009_qav250 | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4009_qav250 b/ROMFS/px4fmu_common/init.d/4009_qav250 index 9165a78168..b27dc9b3e3 100644 --- a/ROMFS/px4fmu_common/init.d/4009_qav250 +++ b/ROMFS/px4fmu_common/init.d/4009_qav250 @@ -27,13 +27,8 @@ then param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 - param set PWM_MIN 1230 + param set PWM_MIN 1075 param set PWM_THR_MIN 0.06 param set PWM_MANTHR_MIN 0.06 fi -# Transitional support: ensure suitable PWM min/max param values -#if param compare PWM_MIN 1075 -#then -# param set PWM_MIN 1230 -#fi From 318eeb04945c21ebfa478bb12656ebb1fed3e141 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Fri, 6 Nov 2015 13:11:02 +0100 Subject: [PATCH 125/161] fixed ros build --- cmake/ros-CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/cmake/ros-CMakeLists.txt b/cmake/ros-CMakeLists.txt index aa42fb6ff4..4e814f4114 100644 --- a/cmake/ros-CMakeLists.txt +++ b/cmake/ros-CMakeLists.txt @@ -81,6 +81,7 @@ add_message_files( offboard_control_mode.msg vehicle_force_setpoint.msg distance_sensor.msg + control_state.msg ) ## Generate services in the 'srv' folder From 556a5dbd1d3a0c478dc7835e7600d91711bf64e1 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Fri, 6 Nov 2015 13:48:50 +0100 Subject: [PATCH 126/161] astyle fix --- .../attitude_estimator_q_main.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index a094772b37..f56e3ad243 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -54,7 +54,7 @@ #include #include #include - #include +#include #include #include #include @@ -527,22 +527,27 @@ void AttitudeEstimatorQ::task_main() } struct control_state_s ctrl_state = {}; + ctrl_state.timestamp = sensors.timestamp; /* Attitude quaternions for control state */ ctrl_state.q[0] = _q(0); + ctrl_state.q[1] = _q(1); + ctrl_state.q[2] = _q(2); + ctrl_state.q[3] = _q(3); /* Attitude rates for control state */ ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0)); + ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1)); + ctrl_state.roll_rate = _rates(2); /* Publish to control state topic */ - if (_ctrl_state_pub == nullptr) - { + if (_ctrl_state_pub == nullptr) { _ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); } else { From e1924c5d666f44f71e521bd50da5690de90861a2 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 6 Nov 2015 16:46:32 +0100 Subject: [PATCH 127/161] ekf: publish control state --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 82b96c4043..f4fcf78e25 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -700,6 +700,9 @@ void AttitudePositionEstimatorEKF::task_main() // Publish attitude estimations publishAttitude(); + // publish control state + publishControlState(); + // Publish Local Position estimations publishLocalPosition(); From ca40cbcf6ea7045edc707cf9ec34fca5f9ea63a2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 6 Nov 2015 20:56:53 +0100 Subject: [PATCH 128/161] Documented parameters in system setup which require a restart to become effective --- src/modules/systemlib/system_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 8170e17bd1..0bf1a3ea9e 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -40,7 +40,7 @@ /** * Auto-start script index. * - * Defines the auto-start script used to bootstrap the system. + * CHANGING THIS VALUE REQUIRES A RESTART. Defines the auto-start script used to bootstrap the system. * * @group System */ @@ -84,7 +84,7 @@ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); /** * Companion computer interface * -* Configures the baud rate of the companion computer interface. +* CHANGING THIS VALUE REQUIRES A RESTART. Configures the baud rate of the companion computer interface. * Set to zero to disable, set to these values to enable (NO OTHER VALUES SUPPORTED!) * 921600: enables onboard mode at 921600 baud, 8N1. 57600: enables onboard mode at 57600 baud, 8N1. * 157600: enables OSD mode at 57600 baud, 8N1. From b54a149b908f3fe5fe580ff39a8d0112803d104e Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Fri, 6 Nov 2015 12:34:27 -0700 Subject: [PATCH 129/161] reduce PID rollrate P and D in QAV250 config --- ROMFS/px4fmu_common/init.d/4009_qav250 | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4009_qav250 b/ROMFS/px4fmu_common/init.d/4009_qav250 index b27dc9b3e3..6a6c1f1727 100644 --- a/ROMFS/px4fmu_common/init.d/4009_qav250 +++ b/ROMFS/px4fmu_common/init.d/4009_qav250 @@ -16,9 +16,9 @@ sh /etc/init.d/4001_quad_x if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.08 + param set MC_ROLLRATE_P 0.05 param set MC_ROLLRATE_I 0.05 - param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_D 0.002 param set MC_PITCH_P 7.0 param set MC_PITCHRATE_P 0.08 param set MC_PITCHRATE_I 0.1 From 4f795309fde88b35e6d147a7a3f10671266a2470 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Nov 2015 00:32:03 +0100 Subject: [PATCH 130/161] ROMFS startup script: Fix up QAV250 script --- ROMFS/px4fmu_common/init.d/4009_qav250 | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4009_qav250 b/ROMFS/px4fmu_common/init.d/4009_qav250 index 6a6c1f1727..dfd94992af 100644 --- a/ROMFS/px4fmu_common/init.d/4009_qav250 +++ b/ROMFS/px4fmu_common/init.d/4009_qav250 @@ -27,8 +27,7 @@ then param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 - param set PWM_MIN 1075 - param set PWM_THR_MIN 0.06 - param set PWM_MANTHR_MIN 0.06 + param set PWM_MIN 980 + param set MPC_THR_MIN 0.06 + param set MPC_MANTHR_MIN 0.06 fi - From f170272669aff6248d9e793e6cc7fb73ce019966 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Nov 2015 09:30:13 +0100 Subject: [PATCH 131/161] Mac OS: Fix clock --- src/platforms/posix/px4_layer/drv_hrt.c | 34 ++++++++----------------- 1 file changed, 11 insertions(+), 23 deletions(-) diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 0544131705..3b0502866c 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -81,35 +81,23 @@ static void hrt_unlock(void) px4_sem_post(&_hrt_lock); } -#ifdef __PX4_DARWIN - -#include -#define MAC_NANO (+1.0E-9) -#define MAC_GIGA UINT64_C(1000000000) -#define CLOCK_MONOTONIC 1 -#define HRT_LOCK_NAME "/hrt_lock" - -static double px4_timebase = 0.0; +#if defined(__APPLE__) && defined(__MACH__) +#include +#include +#define CLOCK_REALTIME 0 int px4_clock_gettime(clockid_t clk_id, struct timespec *tp) { - if (clk_id != CLOCK_MONOTONIC) { - return 1; + struct timeval now; + int rv = gettimeofday(&now, NULL); + + if(rv) { + return rv; } - if (!px4_timestart) { - mach_timebase_info_data_t tb = {}; - mach_timebase_info(&tb); - px4_timebase = tb.numer; - px4_timebase /= tb.denom; - // px4_timestart = mach_absolute_time(); - } + tp->tv_sec = now.tv_sec; + tp->tv_nsec = now.tv_usec * 1000; - memset(tp, 0, sizeof(*tp)); - - double diff = mach_absolute_time() * px4_timebase; - tp->tv_sec = diff * MAC_NANO; - tp->tv_nsec = diff - (tp->tv_sec * MAC_GIGA); return 0; } From ead4cc857b1ca4fb86e5ce266acaafd92cbf48a4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Nov 2015 09:44:38 +0100 Subject: [PATCH 132/161] Matrix lib: Update reference --- .gitmodules | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitmodules b/.gitmodules index 2bcbb90147..744dcd2787 100644 --- a/.gitmodules +++ b/.gitmodules @@ -27,4 +27,4 @@ url = https://github.com/google/googletest.git [submodule "src/lib/matrix"] path = src/lib/matrix - url = https://github.com/dronecrew/matrix.git + url = https://github.com/PX4/Matrix.git From 9721421d86d47d962c25815928bdb1645f5fce91 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Nov 2015 09:47:42 +0100 Subject: [PATCH 133/161] Update math library --- src/lib/matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/matrix b/src/lib/matrix index 7abbdcd88f..222e42f934 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 7abbdcd88fffb44d081e9a212c7d655b6ff5e9ca +Subproject commit 222e42f9342e4ab50ea6a14fccc5051b45acd6d6 From ac893f26fbf56fa32d04875f5a36e14156dac80d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Nov 2015 09:47:58 +0100 Subject: [PATCH 134/161] Update math lib include path --- src/lib/mathlib/math/Matrix.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 3491d8bf0f..67455aff9c 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -49,7 +49,7 @@ #ifdef CONFIG_ARCH_ARM #include "../CMSIS/Include/arm_math.h" #else -#include "matrix/matrix.hpp" +#include "matrix/math.hpp" #endif #include From 2919129a74b177d2d839ab4e6dddb3461fe15b4a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Nov 2015 09:52:58 +0100 Subject: [PATCH 135/161] Launch detect: Remove old params --- src/lib/launchdetection/CMakeLists.txt | 1 - src/lib/launchdetection/launchdetection_params.c | 4 ---- 2 files changed, 5 deletions(-) diff --git a/src/lib/launchdetection/CMakeLists.txt b/src/lib/launchdetection/CMakeLists.txt index 7ea0c18788..b396169a45 100644 --- a/src/lib/launchdetection/CMakeLists.txt +++ b/src/lib/launchdetection/CMakeLists.txt @@ -37,7 +37,6 @@ px4_add_module( SRCS LaunchDetector.cpp CatapultLaunchMethod.cpp - launchdetection_params.c DEPENDS platforms__common ) diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index 5159f2fcb2..e5070eeaaf 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -39,10 +39,6 @@ * @author Thomas Gubler */ -#include - -#include - /* * Catapult launch detection parameters, accessible via MAVLink * From 54fff33e8ff9c132b0c986dc40afe68543089b9a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Nov 2015 09:53:20 +0100 Subject: [PATCH 136/161] MAVLink app: Move param file to generated version --- src/modules/mavlink/CMakeLists.txt | 1 - src/modules/mavlink/mavlink_params.c | 2 -- 2 files changed, 3 deletions(-) diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index a863328f03..c8a0e7d870 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -56,7 +56,6 @@ px4_add_module( mavlink_rate_limiter.cpp mavlink_receiver.cpp mavlink_ftp.cpp - mavlink_params.c DEPENDS platforms__common ) diff --git a/src/modules/mavlink/mavlink_params.c b/src/modules/mavlink/mavlink_params.c index 4edaa7b582..e50a95a25f 100644 --- a/src/modules/mavlink/mavlink_params.c +++ b/src/modules/mavlink/mavlink_params.c @@ -31,8 +31,6 @@ * ****************************************************************************/ -#include - /** * MAVLink system ID * @group MAVLink From 90023b2a0c2073fa0a8610729214a10e94056af6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Nov 2015 09:53:37 +0100 Subject: [PATCH 137/161] MC att control: Move param file to generated version --- src/modules/mc_att_control_multiplatform/CMakeLists.txt | 1 - .../mc_att_control_multiplatform/mc_att_control_params.c | 6 +----- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/src/modules/mc_att_control_multiplatform/CMakeLists.txt b/src/modules/mc_att_control_multiplatform/CMakeLists.txt index 22f9d16ee8..60aefe210a 100644 --- a/src/modules/mc_att_control_multiplatform/CMakeLists.txt +++ b/src/modules/mc_att_control_multiplatform/CMakeLists.txt @@ -38,7 +38,6 @@ px4_add_module( mc_att_control_start_nuttx.cpp mc_att_control.cpp mc_att_control_base.cpp - mc_att_control_params.c DEPENDS platforms__common ) diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c index 9c6ac5dc73..e34732d1f3 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c @@ -36,14 +36,10 @@ * Parameters for multicopter attitude controller. * * @author Tobias Naegeli - * @author Lorenz Meier + * @author Lorenz Meier * @author Anton Babushkin */ -#include -#include "mc_att_control_params.h" -#include - /** * Roll P gain * From 15e8f28c88c90440b2da8f37cb578978610c576e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Nov 2015 09:53:52 +0100 Subject: [PATCH 138/161] VTOL att control: Move param file to generated version --- src/modules/vtol_att_control/CMakeLists.txt | 3 --- src/modules/vtol_att_control/standard_params.c | 4 +--- src/modules/vtol_att_control/tiltrotor_params.c | 2 +- src/modules/vtol_att_control/vtol_att_control_params.c | 4 +--- 4 files changed, 3 insertions(+), 10 deletions(-) diff --git a/src/modules/vtol_att_control/CMakeLists.txt b/src/modules/vtol_att_control/CMakeLists.txt index 6af889f52a..b7dcd83ed0 100644 --- a/src/modules/vtol_att_control/CMakeLists.txt +++ b/src/modules/vtol_att_control/CMakeLists.txt @@ -38,12 +38,9 @@ px4_add_module( SRCS vtol_att_control_main.cpp - vtol_att_control_params.c - tiltrotor_params.c tiltrotor.cpp vtol_type.cpp tailsitter.cpp - standard_params.c standard.cpp DEPENDS platforms__common diff --git a/src/modules/vtol_att_control/standard_params.c b/src/modules/vtol_att_control/standard_params.c index 134dcd0b8a..3a2185eead 100644 --- a/src/modules/vtol_att_control/standard_params.c +++ b/src/modules/vtol_att_control/standard_params.c @@ -36,11 +36,9 @@ * Parameters for the standard VTOL controller. * * @author Simon Wilks - * @author Roman Bapst + * @author Roman Bapst */ -#include - /** * Target throttle value for pusher/puller motor during the transition to fw mode * diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index d1b5b31081..58062c40b0 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -35,7 +35,7 @@ * @file tiltrotor_params.c * Parameters for vtol attitude controller. * - * @author Roman Bapst + * @author Roman Bapst */ #include 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 56779ca8f9..fe7d8ebd82 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -35,11 +35,9 @@ * @file vtol_att_control_params.c * Parameters for vtol attitude controller. * - * @author Roman Bapst + * @author Roman Bapst */ -#include - /** * VTOL number of engines * From 63bc4e0202b2dfd0debcb99e78cb0f9019c67182 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Nov 2015 10:14:46 +0100 Subject: [PATCH 139/161] Fix HRT format --- src/platforms/posix/px4_layer/drv_hrt.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 3b0502866c..e4839f0ac7 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -91,7 +91,7 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp) struct timeval now; int rv = gettimeofday(&now, NULL); - if(rv) { + if (rv) { return rv; } From 5f407f539ecd6137feca815b7e9dc1c0a11db619 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Nov 2015 16:27:32 +0100 Subject: [PATCH 140/161] Sensors: Fix wrong define leading to incorrect battery voltage scaler picked for FMUv1. Found by @xn365 --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 5e330ca7a2..cccf9f6b3a 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -850,7 +850,7 @@ Sensors::parameters_update() _parameters.battery_voltage_scaling = 0.0082f; #elif CONFIG_ARCH_BOARD_AEROCORE _parameters.battery_voltage_scaling = 0.0063f; -#elif CONFIG_ARCH_BOARD_PX4FMU_V2 +#elif CONFIG_ARCH_BOARD_PX4FMU_V1 _parameters.battery_voltage_scaling = 0.00459340659f; #else /* ensure a missing default trips a low voltage lockdown */ From 756ff6a6fb0555e643f35251519f45d98f58ed6a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Nov 2015 17:06:36 +0100 Subject: [PATCH 141/161] Update matrix framework --- src/lib/matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/matrix b/src/lib/matrix index 222e42f934..4058e32f73 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 222e42f9342e4ab50ea6a14fccc5051b45acd6d6 +Subproject commit 4058e32f735c3e3cb36dc8ac174a15c4208560f8 From 2d2feccceed7c04c62b72fd423f706fc07d60ab8 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sat, 7 Nov 2015 11:33:15 -0500 Subject: [PATCH 142/161] Matrix update. --- src/lib/matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/matrix b/src/lib/matrix index 222e42f934..ace2751715 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 222e42f9342e4ab50ea6a14fccc5051b45acd6d6 +Subproject commit ace275171533f527abebcedbfd5dfb8bc0f880a5 From c5f60d602a1185111d9a3aab66468c4d336ae594 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sat, 7 Nov 2015 11:33:15 -0500 Subject: [PATCH 143/161] Matrix update. --- src/lib/matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/matrix b/src/lib/matrix index 4058e32f73..ace2751715 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 4058e32f735c3e3cb36dc8ac174a15c4208560f8 +Subproject commit ace275171533f527abebcedbfd5dfb8bc0f880a5 From ca5a2d1fcabd841c77b7b678c55be8dbab401fca Mon Sep 17 00:00:00 2001 From: jgoppert Date: Fri, 6 Nov 2015 21:11:27 -0500 Subject: [PATCH 144/161] Fixed some sitl init bugs. --- src/modules/sensors/sensors.cpp | 2 +- src/modules/simulator/simulator_mavlink.cpp | 2 ++ src/platforms/posix/drivers/adcsim/adcsim.cpp | 1 + src/platforms/posix/px4_layer/px4_posix_tasks.cpp | 4 +--- 4 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index cccf9f6b3a..003a45137c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1250,7 +1250,7 @@ Sensors::vehicle_control_mode_poll() void Sensors::parameter_update_poll(bool forced) { - bool param_updated; + bool param_updated=false; /* Check if any parameter has changed */ orb_check(_params_sub, ¶m_updated); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index f99dd20450..ca45cdf2b8 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -463,10 +463,12 @@ void Simulator::pollForMAVLinkMessages(bool publish) char serial_buf[1024]; struct pollfd fds[2]; + memset(fds, 0, sizeof(fds)); unsigned fd_count = 1; fds[0].fd = _fd; fds[0].events = POLLIN; + if (serial_fd >= 0) { fds[1].fd = serial_fd; fds[1].events = POLLIN; diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index d9f0dd4f15..c72cc1a494 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -111,6 +111,7 @@ private: ADCSIM::ADCSIM(uint32_t channels) : VDev("adcsim", ADCSIM0_DEVICE_PATH), + _call(), _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), _channel_count(0), _samples(nullptr) diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index ba476b5070..41cade1d5d 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -81,9 +81,7 @@ typedef struct { static void *entry_adapter(void *ptr) { - pthdata_t *data; - data = (pthdata_t *) ptr; - + pthdata_t *data = (pthdata_t *) ptr; data->entry(data->argc, data->argv); free(ptr); PX4_DEBUG("Before px4_task_exit"); From 68e4b33167f7bf6406ff68e7c6091e4834f337fe Mon Sep 17 00:00:00 2001 From: jgoppert Date: Fri, 6 Nov 2015 21:11:54 -0500 Subject: [PATCH 145/161] Added valgrind to sitl. --- Tools/sitl_run.sh | 6 ++++++ src/firmware/posix/CMakeLists.txt | 2 +- src/modules/sensors/sensors.cpp | 2 +- 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 521ebeef21..b4617e40cf 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -83,6 +83,12 @@ then elif [ "$debugger" == "gdb" ] then gdb --args mainapp ../../../../${rc_script}_${program} +elif [ "$debugger" == "ddd" ] +then + ddd --debugger gdb --args mainapp ../../../../${rc_script}_${program} +elif [ "$debugger" == "valgrind" ] +then + valgrind ./mainapp ../../../../${rc_script}_${program} else ./mainapp ../../../../${rc_script}_${program} fi diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index 64f4775c33..fb7490ffe2 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -33,7 +33,7 @@ add_custom_target(run_config add_dependencies(run_config mainapp) foreach(viewer none jmavsim gazebo) - foreach(debugger none gdb lldb) + foreach(debugger none gdb lldb ddd valgrind) foreach(model none iris vtol) if (debugger STREQUAL "none") if (model STREQUAL "none") diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 003a45137c..a349a78ea0 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1250,7 +1250,7 @@ Sensors::vehicle_control_mode_poll() void Sensors::parameter_update_poll(bool forced) { - bool param_updated=false; + bool param_updated = false; /* Check if any parameter has changed */ orb_check(_params_sub, ¶m_updated); From c6b84f522696fe63f50b9dce398fee8ff7fe4c2a Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sat, 7 Nov 2015 16:40:51 -0500 Subject: [PATCH 146/161] Attitude estimator hot fix. --- src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index f56e3ad243..6c78c583b1 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -544,7 +544,7 @@ void AttitudeEstimatorQ::task_main() ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1)); - ctrl_state.roll_rate = _rates(2); + ctrl_state.yaw_rate = _rates(2); /* Publish to control state topic */ if (_ctrl_state_pub == nullptr) { From 825b651d17e56dabe7c725042a99483c4bdc2507 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Nov 2015 10:22:45 +0100 Subject: [PATCH 147/161] Sensors: Initialize poll struct --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a349a78ea0..ab246e14a1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -2146,7 +2146,7 @@ Sensors::task_main() _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); /* wakeup source(s) */ - px4_pollfd_struct_t fds[1]; + px4_pollfd_struct_t fds[1] = {}; /* use the gyro to pace output */ fds[0].fd = _gyro_sub[0]; From e5b8a70680cd9fc667471d7b5908b02b7764468e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Nov 2015 10:23:37 +0100 Subject: [PATCH 148/161] Attitude estimator Q: Add low pass to yaw axis for control state topic, publish raw attitude and prevent double filtering of attitude --- .../attitude_estimator_q_main.cpp | 18 +++++------------- 1 file changed, 5 insertions(+), 13 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 6c78c583b1..eb796d54d0 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -207,7 +207,7 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() : _voter_mag(3), _lp_roll_rate(250.0f, 18.0f), _lp_pitch_rate(250.0f, 18.0f), - _lp_yaw_rate(250, 10.0f) + _lp_yaw_rate(250.0f, 10.0f) { _voter_mag.set_timeout(200000); @@ -495,12 +495,9 @@ void AttitudeEstimatorQ::task_main() att.pitch = euler(1); att.yaw = euler(2); - /* the complimentary filter should reflect the true system - * state, but we need smoother outputs for the control system - */ - att.rollspeed = _lp_roll_rate.apply(_rates(0)); - att.pitchspeed = _lp_pitch_rate.apply(_rates(1)); - att.yawspeed = _lp_yaw_rate.apply(_rates(2)); + att.rollspeed = _rates(0); + att.pitchspeed = _rates(1); + att.yawspeed = _rates(2); for (int i = 0; i < 3; i++) { att.g_comp[i] = _accel(i) - _pos_acc(i); @@ -532,19 +529,14 @@ void AttitudeEstimatorQ::task_main() /* Attitude quaternions for control state */ ctrl_state.q[0] = _q(0); - ctrl_state.q[1] = _q(1); - ctrl_state.q[2] = _q(2); - ctrl_state.q[3] = _q(3); /* Attitude rates for control state */ ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0)); - ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1)); - - ctrl_state.yaw_rate = _rates(2); + ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2)); /* Publish to control state topic */ if (_ctrl_state_pub == nullptr) { From 066be5b9b39dcd04058c02ba719b8cddec9bd08b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Nov 2015 10:24:55 +0100 Subject: [PATCH 149/161] Q estimator: Code style fix --- .../attitude_estimator_q/attitude_estimator_q_main.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index eb796d54d0..1531e32778 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -529,13 +529,18 @@ void AttitudeEstimatorQ::task_main() /* Attitude quaternions for control state */ ctrl_state.q[0] = _q(0); + ctrl_state.q[1] = _q(1); + ctrl_state.q[2] = _q(2); + ctrl_state.q[3] = _q(3); /* Attitude rates for control state */ ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0)); + ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1)); + ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2)); /* Publish to control state topic */ From 3d420289a59f56813625800e34a63603511c6e76 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sun, 8 Nov 2015 12:10:43 -0500 Subject: [PATCH 150/161] Matrix update. --- src/lib/matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/matrix b/src/lib/matrix index ace2751715..222a97e73f 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit ace275171533f527abebcedbfd5dfb8bc0f880a5 +Subproject commit 222a97e73f6be247b24e7d8f95019f62a8a1af3d From 4636670130283da50e759b51d293d473f3f79952 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sun, 8 Nov 2015 12:18:47 -0500 Subject: [PATCH 151/161] Updated inverse in mathlib for new matrix call. --- src/lib/mathlib/math/Matrix.hpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 67455aff9c..54a1b87ee0 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -339,9 +339,8 @@ public: arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); return res; #else - matrix::Matrix Me(this->arm_mat.pData); - matrix::Matrix MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea - Matrix res(MyInverse.data()); + matrix::SquareMatrix Me = matrix::Matrix(this->arm_mat.pData); + Matrix res(Me.I().data()); return res; #endif } From 74ce39a676616b537c7824e2b41daa15b385a093 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Nov 2015 19:05:13 +0100 Subject: [PATCH 152/161] Move Eclipse project files out of the way since they screw over Windows users --- .cproject => template_.cproject | 0 .project => template_.project | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename .cproject => template_.cproject (100%) rename .project => template_.project (100%) diff --git a/.cproject b/template_.cproject similarity index 100% rename from .cproject rename to template_.cproject diff --git a/.project b/template_.project similarity index 100% rename from .project rename to template_.project From a6ba6eb2e4bf848b177d8fe3d4a726adbf88c617 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 7 Nov 2015 16:27:08 -0700 Subject: [PATCH 153/161] reduce Z control deadband from 40% to 5% --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 0ac8199b0a..457682e334 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -119,7 +119,7 @@ public: int start(); private: - const float alt_ctl_dz = 0.2f; + const float alt_ctl_dz = 0.025f; bool _task_should_exit; /**< if true, task should exit */ int _control_task; /**< task handle for task */ From 56d67c6f6c8f19d5362936257ed4e6a76513c0dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 9 Nov 2015 09:31:43 +0100 Subject: [PATCH 154/161] Tools: Support injecting param meta, do not generate code for these --- Tools/parameters_injected.xml | 146 ++++++++++++++++++++++++++++++++++ Tools/px4params/xmlout.py | 6 +- Tools/px_generate_params.py | 5 +- Tools/px_process_params.py | 8 +- 4 files changed, 161 insertions(+), 4 deletions(-) create mode 100644 Tools/parameters_injected.xml diff --git a/Tools/parameters_injected.xml b/Tools/parameters_injected.xml new file mode 100644 index 0000000000..a5b6bc0883 --- /dev/null +++ b/Tools/parameters_injected.xml @@ -0,0 +1,146 @@ + + + 3 + + + Speed controller bandwidth + Speed controller bandwidth, in Hz. Higher values result in faster speed and current rise times, but may result in overshoot and higher current consumption. For fixed-wing aircraft, this value should be less than 50 Hz; for multirotors, values up to 100 Hz may provide improvements in responsiveness. + Hertz + 10 + 250 + + + Reverse direction + Motor spin direction as detected during initial enumeration. Use 0 or 1 to reverse direction. + 0 + 1 + + + Speed (RPM) controller gain + Speed (RPM) controller gain. Determines controller + aggressiveness; units are amp-seconds per radian. Systems with + higher rotational inertia (large props) will need gain increased; + systems with low rotational inertia (small props) may need gain + decreased. Higher values result in faster response, but may result + in oscillation and excessive overshoot. Lower values result in a + slower, smoother response. + amp-seconds per radian + 3 + 0.00 + 1.00 + + + Idle speed (e Hz) + Idle speed (e Hz) + Hertz + 3 + 0.0 + 100.0 + + + Spin-up rate (e Hz/s) + Spin-up rate (e Hz/s) + Hz/s + + 5 + 1000 + + + Index of this ESC in throttle command messages. + Index of this ESC in throttle command messages. + Index + + 0 + 15 + + + Extended status ID + Extended status ID + + + 1 + 1023 + + + Extended status interval (µs) + Extended status interval (µs) + µs + + 0 + 1000000 + + + ESC status interval (µs) + ESC status interval (µs) + µs + + 1000000 + + + Motor current limit in amps + Motor current limit in amps. This determines the maximum + current controller setpoint, as well as the maximum allowable + current setpoint slew rate. This value should generally be set to + the continuous current rating listed in the motor’s specification + sheet, or set equal to the motor’s specified continuous power + divided by the motor voltage limit. + Amps + 3 + 1 + 80 + + + Motor Kv in RPM per volt + Motor Kv in RPM per volt. This can be taken from the motor’s + specification sheet; accuracy will help control performance but + some deviation from the specified value is acceptable. + RPM/v + 0 + 0 + 100 + + + READ ONLY: Motor inductance in henries. + READ ONLY: Motor inductance in henries. This is measured on start-up. + henries + 3 + + + Number of motor poles. + Number of motor poles. Used to convert mechanical speeds to + electrical speeds. This number should be taken from the motor’s + specification sheet. + Poles + + 2 + 40 + + + READ ONLY: Motor resistance in ohms + READ ONLY: Motor resistance in ohms. This is measured on start-up. When + tuning a new motor, check that this value is approximately equal + to the value shown in the motor’s specification sheet. + Ohms + 3 + + + Acceleration limit (V) + Acceleration limit (V) + Volts + 3 + 0.01 + 1.00 + + + Motor voltage limit in volts + Motor voltage limit in volts. The current controller’s + commanded voltage will never exceed this value. Note that this may + safely be above the nominal voltage of the motor; to determine the + actual motor voltage limit, divide the motor’s rated power by the + motor current limit. + Volts + 3 + 0 + + + \ No newline at end of file diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py index b072ab79f8..b37cdb0627 100644 --- a/Tools/px4params/xmlout.py +++ b/Tools/px4params/xmlout.py @@ -18,10 +18,14 @@ def indent(elem, level=0): class XMLOutput(): - def __init__(self, groups, board): + def __init__(self, groups, board, inject_xml_file_name): xml_parameters = ET.Element("parameters") xml_version = ET.SubElement(xml_parameters, "version") xml_version.text = "3" + importtree = ET.parse(inject_xml_file_name) + injectgroups = importtree.getroot().findall("group") + for igroup in injectgroups: + xml_parameters.append(igroup) last_param_name = "" board_specific_param_set = False for group in groups: diff --git a/Tools/px_generate_params.py b/Tools/px_generate_params.py index 3df124f523..f1877987ad 100755 --- a/Tools/px_generate_params.py +++ b/Tools/px_generate_params.py @@ -29,7 +29,7 @@ start_name = "" end_name = "" for group in root: - if group.tag == "group": + if group.tag == "group" and "no_code_generation" not in group.attrib: header += """ /***************************************************************** * %s @@ -62,7 +62,8 @@ struct px4_parameters_t px4_parameters = { """ i=0 for group in root: - if group.tag == "group": + if group.tag == "group" and "no_code_generation" not in group.attrib: + src += """ /***************************************************************** * %s diff --git a/Tools/px_process_params.py b/Tools/px_process_params.py index c2024dc726..3a11fc5c50 100644 --- a/Tools/px_process_params.py +++ b/Tools/px_process_params.py @@ -65,6 +65,12 @@ def main(): metavar="FILENAME", help="Create XML file" " (default FILENAME: parameters.xml)") + parser.add_argument("-i", "--inject-xml", + nargs='?', + const="../Tools/parameters_injected.xml", + metavar="FILENAME", + help="Inject additional param XML file" + " (default FILENAME: ../Tools/parameters_injected.xml)") parser.add_argument("-b", "--board", nargs='?', const="", @@ -124,7 +130,7 @@ def main(): # Output to XML file if args.xml: print("Creating XML file " + args.xml) - out = xmlout.XMLOutput(param_groups, args.board) + out = xmlout.XMLOutput(param_groups, args.board, args.inject_xml) out.Save(args.xml) # Output to DokuWiki tables From a9a1e69dd6f11d6c9a1ce0a63b18636f841a1f72 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 9 Nov 2015 09:31:57 +0100 Subject: [PATCH 155/161] CMake: Enable param meta injection --- cmake/common/px4_base.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index e6ff9123d6..8a3a844886 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -741,7 +741,7 @@ function(px4_generate_parameters_xml) ) add_custom_command(OUTPUT ${OUT} COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/Tools/px_process_params.py - -s ${path} --board CONFIG_ARCH_${BOARD} --xml + -s ${path} --board CONFIG_ARCH_${BOARD} --xml --inject-xml DEPENDS ${param_src_files} ) set(${OUT} ${${OUT}} PARENT_SCOPE) From 1b640f80fe17a055db7de3750f063a87b45a2252 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 9 Nov 2015 23:40:31 +0100 Subject: [PATCH 156/161] MC pos control: Widen manual deadband back to 20% --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 457682e334..c36ba5bfc6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -119,7 +119,7 @@ public: int start(); private: - const float alt_ctl_dz = 0.025f; + const float alt_ctl_dz = 0.1f; bool _task_should_exit; /**< if true, task should exit */ int _control_task; /**< task handle for task */ From b3bb8adbc5d3498bb95865820ec7a62bbf14c28e Mon Sep 17 00:00:00 2001 From: jgoppert Date: Mon, 9 Nov 2015 19:59:45 -0500 Subject: [PATCH 157/161] Matrix update. --- src/lib/matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/matrix b/src/lib/matrix index 222a97e73f..2c7a375e3d 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 222a97e73f6be247b24e7d8f95019f62a8a1af3d +Subproject commit 2c7a375e3d7ce143dd1991c9135a5a55952a8977 From dcc9e29b66e13f9a11a3a50ac6261045d8d463ca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Nov 2015 23:02:54 +0100 Subject: [PATCH 158/161] Updated injected XML file --- Tools/parameters_injected.xml | 75 ++++++++++++++++------------------- 1 file changed, 34 insertions(+), 41 deletions(-) diff --git a/Tools/parameters_injected.xml b/Tools/parameters_injected.xml index a5b6bc0883..e1fc51c619 100644 --- a/Tools/parameters_injected.xml +++ b/Tools/parameters_injected.xml @@ -15,7 +15,7 @@ 0 1 - + Speed (RPM) controller gain Speed (RPM) controller gain. Determines controller aggressiveness; units are amp-seconds per radian. Systems with @@ -24,59 +24,53 @@ decreased. Higher values result in faster response, but may result in oscillation and excessive overshoot. Lower values result in a slower, smoother response. - amp-seconds per radian - 3 + amp-seconds per radian + 3 0.00 1.00 - + Idle speed (e Hz) Idle speed (e Hz) - Hertz - 3 + Hertz + 3 0.0 100.0 - + Spin-up rate (e Hz/s) Spin-up rate (e Hz/s) - Hz/s - + Hz/s 5 1000 - + Index of this ESC in throttle command messages. Index of this ESC in throttle command messages. - Index - + Index 0 15 - + Extended status ID Extended status ID - - 1 1023 - + Extended status interval (µs) Extended status interval (µs) - µs - + µs 0 1000000 - + ESC status interval (µs) ESC status interval (µs) - µs - + µs 1000000 - + Motor current limit in amps Motor current limit in amps. This determines the maximum current controller setpoint, as well as the maximum allowable @@ -84,62 +78,61 @@ the continuous current rating listed in the motor’s specification sheet, or set equal to the motor’s specified continuous power divided by the motor voltage limit. - Amps - 3 + Amps + 3 1 80 - + Motor Kv in RPM per volt Motor Kv in RPM per volt. This can be taken from the motor’s specification sheet; accuracy will help control performance but some deviation from the specified value is acceptable. - RPM/v - 0 + RPM/v + 0 0 100 READ ONLY: Motor inductance in henries. READ ONLY: Motor inductance in henries. This is measured on start-up. - henries - 3 + henries + 3 - + Number of motor poles. Number of motor poles. Used to convert mechanical speeds to electrical speeds. This number should be taken from the motor’s specification sheet. - Poles - + Poles 2 40 - + READ ONLY: Motor resistance in ohms READ ONLY: Motor resistance in ohms. This is measured on start-up. When tuning a new motor, check that this value is approximately equal to the value shown in the motor’s specification sheet. - Ohms - 3 + Ohms + 3 - + Acceleration limit (V) Acceleration limit (V) - Volts - 3 + Volts + 3 0.01 1.00 - + Motor voltage limit in volts Motor voltage limit in volts. The current controller’s commanded voltage will never exceed this value. Note that this may safely be above the nominal voltage of the motor; to determine the actual motor voltage limit, divide the motor’s rated power by the motor current limit. - Volts - 3 + Volts + 3 0 From 8ae78ab09353a7e67fde980a9c488ee0d893a01a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 11 Nov 2015 00:06:26 +0100 Subject: [PATCH 159/161] Params: Fix max range for injected params --- Tools/parameters_injected.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/parameters_injected.xml b/Tools/parameters_injected.xml index e1fc51c619..6cf856017c 100644 --- a/Tools/parameters_injected.xml +++ b/Tools/parameters_injected.xml @@ -55,7 +55,7 @@ Extended status ID Extended status ID 1 - 1023 + 1000000 Extended status interval (µs) From 40defeeceda22715a7ac4fabcdb6c0771d82aa28 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Wed, 11 Nov 2015 15:35:55 +0530 Subject: [PATCH 160/161] commander : fix led reporting --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3573f5d6eb..fc755437da 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2536,7 +2536,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu } else if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { rgbled_set_color(RGBLED_COLOR_RED); } else { - if (status_local->condition_home_position_valid) { + if (status_local->condition_home_position_valid && status_local->condition_global_position_valid) { rgbled_set_color(RGBLED_COLOR_GREEN); } else { From de39e7de62cc7bc4006d890fbd808708d83e8eac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Nov 2015 11:33:36 +0100 Subject: [PATCH 161/161] Fix param meta info --- Tools/parameters_injected.xml | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/Tools/parameters_injected.xml b/Tools/parameters_injected.xml index 6cf856017c..2f164ee52b 100644 --- a/Tools/parameters_injected.xml +++ b/Tools/parameters_injected.xml @@ -89,9 +89,8 @@ specification sheet; accuracy will help control performance but some deviation from the specified value is acceptable. RPM/v - 0 0 - 100 + 4000 READ ONLY: Motor inductance in henries. @@ -116,7 +115,7 @@ Ohms 3 - + Acceleration limit (V) Acceleration limit (V) Volts