From 9d5ea4bcebcb574ba55543ae0fe68c290dd0df10 Mon Sep 17 00:00:00 2001 From: mswingtra Date: Fri, 28 Aug 2015 11:06:12 +0200 Subject: [PATCH 001/106] 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/106] 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/106] 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/106] 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/106] 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/106] 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/106] 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/106] 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/106] 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/106] 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/106] 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/106] 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/106] 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/106] 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 efe82194af9655e5e14a733a5ab8fc9dd784c126 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Fri, 16 Oct 2015 16:03:25 +0200 Subject: [PATCH 015/106] 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 016/106] 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 e227f139a5b362f308a20197e87aa79faec62819 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Oct 2015 17:24:14 +0300 Subject: [PATCH 017/106] Libuavcan switched to footprint_reduction --- src/modules/uavcan/libuavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/libuavcan b/src/modules/uavcan/libuavcan index 0643879922..4e4d9b7854 160000 --- a/src/modules/uavcan/libuavcan +++ b/src/modules/uavcan/libuavcan @@ -1 +1 @@ -Subproject commit 0643879922239930cf7482777356f06891c26616 +Subproject commit 4e4d9b7854b7821f42fd83d89885cc489cc18c33 From 7373d99d72db00d76a6cb2a776d5e3f31d7d90fb Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Oct 2015 18:56:07 +0300 Subject: [PATCH 018/106] UAVCAN CMakeLists - removed useless definitions --- src/modules/uavcan/CMakeLists.txt | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/modules/uavcan/CMakeLists.txt b/src/modules/uavcan/CMakeLists.txt index b8c899840a..ce348ec910 100644 --- a/src/modules/uavcan/CMakeLists.txt +++ b/src/modules/uavcan/CMakeLists.txt @@ -37,15 +37,12 @@ set(UAVCAN_PLATFORM stm32 CACHE STRING "uavcan platform") string(TOUPPER "${OS}" OS_UPPER) add_definitions( -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 - -DUAVCAN_MAX_NETWORK_SIZE_HINT=16 -DUAVCAN_MEM_POOL_BLOCK_SIZE=48 -DUAVCAN_NO_ASSERTIONS -DUAVCAN_PLATFORM=stm32 -DUAVCAN_STM32_${OS_UPPER}=1 -DUAVCAN_STM32_NUM_IFACES=2 -DUAVCAN_STM32_TIMER_NUMBER=5 - -DUAVCAN_USE_CPP03=ON - -DUAVCAN_USE_EXTERNAL_SNPRINT ) add_subdirectory(libuavcan EXCLUDE_FROM_ALL) From 407191d4ab1a90bb413c3db1241cbba8adb95172 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Oct 2015 19:37:18 +0300 Subject: [PATCH 019/106] UAVCAN driver transformed to use global memory pool --- src/modules/uavcan/uavcan_main.cpp | 3 ++- src/modules/uavcan/uavcan_main.hpp | 20 ++++++++++---- src/modules/uavcan/uavcan_servers.cpp | 4 +-- src/modules/uavcan/uavcan_servers.hpp | 26 ++++--------------- .../uavcan/uavcan_virtual_can_driver.hpp | 18 +++++-------- 5 files changed, 31 insertions(+), 40 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 1ba9d345fb..871551dd2c 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -77,7 +77,8 @@ UavcanNode *UavcanNode::_instance; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), - _node(can_driver, system_clock), + _pool_allocator(MemoryPoolBlockCapacitySoftLimit, MemoryPoolBlockCapacityHardLimit), + _node(can_driver, system_clock, _pool_allocator), _node_mutex(), _esc_controller(_node), _time_sync_master(_node), diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 6a1d2f391d..05ceca83f6 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -80,7 +81,6 @@ class UavcanNode : public device::CDev static constexpr unsigned PollTimeoutMs = 10; - static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize; /* * This memory is reserved for uavcan to use for queuing CAN frames. * At 1Mbit there is approximately one CAN frame every 145 uS. @@ -96,8 +96,10 @@ class UavcanNode : public device::CDev static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At static constexpr unsigned StackSize = 1800; + static constexpr unsigned MemoryPoolBlockCapacitySoftLimit = 250; + static constexpr unsigned MemoryPoolBlockCapacityHardLimit = 500; + public: - typedef uavcan::Node Node; typedef uavcan_stm32::CanInitHelper CanInitHelper; enum eServerAction {None, Start, Stop, CheckFW , Busy}; @@ -109,7 +111,7 @@ public: static int start(uavcan::NodeID node_id, uint32_t bitrate); - Node &get_node() { return _node; } + uavcan::Node<> &get_node() { return _node; } // TODO: move the actuator mixing stuff into the ESC controller class static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); @@ -130,8 +132,8 @@ public: int set_param(int remote_node_id, const char *name, char *value); int get_param(int remote_node_id, const char *name); int reset_node(int remote_node_id); -private: +private: void fill_node_info(); int init(uavcan::NodeID node_id); void node_spin_once(); @@ -167,7 +169,15 @@ private: static UavcanNode *_instance; ///< singleton pointer - Node _node; ///< library instance + struct MemoryPoolSynchronizer + { + const ::irqstate_t state = ::irqsave(); + ~MemoryPoolSynchronizer() { ::irqrestore(state); } + }; + + uavcan::HeapBasedPoolAllocator _pool_allocator; + + uavcan::Node<> _node; ///< library instance pthread_mutex_t _node_mutex; px4_sem_t _server_command_sem; UavcanEscController _esc_controller; diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 713a4e00f7..8ae013ce48 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -85,8 +85,8 @@ UavcanServers *UavcanServers::_instance; UavcanServers::UavcanServers(uavcan::INode &main_node) : _subnode_thread(-1), - _vdriver(NumIfaces, uavcan_stm32::SystemClock::instance()), - _subnode(_vdriver, uavcan_stm32::SystemClock::instance()), + _vdriver(NumIfaces, uavcan_stm32::SystemClock::instance(), main_node.getAllocator(), VirtualIfaceBlockAllocationQuota), + _subnode(_vdriver, uavcan_stm32::SystemClock::instance(), main_node.getAllocator()), _main_node(main_node), _tracer(), _storage_backend(), diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp index 0b83baa941..9bf9705771 100644 --- a/src/modules/uavcan/uavcan_servers.hpp +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -81,24 +81,10 @@ class UavcanServers { static constexpr unsigned NumIfaces = 1; // UAVCAN_STM32_NUM_IFACES - static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize; - - static constexpr unsigned MaxCanFramesPerTransfer = 63; - - /** - * This number is based on the worst case max number of frames per interface. With - * MemPoolBlockSize set at 48 this is 6048 Bytes. - * - * The servers can be forced to use the primary interface only, this can be achieved simply by passing - * 1 instead of UAVCAN_STM32_NUM_IFACES into the constructor of the virtual CAN driver. - */ - static constexpr unsigned QueuePoolSize = - (NumIfaces * uavcan::MemPoolBlockSize * MaxCanFramesPerTransfer); - static constexpr unsigned StackSize = 6000; static constexpr unsigned Priority = 120; - typedef uavcan::SubNode SubNode; + static constexpr unsigned VirtualIfaceBlockAllocationQuota = 80; public: UavcanServers(uavcan::INode &main_node); @@ -108,7 +94,7 @@ public: static int start(uavcan::INode &main_node); static int stop(); - SubNode &get_node() { return _subnode; } + uavcan::SubNode<> &get_node() { return _subnode; } static UavcanServers *instance() { return _instance; } @@ -131,12 +117,10 @@ private: static UavcanServers *_instance; ///< singleton pointer - typedef VirtualCanDriver vCanDriver; + VirtualCanDriver _vdriver; - vCanDriver _vdriver; - - uavcan::SubNode _subnode; ///< library instance - uavcan::INode &_main_node; ///< library instance + uavcan::SubNode<> _subnode; + uavcan::INode &_main_node; uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer; uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend; diff --git a/src/modules/uavcan/uavcan_virtual_can_driver.hpp b/src/modules/uavcan/uavcan_virtual_can_driver.hpp index 1c861e481c..500750c441 100644 --- a/src/modules/uavcan/uavcan_virtual_can_driver.hpp +++ b/src/modules/uavcan/uavcan_virtual_can_driver.hpp @@ -329,11 +329,7 @@ public: /** * Objects of this class are owned by the sub-node thread. * This class does not use heap memory. - * @tparam SharedMemoryPoolSize Amount of memory, in bytes, that will be statically allocated for the - * memory pool that will be shared across all interfaces for RX/TX queues. - * Typically this value should be no less than 4K per interface. */ -template class VirtualCanDriver : public uavcan::ICanDriver, public uavcan::IRxFrameListener, public ITxQueueInjector, @@ -402,7 +398,7 @@ class VirtualCanDriver : public uavcan::ICanDriver, Event event_; ///< Used to unblock the select() call when IO happens. pthread_mutex_t driver_mutex_; ///< Shared across all ifaces - uavcan::PoolAllocator allocator_; ///< Shared across all ifaces + uavcan::IPoolAllocator& allocator_; ///< Shared across all ifaces uavcan::LazyConstructor ifaces_[uavcan::MaxCanIfaces]; const unsigned num_ifaces_; uavcan::ISystemClock &clock_; @@ -476,7 +472,11 @@ class VirtualCanDriver : public uavcan::ICanDriver, } public: - VirtualCanDriver(unsigned arg_num_ifaces, uavcan::ISystemClock &system_clock) : + VirtualCanDriver(unsigned arg_num_ifaces, + uavcan::ISystemClock &system_clock, + uavcan::IPoolAllocator& allocator, + unsigned virtual_iface_block_allocation_quota) : + allocator_(allocator), num_ifaces_(arg_num_ifaces), clock_(system_clock) { @@ -485,11 +485,7 @@ public: assert(num_ifaces_ > 0 && num_ifaces_ <= uavcan::MaxCanIfaces); - const unsigned quota_per_iface = allocator_.getNumBlocks() / num_ifaces_; - const unsigned quota_per_queue = quota_per_iface; // 2x overcommit - - UAVCAN_TRACE("VirtualCanDriver", "Total blocks: %u, quota per queue: %u", - unsigned(allocator_.getNumBlocks()), unsigned(quota_per_queue)); + const unsigned quota_per_queue = virtual_iface_block_allocation_quota; // 2x overcommit for (unsigned i = 0; i < num_ifaces_; i++) { ifaces_[i].template construct Date: Fri, 16 Oct 2015 20:37:47 +0300 Subject: [PATCH 020/106] Libuavcan update --- src/modules/uavcan/libuavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/libuavcan b/src/modules/uavcan/libuavcan index 4e4d9b7854..9a432c0323 160000 --- a/src/modules/uavcan/libuavcan +++ b/src/modules/uavcan/libuavcan @@ -1 +1 @@ -Subproject commit 4e4d9b7854b7821f42fd83d89885cc489cc18c33 +Subproject commit 9a432c03231781403f2a4808846183623cbfc49e From a570d1de7d20f6d0817fd6210473a58db9c3aa13 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Oct 2015 22:59:17 +0300 Subject: [PATCH 021/106] UAVCAN memory usage status and shrink --- src/modules/uavcan/uavcan_main.cpp | 21 ++++++++++++++++++++- src/modules/uavcan/uavcan_main.hpp | 2 ++ 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 871551dd2c..6508e339f0 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -1120,6 +1120,13 @@ UavcanNode::print_info() (void)pthread_mutex_lock(&_node_mutex); + // Memory status + printf("Pool allocator status:\n"); + printf("\tCapacity hard/soft: %u/%u blocks\n", + _pool_allocator.getBlockCapacityHardLimit(), _pool_allocator.getBlockCapacity()); + printf("\tReserved: %u blocks\n", _pool_allocator.getNumReservedBlocks()); + printf("\tAllocated: %u blocks\n", _pool_allocator.getNumAllocatedBlocks()); + // ESC mixer status printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); @@ -1170,13 +1177,20 @@ UavcanNode::print_info() (void)pthread_mutex_unlock(&_node_mutex); } +void UavcanNode::shrink() +{ + (void)pthread_mutex_lock(&_node_mutex); + _pool_allocator.shrink(); + (void)pthread_mutex_unlock(&_node_mutex); +} + /* * App entry point */ static void print_usage() { warnx("usage: \n" - "\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw|param [set|get|list|save] nodeid [name] [value]|reset nodeid}"); + "\tuavcan {start [fw]|status|stop [all|fw]|shrink|arm|disarm|update fw|param [set|get|list|save] nodeid [name] [value]|reset nodeid}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -1252,6 +1266,11 @@ int uavcan_main(int argc, char *argv[]) ::exit(0); } + if (!std::strcmp(argv[1], "shrink")) { + inst->shrink(); + ::exit(0); + } + if (!std::strcmp(argv[1], "arm")) { inst->arm_actuators(true); ::exit(0); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 05ceca83f6..00cdaf418f 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -123,6 +123,8 @@ public: void print_info(); + void shrink(); + static UavcanNode *instance() { return _instance; } static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver); int fw_server(eServerAction action); From 1ace017cb88319829c80e9571d71c903a4bc78a4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Oct 2015 23:17:07 +0300 Subject: [PATCH 022/106] Deallocating memory used by UAVCAN virtual iface on destruction --- src/modules/uavcan/uavcan_virtual_can_driver.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/uavcan/uavcan_virtual_can_driver.hpp b/src/modules/uavcan/uavcan_virtual_can_driver.hpp index 500750c441..57b664068c 100644 --- a/src/modules/uavcan/uavcan_virtual_can_driver.hpp +++ b/src/modules/uavcan/uavcan_virtual_can_driver.hpp @@ -114,6 +114,14 @@ public: uavcan::IsDynamicallyAllocatable::check(); } + ~Queue() + { + while (!isEmpty()) + { + pop(); + } + } + bool isEmpty() const { return list_.isEmpty(); } /** From edfb19d9afa3bcba28dbf28227ee11a13a3dc250 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 01:06:35 +0300 Subject: [PATCH 023/106] Libuavcan update --- src/modules/uavcan/libuavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/libuavcan b/src/modules/uavcan/libuavcan index 9a432c0323..9b092509c9 160000 --- a/src/modules/uavcan/libuavcan +++ b/src/modules/uavcan/libuavcan @@ -1 +1 @@ -Subproject commit 9a432c03231781403f2a4808846183623cbfc49e +Subproject commit 9b092509c93794ec60484b2864be7a2995aefac6 From ca4e55fec301bb80af20a94fdfcdbf8d9551a83d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 03:24:59 +0300 Subject: [PATCH 024/106] UAVCAN allocator as a dedicated type; reporting a warning if memory leak is deetcted upon destruction --- src/modules/uavcan/allocator.hpp | 73 ++++++++++++++++++++++++++++++ src/modules/uavcan/uavcan_main.cpp | 1 - src/modules/uavcan/uavcan_main.hpp | 12 +---- 3 files changed, 75 insertions(+), 11 deletions(-) create mode 100644 src/modules/uavcan/allocator.hpp diff --git a/src/modules/uavcan/allocator.hpp b/src/modules/uavcan/allocator.hpp new file mode 100644 index 0000000000..db34aa04fb --- /dev/null +++ b/src/modules/uavcan/allocator.hpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (C) 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. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko + */ + +#pragma once + +#include +#include +#include + +// TODO: Entire UAVCAN application should be moved into a namespace later; this is the first step. +namespace uavcan_node +{ + +struct AllocatorSynchronizer +{ + const ::irqstate_t state = ::irqsave(); + ~AllocatorSynchronizer() { ::irqrestore(state); } +}; + +struct Allocator : public uavcan::HeapBasedPoolAllocator +{ + static constexpr unsigned CapacitySoftLimit = 250; + static constexpr unsigned CapacityHardLimit = 500; + + Allocator() : + uavcan::HeapBasedPoolAllocator(CapacitySoftLimit, CapacityHardLimit) + { } + + ~Allocator() + { + if (getNumAllocatedBlocks() > 0) + { + warnx("UAVCAN LEAKS MEMORY: %u BLOCKS (%u BYTES) LOST", + getNumAllocatedBlocks(), getNumAllocatedBlocks() * uavcan::MemPoolBlockSize); + } + } +}; + +} diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 6508e339f0..affaa36b0a 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -77,7 +77,6 @@ UavcanNode *UavcanNode::_instance; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), - _pool_allocator(MemoryPoolBlockCapacitySoftLimit, MemoryPoolBlockCapacityHardLimit), _node(can_driver, system_clock, _pool_allocator), _node_mutex(), _esc_controller(_node), diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 00cdaf418f..b91765591d 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -56,6 +56,7 @@ #include "sensors/sensor_bridge.hpp" #include "uavcan_servers.hpp" +#include "allocator.hpp" /** * @file uavcan_main.hpp @@ -96,9 +97,6 @@ class UavcanNode : public device::CDev static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At static constexpr unsigned StackSize = 1800; - static constexpr unsigned MemoryPoolBlockCapacitySoftLimit = 250; - static constexpr unsigned MemoryPoolBlockCapacityHardLimit = 500; - public: typedef uavcan_stm32::CanInitHelper CanInitHelper; enum eServerAction {None, Start, Stop, CheckFW , Busy}; @@ -171,13 +169,7 @@ private: static UavcanNode *_instance; ///< singleton pointer - struct MemoryPoolSynchronizer - { - const ::irqstate_t state = ::irqsave(); - ~MemoryPoolSynchronizer() { ::irqrestore(state); } - }; - - uavcan::HeapBasedPoolAllocator _pool_allocator; + uavcan_node::Allocator _pool_allocator; uavcan::Node<> _node; ///< library instance pthread_mutex_t _node_mutex; From 109bee855b3d734b63fe88bd3eec5fdc8d443b58 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 03:49:02 +0300 Subject: [PATCH 025/106] Node on leaked memory in UAVCAN driver --- src/modules/uavcan/uavcan_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index affaa36b0a..ed5814a3f4 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -1069,6 +1069,7 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) unsigned buflen = strnlen(buf, 1024); if (_mixers == nullptr) { + // TODO: Do we have to delete it when stopping? _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); } From 6c4f09c0e4fe3a243bb68c9bcac03110b1c2a4d1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 04:05:35 +0300 Subject: [PATCH 026/106] Fixed memory leak in UAVCAN baro driver --- src/modules/uavcan/sensors/baro.cpp | 15 ++++----------- src/modules/uavcan/sensors/baro.hpp | 4 +++- 2 files changed, 7 insertions(+), 12 deletions(-) diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 694a3988a1..36f28a423c 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -44,7 +44,7 @@ UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) : UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)), _sub_air_pressure_data(node), _sub_air_temperature_data(node), - _reports(nullptr) + _reports(2, sizeof(baro_report)) { } int UavcanBarometerBridge::init() @@ -55,13 +55,6 @@ int UavcanBarometerBridge::init() return res; } - /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); - - if (_reports == nullptr) { - return -1; - } - res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb)); if (res < 0) { @@ -91,7 +84,7 @@ ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t bufl } while (count--) { - if (_reports->get(baro_buf)) { + if (_reports.get(baro_buf)) { ret += sizeof(*baro_buf); baro_buf++; } @@ -132,7 +125,7 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { + if (!_reports.resize(arg)) { irqrestore(flags); return -ENOMEM; } @@ -186,7 +179,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; // add to the ring buffer - _reports->force(&report); + _reports.force(&report); publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index 38cf6ab89a..ed1e308515 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -76,8 +76,10 @@ private: uavcan::Subscriber _sub_air_pressure_data; uavcan::Subscriber _sub_air_temperature_data; + + ringbuffer::RingBuffer _reports; + unsigned _msl_pressure = 101325; - ringbuffer::RingBuffer *_reports; float last_temperature_kelvin = 0.0f; }; From 9d86dbb6a1c3625f912ffc9470c0184c274d0da2 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 19:50:01 +0300 Subject: [PATCH 027/106] Fixed memory leaks in the primary UAVCAN thread --- src/modules/uavcan/uavcan_main.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index ed5814a3f4..3d7580b534 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -144,10 +144,9 @@ UavcanNode::~UavcanNode() } while (_task != -1); } - /* clean up the alternate device node */ - // unregister_driver(PWM_OUTPUT_DEVICE_PATH); - - ::close(_armed_sub); + (void)::close(_armed_sub); + (void)::close(_test_motor_sub); + (void)::close(_actuator_direct_sub); // Removing the sensor bridges auto br = _sensor_bridges.getHead(); @@ -166,6 +165,10 @@ UavcanNode::~UavcanNode() pthread_mutex_destroy(&_node_mutex); px4_sem_destroy(&_server_command_sem); + // Is it allowed to delete it like that? + if (_mixers != nullptr) { + delete _mixers; + } } int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver) @@ -965,6 +968,8 @@ int UavcanNode::run() } } + (void)::close(busevent_fd); + teardown(); warnx("exiting."); @@ -1069,7 +1074,6 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) unsigned buflen = strnlen(buf, 1024); if (_mixers == nullptr) { - // TODO: Do we have to delete it when stopping? _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); } From 5b9fc2d9fa16a7ac9f6ddfb4f0a29d8ef4b3ebfb Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 20:29:49 +0300 Subject: [PATCH 028/106] Proper termination of UAVACN server thread --- src/modules/uavcan/uavcan_servers.cpp | 17 ++++++++++------- src/modules/uavcan/uavcan_servers.hpp | 1 + 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 8ae013ce48..ca5bcede66 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -141,13 +141,14 @@ int UavcanServers::stop() return -1; } - _instance = nullptr; - - if (server->_subnode_thread != -1) { - pthread_cancel(server->_subnode_thread); - pthread_join(server->_subnode_thread, NULL); + if (server->_subnode_thread) { + warnx("stopping fw srv thread..."); + server->_subnode_thread_should_exit = true; + (void)pthread_join(server->_subnode_thread, NULL); } + _instance = nullptr; + server->_main_node.getDispatcher().removeRxFrameListener(); delete server; @@ -334,7 +335,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids)); _esc_enumeration_index = 0; - while (1) { + while (!_subnode_thread_should_exit) { if (_check_fw == true) { _check_fw = false; @@ -554,7 +555,9 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) } } - warnx("exiting."); + _subnode_thread_should_exit = false; + + warnx("exiting"); return (pthread_addr_t) 0; } diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp index 9bf9705771..3cf4cbb4da 100644 --- a/src/modules/uavcan/uavcan_servers.hpp +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -110,6 +110,7 @@ public: private: pthread_t _subnode_thread; pthread_mutex_t _subnode_mutex; + volatile bool _subnode_thread_should_exit = false; int init(); From 53ed0425daee0acaea4179af848f9763bfb0b11e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 20:49:08 +0300 Subject: [PATCH 029/106] Libuavcan set to master --- src/modules/uavcan/libuavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/libuavcan b/src/modules/uavcan/libuavcan index 9b092509c9..ed1d71e639 160000 --- a/src/modules/uavcan/libuavcan +++ b/src/modules/uavcan/libuavcan @@ -1 +1 @@ -Subproject commit 9b092509c93794ec60484b2864be7a2995aefac6 +Subproject commit ed1d71e639543ea5743a839c313c53237ab3a27d From 72e651bedac613c1b3e96b47c7d4feffc8aef10d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 21:08:03 +0300 Subject: [PATCH 030/106] UAVCAN virtual iface: removed a useless class field --- src/modules/uavcan/uavcan_virtual_can_driver.hpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/modules/uavcan/uavcan_virtual_can_driver.hpp b/src/modules/uavcan/uavcan_virtual_can_driver.hpp index 57b664068c..956caf11ee 100644 --- a/src/modules/uavcan/uavcan_virtual_can_driver.hpp +++ b/src/modules/uavcan/uavcan_virtual_can_driver.hpp @@ -404,9 +404,8 @@ class VirtualCanDriver : public uavcan::ICanDriver, } }; - Event event_; ///< Used to unblock the select() call when IO happens. + Event event_; ///< Used to unblock the select() call when IO happens. pthread_mutex_t driver_mutex_; ///< Shared across all ifaces - uavcan::IPoolAllocator& allocator_; ///< Shared across all ifaces uavcan::LazyConstructor ifaces_[uavcan::MaxCanIfaces]; const unsigned num_ifaces_; uavcan::ISystemClock &clock_; @@ -484,7 +483,6 @@ public: uavcan::ISystemClock &system_clock, uavcan::IPoolAllocator& allocator, unsigned virtual_iface_block_allocation_quota) : - allocator_(allocator), num_ifaces_(arg_num_ifaces), clock_(system_clock) { @@ -496,8 +494,9 @@ public: const unsigned quota_per_queue = virtual_iface_block_allocation_quota; // 2x overcommit for (unsigned i = 0; i < num_ifaces_; i++) { - ifaces_[i].template construct(allocator_, clock_, driver_mutex_, quota_per_queue); + ifaces_[i].template + construct + (allocator, clock_, driver_mutex_, quota_per_queue); } } From 635bfb6a7b4df37add20af8f5582b23ed992e03d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 18 Oct 2015 03:55:47 +0300 Subject: [PATCH 031/106] CAN driver RX queue reduced to 21 frames per interface; poll() timeout set to 3 ms --- src/modules/uavcan/uavcan_main.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index b91765591d..b236946e24 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -80,7 +80,7 @@ class UavcanNode : public device::CDev static constexpr unsigned FramePerSecond = MaxBitRatePerSec / bitPerFrame; static constexpr unsigned FramePerMSecond = ((FramePerSecond / 1000) + 1); - static constexpr unsigned PollTimeoutMs = 10; + static constexpr unsigned PollTimeoutMs = 3; /* * This memory is reserved for uavcan to use for queuing CAN frames. From e06c46da03d75dd01423485f91c0502724f1aa30 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 18 Oct 2015 03:56:09 +0300 Subject: [PATCH 032/106] uavcan status output extended with CAN error reporting --- src/modules/uavcan/uavcan_main.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 3d7580b534..4b77c91648 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -1131,6 +1131,13 @@ UavcanNode::print_info() printf("\tReserved: %u blocks\n", _pool_allocator.getNumReservedBlocks()); printf("\tAllocated: %u blocks\n", _pool_allocator.getNumAllocatedBlocks()); + // CAN driver status + printf("CAN status:\n"); + for (unsigned i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) { + auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i); + printf("\tCAN%u: errors: %llu\n", unsigned(i + 1), iface->getErrorCount()); + } + // ESC mixer status printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); From 96a12a60278f9c2aae7a46d0cafa7b2cf3fad457 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 18 Oct 2015 04:13:25 +0300 Subject: [PATCH 033/106] UAVCAN extended status reporting --- src/modules/uavcan/uavcan_main.cpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 4b77c91648..00671decd2 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -1131,11 +1131,24 @@ UavcanNode::print_info() printf("\tReserved: %u blocks\n", _pool_allocator.getNumReservedBlocks()); printf("\tAllocated: %u blocks\n", _pool_allocator.getNumAllocatedBlocks()); + // UAVCAN node perfcounters + printf("UAVCAN node status:\n"); + printf("\tInternal failures: %llu\n", _node.getInternalFailureCount()); + printf("\tTransfer errors: %llu\n", _node.getDispatcher().getTransferPerfCounter().getErrorCount()); + printf("\tRX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getRxTransferCount()); + printf("\tTX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getTxTransferCount()); + // CAN driver status - printf("CAN status:\n"); for (unsigned i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) { + printf("CAN%u status:\n", unsigned(i + 1)); + auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i); - printf("\tCAN%u: errors: %llu\n", unsigned(i + 1), iface->getErrorCount()); + printf("\tHW errors: %llu\n", iface->getErrorCount()); + + auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i); + printf("\tIO errors: %llu\n", iface_perf_cnt.errors); + printf("\tRX frames: %llu\n", iface_perf_cnt.frames_rx); + printf("\tTX frames: %llu\n", iface_perf_cnt.frames_tx); } // ESC mixer status From 34ff90c62483bd4c983ae8fcf4ed6bede9792ce0 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Tue, 27 Oct 2015 09:41:40 +0100 Subject: [PATCH 034/106] 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 93e557ab757259e9d43510b52511606eacc1fbf3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 27 Oct 2015 09:58:43 +0100 Subject: [PATCH 035/106] 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 036/106] 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 6634952a0c95b399d7884b9602886ce0a987561e Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 29 Oct 2015 07:54:38 +0100 Subject: [PATCH 037/106] 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 038/106] 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 9b5b2a4cccb092bcb42ae2a5025ce21a3def4d60 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 3 Nov 2015 16:47:09 +0100 Subject: [PATCH 039/106] 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 f6a9647fe61167e77af955ec5d8756253e78f8db Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Fri, 30 Oct 2015 17:52:17 +0530 Subject: [PATCH 040/106] 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 27df787bfffbc5858e2fd3e41491e293521c8683 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Thu, 5 Nov 2015 19:29:04 -0500 Subject: [PATCH 041/106] 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 318eeb04945c21ebfa478bb12656ebb1fed3e141 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Fri, 6 Nov 2015 13:11:02 +0100 Subject: [PATCH 042/106] 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 043/106] 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 044/106] 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 045/106] 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 046/106] 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 047/106] 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 048/106] 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 049/106] 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 050/106] 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 051/106] 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 052/106] 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 053/106] 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 054/106] 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 055/106] 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 056/106] 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 057/106] 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 058/106] 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 059/106] 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 060/106] 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 061/106] 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 062/106] 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 063/106] 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 064/106] 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 065/106] 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 066/106] 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 067/106] 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 068/106] 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 069/106] 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 070/106] 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 071/106] 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 072/106] 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 073/106] 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 074/106] 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 075/106] 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 076/106] 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 077/106] 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 5fcfdb759c2a83c3e13235b0f71d8d6299d46c33 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Tue, 10 Nov 2015 19:33:14 +0530 Subject: [PATCH 078/106] commander : hotplug sensor support, better failure reporting --- msg/vehicle_status.msg | 3 + src/modules/commander/PreflightCheck.cpp | 68 ++++++++--------- src/modules/commander/PreflightCheck.h | 2 +- src/modules/commander/commander.cpp | 76 ++++++++++++++----- .../commander/state_machine_helper.cpp | 46 +++++++---- src/modules/commander/state_machine_helper.h | 2 +- src/modules/systemlib/rc_check.c | 37 ++++++--- src/modules/systemlib/rc_check.h | 2 +- 8 files changed, 153 insertions(+), 83 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index e575b9d5e3..6d01b3b58e 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -120,6 +120,8 @@ bool in_transition_mode bool condition_battery_voltage_valid bool condition_system_in_air_restore # true if we can restore in mid air bool condition_system_sensors_initialized +bool condition_system_prearm_error_reported # true if errors have already been reported +bool condition_system_hotplug_timeout # true if the hotplug sensor search is over bool condition_system_returned_to_home bool condition_auto_mission_available bool condition_global_position_valid # set to true by the commander app if the quality of the position estimate is good enough to use it for navigation @@ -139,6 +141,7 @@ bool rc_signal_lost_cmd # true if RC lost mode is commanded bool rc_input_blocked # set if RC input should be ignored temporarily uint8 rc_input_mode # set to 1 to disable the RC input, 2 to enable manual control to RC in mapping. +bool data_link_found_new # new datalink to GCS found bool data_link_lost # datalink to GCS lost bool data_link_lost_cmd # datalink to GCS lost mode commanded uint8 data_link_lost_counter # counts unique data link lost events diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 2a8a0695cf..bfeabb9e7e 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -114,7 +114,7 @@ int check_calibration(int fd, const char* param_template, int &devid) return !calibration_found; } -static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id) +static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail) { bool success = true; @@ -124,7 +124,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in if (fd < 0) { if (!optional) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance); } @@ -134,7 +134,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id); if (ret) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance); success = false; goto out; @@ -143,7 +143,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in ret = px4_ioctl(fd, MAGIOCSELFTEST, 0); if (ret != OK) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance); success = false; goto out; @@ -154,7 +154,7 @@ out: return success; } -static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id) +static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail) { bool success = true; @@ -164,7 +164,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, if (fd < 0) { if (!optional) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance); } @@ -174,7 +174,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id); if (ret) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance); success = false; goto out; @@ -183,7 +183,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance); success = false; goto out; @@ -200,13 +200,13 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { - mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming"); + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming"); /* this is frickin' fatal */ success = false; goto out; } } else { - mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ"); + if(report_fail) mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ"); /* this is frickin' fatal */ success = false; goto out; @@ -219,7 +219,7 @@ out: return success; } -static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id) +static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail) { bool success = true; @@ -229,7 +229,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev if (fd < 0) { if (!optional) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance); } @@ -239,7 +239,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id); if (ret) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance); success = false; goto out; @@ -248,7 +248,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev ret = px4_ioctl(fd, GYROIOCSELFTEST, 0); if (ret != OK) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance); success = false; goto out; @@ -259,7 +259,7 @@ out: return success; } -static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id) +static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail) { bool success = true; @@ -269,7 +269,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev if (fd < 0) { if (!optional) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance); } @@ -294,7 +294,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev return success; } -static bool airspeedCheck(int mavlink_fd, bool optional) +static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail) { bool success = true; int ret; @@ -304,13 +304,13 @@ static bool airspeedCheck(int mavlink_fd, bool optional) if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) { - mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); success = false; goto out; } if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) { - mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); // XXX do not make this fatal yet } @@ -319,7 +319,7 @@ out: return success; } -static bool gnssCheck(int mavlink_fd) +static bool gnssCheck(int mavlink_fd, bool report_fail) { bool success = true; @@ -342,7 +342,7 @@ static bool gnssCheck(int mavlink_fd) //Report failure to detect module if(!success) { - mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); } px4_close(gpsSub); @@ -350,7 +350,7 @@ static bool gnssCheck(int mavlink_fd) } bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, - bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic) + bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures) { bool failed = false; @@ -365,7 +365,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro bool required = (i < max_mandatory_mag_count); int device_id = -1; - if (!magnometerCheck(mavlink_fd, i, !required, device_id) && required) { + if (!magnometerCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) { failed = true; } @@ -376,7 +376,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check if the primary device is present */ if (!prime_found && prime_id != 0) { - mavlink_log_critical(mavlink_fd, "warning: primary compass not operational"); + if(reportFailures) mavlink_log_critical(mavlink_fd, "Warning: Primary compass not found"); failed = true; } } @@ -392,7 +392,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro bool required = (i < max_mandatory_accel_count); int device_id = -1; - if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) { + if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id, reportFailures) && required) { failed = true; } @@ -403,7 +403,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check if the primary device is present */ if (!prime_found && prime_id != 0) { - mavlink_log_critical(mavlink_fd, "warning: primary accelerometer not operational"); + if(reportFailures) mavlink_log_critical(mavlink_fd, "Warning: Primary accelerometer not found"); failed = true; } } @@ -419,7 +419,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro bool required = (i < max_mandatory_gyro_count); int device_id = -1; - if (!gyroCheck(mavlink_fd, i, !required, device_id) && required) { + if (!gyroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) { failed = true; } @@ -430,7 +430,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check if the primary device is present */ if (!prime_found && prime_id != 0) { - mavlink_log_critical(mavlink_fd, "warning: primary gyro not operational"); + if(reportFailures) mavlink_log_critical(mavlink_fd, "Warning: Primary gyro not found"); failed = true; } } @@ -446,7 +446,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro bool required = (i < max_mandatory_baro_count); int device_id = -1; - if (!baroCheck(mavlink_fd, i, !required, device_id) && required) { + if (!baroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) { failed = true; } @@ -458,29 +458,29 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro // TODO there is no logic in place to calibrate the primary baro yet // // check if the primary device is present if (!prime_found && prime_id != 0) { - mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational"); + if(reportFailures) mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational"); failed = true; } } /* ---- AIRSPEED ---- */ if (checkAirspeed) { - if (!airspeedCheck(mavlink_fd, true)) { + if (!airspeedCheck(mavlink_fd, true, reportFailures)) { failed = true; } } /* ---- RC CALIBRATION ---- */ if (checkRC) { - if (rc_calibration_check(mavlink_fd) != OK) { - mavlink_log_critical(mavlink_fd, "RC calibration check failed"); + if (rc_calibration_check(mavlink_fd, reportFailures) != OK) { + if(reportFailures) mavlink_log_critical(mavlink_fd, "RC calibration check failed"); failed = true; } } /* ---- Global Navigation Satellite System receiver ---- */ if (checkGNSS) { - if(!gnssCheck(mavlink_fd)) { + if(!gnssCheck(mavlink_fd, reportFailures)) { failed = true; } } diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index b6423a4d9a..058d6b8956 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -67,7 +67,7 @@ namespace Commander * true if the GNSS receiver should be checked **/ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, - bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic = false); + bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures = false); const unsigned max_mandatory_gyro_count = 1; const unsigned max_optional_gyro_count = 3; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3573f5d6eb..32b24a0be3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -148,6 +148,8 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 +#define HOTPLUG_SENS_TIMEOUT (8 * 1000 * 1000) /**< wait for hotplug sensors to come online for upto 10 seconds */ + #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 10000000 @@ -377,9 +379,12 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "check")) { int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); - int checkres = prearm_check(&status, mavlink_fd_local); + int checkres = 0; + checkres = preflight_check(&status, mavlink_fd_local, false); + warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED"); + checkres = preflight_check(&status, mavlink_fd_local, true); + warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED"); px4_close(mavlink_fd_local); - warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); return 0; } @@ -891,6 +896,7 @@ int commander_thread_main(int argc, char *argv[]) /* not yet initialized */ commander_initialized = false; + bool sensor_fail_tune_played = false; bool arm_tune_played = false; bool was_landed = true; bool was_armed = false; @@ -1020,6 +1026,9 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; + + status.condition_system_prearm_error_reported = true; + status.condition_system_hotplug_timeout = false; status.counter++; status.timestamp = hrt_absolute_time(); @@ -1101,7 +1110,7 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; - rc_calibration_check(mavlink_fd); + rc_calibration_check(mavlink_fd, true); /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); @@ -1243,6 +1252,7 @@ int commander_thread_main(int argc, char *argv[]) // Run preflight check int32_t rc_in_off = 0; + bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; param_get(_param_autostart_id, &autostart_id); param_get(_param_rc_in_off, &rc_in_off); status.rc_input_mode = rc_in_off; @@ -1251,14 +1261,10 @@ int commander_thread_main(int argc, char *argv[]) status.condition_system_sensors_initialized = false; set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } else { - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, - checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check); - if (!status.condition_system_sensors_initialized) { - set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune - } - else { + // sensor diagnostics done continiously, not just at boot so don't warn about any issues just yet + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, + checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, false); set_tune_override(TONE_STARTUP_TUNE); //normal boot tune - } } commander_boot_timestamp = hrt_absolute_time(); @@ -1354,11 +1360,11 @@ int commander_thread_main(int argc, char *argv[]) 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); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); } /* re-check RC calibration */ - rc_calibration_check(mavlink_fd); + rc_calibration_check(mavlink_fd, true); } /* Safety parameters */ @@ -1446,6 +1452,8 @@ int commander_thread_main(int argc, char *argv[]) !armed.armed) { bool chAirspeed = false; + bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; + /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { @@ -1456,11 +1464,11 @@ int commander_thread_main(int argc, char *argv[]) if (is_hil_setup(autostart_id)) { /* HIL configuration: check only RC input */ (void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false, true); } else { /* check sensors also */ (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); } } @@ -2177,9 +2185,14 @@ int commander_thread_main(int argc, char *argv[]) if (telemetry_lost[i] && hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { - /* only report a regain */ + /* report a regain */ if (telemetry_last_dl_loss[i] > 0) { mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i); + } else if (telemetry_last_dl_loss[i] == 0){ + /* report a new link */ + mavlink_and_console_log_info(mavlink_fd, "new data link connected : #%i", i); + status.data_link_found_new = true; + status_changed = true; } telemetry_lost[i] = false; @@ -2189,6 +2202,10 @@ int commander_thread_main(int argc, char *argv[]) /* telemetry was healthy also in last iteration * we don't have to check a timeout */ have_link = true; + if(status.data_link_found_new) { + status.data_link_found_new = false; + status_changed = true; + } } } else { @@ -2414,7 +2431,7 @@ int commander_thread_main(int argc, char *argv[]) } else { set_tune(TONE_STOP_TUNE); } - + /* reset arm_tune_played when disarmed */ if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) { @@ -2425,6 +2442,21 @@ int commander_thread_main(int argc, char *argv[]) arm_tune_played = false; } + + /* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */ + hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; + + if (!sensor_fail_tune_played && (!status.condition_system_sensors_initialized && hotplug_timeout)) { + set_tune_override(TONE_GPS_WARNING_TUNE); + sensor_fail_tune_played = true; + status_changed = true; + } + + /* update timeout flag */ + if(!(hotplug_timeout == status.condition_system_hotplug_timeout)) { + status.condition_system_hotplug_timeout = hotplug_timeout; + status_changed = true; + } counter++; @@ -2508,19 +2540,24 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu /* driving rgbled */ if (changed) { bool set_normal_color = false; - + bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; + /* set mode */ if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) { rgbled_set_mode(RGBLED_MODE_ON); set_normal_color = true; - } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || !status.condition_system_sensors_initialized) { + } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || (!status.condition_system_sensors_initialized && hotplug_timeout)) { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); rgbled_set_color(RGBLED_COLOR_RED); } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { rgbled_set_mode(RGBLED_MODE_BREATHE); set_normal_color = true; + + } else if (!status.condition_system_sensors_initialized && !hotplug_timeout) { + rgbled_set_mode(RGBLED_MODE_BREATHE); + set_normal_color = true; } else { // STANDBY_ERROR and other states rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL); @@ -3197,6 +3234,7 @@ void *commander_low_prio_loop(void *arg) // so this would be prone to false negatives. bool checkAirspeed = false; + bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { @@ -3204,7 +3242,7 @@ void *commander_low_prio_loop(void *arg) } 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); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 21789c9c66..bdf88920fa 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -93,8 +93,6 @@ static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = { "ARMING_STATE_IN_AIR_RESTORE", }; -static bool sensor_feedback_provided = false; - transition_result_t arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status const struct safety_s *safety, ///< current safety settings @@ -122,10 +120,18 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s */ int prearm_ret = OK; - /* only perform the check if we have to */ + /* only perform the pre-arm check if we have to */ if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { - prearm_ret = prearm_check(status, mavlink_fd); + prearm_ret = preflight_check(status, mavlink_fd, true /* pre-arm */ ); + } + /* re-run the pre-flight check as long as sensors are failing */ + if (!status->condition_system_sensors_initialized + && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || + new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) + && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { + prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */); + status->condition_system_sensors_initialized = !prearm_ret; } /* @@ -243,9 +249,11 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && (!status->condition_system_sensors_initialized)) { - if (!sensor_feedback_provided || (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { + if ((!status->condition_system_prearm_error_reported && + status->condition_system_hotplug_timeout) || + (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); - sensor_feedback_provided = true; + status->condition_system_prearm_error_reported = true; } feedback_provided = true; valid_transition = false; @@ -262,7 +270,11 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s /* reset feedback state */ if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR && valid_transition) { - sensor_feedback_provided = false; + status->condition_system_prearm_error_reported = false; + } + if(status->data_link_found_new == true) + { + status->condition_system_prearm_error_reported = false; } /* end of atomic state update */ @@ -731,11 +743,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en return status->nav_state != nav_state_old; } -int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) +int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm) { - /* at this point this equals the preflight check, but might add additional - * quantities later. + /* */ + bool reportFailures = false; + reportFailures = !status->condition_system_prearm_error_reported && status->condition_system_hotplug_timeout; + bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ @@ -743,12 +757,12 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) checkAirspeed = 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"); + bool preflight_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, reportFailures); + + if (status->usb_connected && prearm) { + preflight_ok = false; + if(reportFailures) mavlink_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); } - return !prearm_ok; + return !preflight_ok; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 084813f8cb..bdb7b13f28 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -65,6 +65,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe); -int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); +int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 25dea7a9b0..0f5d96f2b8 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -52,7 +52,7 @@ #define RC_INPUT_MAP_UNMAPPED 0 -int rc_calibration_check(int mavlink_fd) +int rc_calibration_check(int mavlink_fd, bool report_fail) { char nbuf[20]; @@ -74,7 +74,8 @@ int rc_calibration_check(int mavlink_fd) param_t map_parm = param_find(rc_map_mandatory[j]); if (map_parm == PARAM_INVALID) { - mavlink_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); + if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); } + /* give system time to flush error message in case there are more */ usleep(100000); map_fail_count++; @@ -86,14 +87,16 @@ int rc_calibration_check(int mavlink_fd) param_get(map_parm, &mapping); if (mapping > RC_INPUT_MAX_CHANNELS) { - mavlink_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); + if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); } + /* give system time to flush error message in case there are more */ usleep(100000); map_fail_count++; } if (mapping == 0) { - mavlink_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); + if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); } + /* give system time to flush error message in case there are more */ usleep(100000); map_fail_count++; @@ -144,35 +147,44 @@ int rc_calibration_check(int mavlink_fd) /* assert min..center..max ordering */ if (param_min < RC_INPUT_LOWEST_MIN_US) { count++; - mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); + + if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); } + /* give system time to flush error message in case there are more */ usleep(100000); } if (param_max > RC_INPUT_HIGHEST_MAX_US) { count++; - mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); + + if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); } + /* give system time to flush error message in case there are more */ usleep(100000); } if (param_trim < param_min) { count++; - mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); + + if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); } + /* give system time to flush error message in case there are more */ usleep(100000); } if (param_trim > param_max) { count++; - mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); + + if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); } + /* give system time to flush error message in case there are more */ usleep(100000); } /* assert deadzone is sane */ if (param_dz > RC_INPUT_MAX_DEADZONE_US) { - mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); + if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); } + /* give system time to flush error message in case there are more */ usleep(100000); count++; @@ -187,8 +199,11 @@ int rc_calibration_check(int mavlink_fd) if (channels_failed) { sleep(2); - mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", total_fail_count, - (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : ""); + + if (report_fail) mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", + total_fail_count, + (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : ""); + usleep(100000); } diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h index 035f63bef4..239c8a6a13 100644 --- a/src/modules/systemlib/rc_check.h +++ b/src/modules/systemlib/rc_check.h @@ -47,6 +47,6 @@ __BEGIN_DECLS * @return 0 / OK if RC calibration is ok, index + 1 of the first * channel that failed else (so 1 == first channel failed) */ -__EXPORT int rc_calibration_check(int mavlink_fd); +__EXPORT int rc_calibration_check(int mavlink_fd, bool report_fail); __END_DECLS From de39e7de62cc7bc4006d890fbd808708d83e8eac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Nov 2015 11:33:36 +0100 Subject: [PATCH 079/106] 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 From e8c7ee7ff2c5ccd818df35bce31f997d0c16d645 Mon Sep 17 00:00:00 2001 From: Bartosz Wawrzacz Date: Thu, 12 Nov 2015 15:03:06 +0100 Subject: [PATCH 080/106] enable logging of the RC RSSI value --- src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index be08851c25..af77d955e9 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1675,6 +1675,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_RC_MSG; /* Copy only the first 8 channels of 14 */ memcpy(log_msg.body.log_RC.channel, buf.rc.channels, sizeof(log_msg.body.log_RC.channel)); + log_msg.body.log_RC.rssi = buf.rc.rssi; log_msg.body.log_RC.channel_count = buf.rc.channel_count; log_msg.body.log_RC.signal_lost = buf.rc.signal_lost; LOGBUFFER_WRITE_AND_COUNT(RC); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 462b753f98..6ccbec2fa9 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -189,6 +189,7 @@ struct log_STAT_s { #define LOG_RC_MSG 11 struct log_RC_s { float channel[8]; + uint8_t rssi; uint8_t channel_count; uint8_t signal_lost; }; @@ -532,7 +533,7 @@ static const struct log_format_s log_formats[] = { 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(RC, "ffffffffBBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,RSSI,Count,SignalLost"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), From 132a57c314c94a1cb10c229d1b880b62a4e6e005 Mon Sep 17 00:00:00 2001 From: Bartosz Wawrzacz Date: Thu, 12 Nov 2015 15:01:29 +0100 Subject: [PATCH 081/106] fix the scaling when using "RSSI from input channel" functionality --- src/drivers/px4io/px4io.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 719bacb41f..609320a214 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1780,8 +1780,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) /* get RSSI from input channel */ if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= input_rc_s::RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) { - int rssi = (input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) / - ((_rssi_pwm_max - _rssi_pwm_min) / 100); + int rssi = ((input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) * 100) / + (_rssi_pwm_max - _rssi_pwm_min); rssi = rssi > 100 ? 100 : rssi; rssi = rssi < 0 ? 0 : rssi; input_rc.rssi = rssi; From c655b51d363194bf8399908a218d91e3b623ef35 Mon Sep 17 00:00:00 2001 From: Bartosz Wawrzacz Date: Thu, 12 Nov 2015 16:21:11 +0100 Subject: [PATCH 082/106] fixed formatting in px4io driver --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 609320a214..2cabe98a23 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1781,7 +1781,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) /* get RSSI from input channel */ if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= input_rc_s::RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) { int rssi = ((input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) * 100) / - (_rssi_pwm_max - _rssi_pwm_min); + (_rssi_pwm_max - _rssi_pwm_min); rssi = rssi > 100 ? 100 : rssi; rssi = rssi < 0 ? 0 : rssi; input_rc.rssi = rssi; From fc3ea7143c831452adfcadc7f176f6352ad854b3 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Thu, 12 Nov 2015 14:20:22 -0500 Subject: [PATCH 083/106] Matrix lib update. --- src/lib/matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/matrix b/src/lib/matrix index 2c7a375e3d..7656385ea1 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 2c7a375e3d7ce143dd1991c9135a5a55952a8977 +Subproject commit 7656385ea1d3f0a374a8146430bc63cf02e66d6b From 199f6478ee87cb4b7ed1631793a1b7c29e8dc07b Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Thu, 12 Nov 2015 15:30:03 +0100 Subject: [PATCH 084/106] enable online model lookup in gazebo --- 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 b4617e40cf..8767f9f841 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -56,8 +56,8 @@ then export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:$curr_dir/Tools/sitl_gazebo/Build # Set the model path so Gazebo finds the airframes export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$curr_dir/Tools/sitl_gazebo/models - # Disable online model lookup since this is quite experimental and unstable - export GAZEBO_MODEL_DATABASE_URI="" + # The next line would disable online model lookup, can be commented in, in case of unstable behaviour. + # export GAZEBO_MODEL_DATABASE_URI="" export SITL_GAZEBO_PATH=$curr_dir/Tools/sitl_gazebo mkdir -p Tools/sitl_gazebo/Build cd Tools/sitl_gazebo/Build From 48c467f6027702c2c61f5d8a21c43201a4158ee7 Mon Sep 17 00:00:00 2001 From: Karl Schwabe Date: Sun, 8 Nov 2015 14:39:19 +0100 Subject: [PATCH 085/106] Remove CONFIG_ARCH_BOARD #ifdef dependencies from mtd Since mtd already checks whether CONFIG_MTD_RAMTRON is set in defconfig use this define to check whether to call ramtron_attach() or at24xxx_attach() in the main function, instead of checking CONFIG_ARCH_BOARD_PX4FMU-V1. Also instead of using #ifdef CONFIG_ARCH_BOARD_AEROCORE to determine whether to start mtd on spi 2 or 4, rather use a new board define PX4_SPI_BUS_RAMTRON in the board_config.h file. --- src/drivers/boards/aerocore/board_config.h | 3 +++ src/drivers/boards/px4fmu-v2/board_config.h | 1 + src/systemcmds/mtd/mtd.c | 15 ++++++--------- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h index 0b52ca48ff..155173db5f 100644 --- a/src/drivers/boards/aerocore/board_config.h +++ b/src/drivers/boards/aerocore/board_config.h @@ -82,6 +82,9 @@ __BEGIN_DECLS #define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) #define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) +/* SPI4--Ramtron */ +#define PX4_SPI_BUS_RAMTRON 4 + /* Nominal chip selects for devices on SPI bus #3 */ #define PX4_SPIDEV_ACCEL_MAG 0 #define PX4_SPIDEV_GYRO 1 diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 0f1f2fd4a2..ea60df9100 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -113,6 +113,7 @@ __BEGIN_DECLS #define GPIO_SPI_CS_EXT3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) #define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPI_BUS_RAMTRON 2 #define PX4_SPI_BUS_EXT 4 /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index 0f54c9c4c3..b38504c623 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -176,12 +176,9 @@ struct mtd_dev_s *mtd_partition(FAR struct mtd_dev_s *mtd, static void ramtron_attach(void) { - /* find the right spi */ -#ifdef CONFIG_ARCH_BOARD_AEROCORE - struct spi_dev_s *spi = up_spiinitialize(4); -#else - struct spi_dev_s *spi = up_spiinitialize(2); -#endif + /* initialize the right spi */ + struct spi_dev_s *spi = up_spiinitialize(PX4_SPI_BUS_RAMTRON); + /* this resets the spi bus, set correct bus speed again */ SPI_SETFREQUENCY(spi, 10 * 1000 * 1000); SPI_SETBITS(spi, 8); @@ -269,10 +266,10 @@ mtd_start(char *partition_names[], unsigned n_partitions) } if (!attached) { -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - at24xxx_attach(); -#else +#ifdef CONFIG_MTD_RAMTRON ramtron_attach(); +#else + at24xxx_attach(); #endif } From 3aa3bf8f5f8296a5ed7f620d53d70b810db53a1b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 10:33:20 +0100 Subject: [PATCH 086/106] Commander: Do not report every new data link --- src/modules/commander/commander.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5919e78f5c..817d44ab74 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2189,8 +2189,7 @@ int commander_thread_main(int argc, char *argv[]) if (telemetry_last_dl_loss[i] > 0) { mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i); } else if (telemetry_last_dl_loss[i] == 0){ - /* report a new link */ - mavlink_and_console_log_info(mavlink_fd, "new data link connected : #%i", i); + /* do not report a new data link in order to not spam the user */ status.data_link_found_new = true; status_changed = true; } From 58c04d2d319ffa00f2a99fca396bfe7affdcf2f0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 11:18:30 +0100 Subject: [PATCH 087/106] sdlog2: Fix string overflow in file name --- src/modules/sdlog2/sdlog2.c | 34 ++++++++++++++++++---------------- 1 file changed, 18 insertions(+), 16 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index af77d955e9..6b98e64835 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -156,7 +156,9 @@ struct logbuffer_s lb; static pthread_mutex_t logbuffer_mutex; static pthread_cond_t logbuffer_cond; -static char log_dir[32]; +#define LOG_BASE_PATH_LEN 64 + +static char log_dir[LOG_BASE_PATH_LEN]; /* statistics counters */ static uint64_t start_time = 0; @@ -444,15 +446,15 @@ int create_log_dir() } /* print logging path, important to find log file later */ - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir); + mavlink_and_console_log_info(mavlink_fd, "[log] log dir: %s", log_dir); return 0; } int open_log_file() { /* string to hold the path to the log */ - char log_file_name[32] = ""; - char log_file_path[64] = ""; + char log_file_name[64] = ""; + char log_file_path[sizeof(log_file_name) + LOG_BASE_PATH_LEN] = ""; struct tm tt; bool time_ok = get_log_time_utc_tt(&tt, false); @@ -480,7 +482,7 @@ int open_log_file() if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); + mavlink_and_console_log_critical(mavlink_fd, "[log] ERR: max files %d", MAX_NO_LOGFILE); return -1; } } @@ -492,10 +494,10 @@ int open_log_file() #endif if (fd < 0) { - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name); + mavlink_and_console_log_critical(mavlink_fd, "[log] failed: %s", log_file_name); } else { - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] starting: %s", log_file_name); + mavlink_and_console_log_info(mavlink_fd, "[log] start: %s", log_file_name); } return fd; @@ -504,8 +506,8 @@ int open_log_file() int open_perf_file(const char* str) { /* string to hold the path to the log */ - char log_file_name[32] = ""; - char log_file_path[64] = ""; + char log_file_name[64] = ""; + char log_file_path[sizeof(log_file_name) + LOG_BASE_PATH_LEN] = ""; struct tm tt; bool time_ok = get_log_time_utc_tt(&tt, false); @@ -532,7 +534,7 @@ int open_perf_file(const char* str) if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); + mavlink_and_console_log_critical(mavlink_fd, "[log] ERR: max files %d", MAX_NO_LOGFILE); return -1; } } @@ -544,7 +546,7 @@ int open_perf_file(const char* str) #endif if (fd < 0) { - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name); + mavlink_and_console_log_critical(mavlink_fd, "[log] failed: %s", log_file_name); } @@ -671,7 +673,7 @@ void sdlog2_start_log() /* create log dir if needed */ if (create_log_dir() != 0) { - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); + mavlink_and_console_log_critical(mavlink_fd, "[log] error creating log dir"); return; } @@ -761,7 +763,7 @@ void sdlog2_stop_log() /* free log writer performance counter */ perf_free(perf_write); - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] logging stopped"); + mavlink_and_console_log_info(mavlink_fd, "[log] logging stopped"); sdlog2_status(); } @@ -1912,7 +1914,7 @@ void sdlog2_status() float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); - mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); + mavlink_log_info(mavlink_fd, "[log] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); } } @@ -1937,7 +1939,7 @@ int check_free_space() /* use a threshold of 50 MiB */ if (statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(50 * 1024 * 1024 / statfs_buf.f_bsize)) { mavlink_and_console_log_critical(mavlink_fd, - "[sdlog2] no space on MicroSD: %u MiB", + "[log] no space on MicroSD: %u MiB", (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); /* we do not need a flag to remember that we sent this warning because we will exit anyway */ return PX4_ERROR; @@ -1945,7 +1947,7 @@ int check_free_space() /* use a threshold of 100 MiB to send a warning */ } else if (!space_warning_sent && statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(100 * 1024 * 1024 / statfs_buf.f_bsize)) { mavlink_and_console_log_critical(mavlink_fd, - "[sdlog2] space on MicroSD low: %u MiB", + "[log] space on MicroSD low: %u MiB", (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); /* we don't want to flood the user with warnings */ space_warning_sent = true; From 9b758e7285448e4eb6f5afc84a30388481da73d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 12:07:19 +0100 Subject: [PATCH 088/106] Set minimum takeoff alt to 2.5 meters --- src/modules/navigator/mission_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 7c2942f53e..1ef0c57e3e 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -52,7 +52,7 @@ * @unit meters * @group Mission */ -PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); +PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f); /** * Enable persistent onboard mission storage From ff88fc00c06d22c94616841cfb75138bf7559d31 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:02:56 +0100 Subject: [PATCH 089/106] Commander: Preflight check reporting cleanup, add USB breaker --- src/modules/commander/PreflightCheck.cpp | 54 ++++++++++++++----- src/modules/commander/commander.cpp | 13 +++-- .../commander/state_machine_helper.cpp | 18 +++---- 3 files changed, 59 insertions(+), 26 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index bfeabb9e7e..3423ad8340 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -206,7 +206,9 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, goto out; } } else { - if(report_fail) mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ"); + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ"); + } /* this is frickin' fatal */ success = false; goto out; @@ -229,8 +231,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev if (fd < 0) { if (!optional) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance); + } } return false; @@ -239,8 +243,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id); if (ret) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance); + } success = false; goto out; } @@ -248,8 +254,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev ret = px4_ioctl(fd, GYROIOCSELFTEST, 0); if (ret != OK) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance); + } success = false; goto out; } @@ -269,8 +277,10 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev if (fd < 0) { if (!optional) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance); + } } return false; @@ -304,13 +314,17 @@ static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail) if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); + } success = false; goto out; } if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); + } // XXX do not make this fatal yet } @@ -341,8 +355,10 @@ static bool gnssCheck(int mavlink_fd, bool report_fail) } //Report failure to detect module - if(!success) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); + if (!success) { + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); + } } px4_close(gpsSub); @@ -376,7 +392,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check if the primary device is present */ if (!prime_found && prime_id != 0) { - if(reportFailures) mavlink_log_critical(mavlink_fd, "Warning: Primary compass not found"); + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary compass not found"); + } failed = true; } } @@ -403,7 +421,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check if the primary device is present */ if (!prime_found && prime_id != 0) { - if(reportFailures) mavlink_log_critical(mavlink_fd, "Warning: Primary accelerometer not found"); + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary accelerometer not found"); + } failed = true; } } @@ -430,7 +450,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check if the primary device is present */ if (!prime_found && prime_id != 0) { - if(reportFailures) mavlink_log_critical(mavlink_fd, "Warning: Primary gyro not found"); + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary gyro not found"); + } failed = true; } } @@ -458,7 +480,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro // TODO there is no logic in place to calibrate the primary baro yet // // check if the primary device is present if (!prime_found && prime_id != 0) { - if(reportFailures) mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational"); + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "warning: primary barometer not operational"); + } failed = true; } } @@ -473,7 +497,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* ---- RC CALIBRATION ---- */ if (checkRC) { if (rc_calibration_check(mavlink_fd, reportFailures) != OK) { - if(reportFailures) mavlink_log_critical(mavlink_fd, "RC calibration check failed"); + if (reportFailures) { + mavlink_log_critical(mavlink_fd, "RC calibration check failed"); + } failed = true; } } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 817d44ab74..7a65e69e30 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -390,15 +390,20 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "arm")) { int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); - arm_disarm(true, mavlink_fd_local, "command line"); - warnx("note: not updating home position on commandline arming!"); + if (TRANSITION_CHANGED == arm_disarm(true, mavlink_fd_local, "command line")) { + warnx("note: not updating home position on commandline arming!"); + } else { + warnx("arming failed"); + } px4_close(mavlink_fd_local); return 0; } if (!strcmp(argv[1], "disarm")) { int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); - arm_disarm(false, mavlink_fd_local, "command line"); + if (TRANSITION_DENIED == arm_disarm(false, mavlink_fd_local, "command line")) { + warnx("rejected disarm"); + } px4_close(mavlink_fd_local); return 0; } @@ -2513,6 +2518,8 @@ get_circuit_breaker_params() { status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status.cb_usb = + circuit_breaker_enabled("CBRK_USB_CHK", CBRK_USB_CHK_KEY); status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); status.circuit_breaker_engaged_enginefailure_check = diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index bdf88920fa..981644c342 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -177,7 +177,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // Fail transition if we need safety switch press } else if (safety->safety_switch_available && !safety->safety_off) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!"); + mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!"); feedback_provided = true; valid_transition = false; } @@ -202,10 +202,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s feedback_provided = true; valid_transition = false; } else if (status->avionics_power_rail_voltage < 4.9f) { - mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); + mavlink_and_console_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; } else if (status->avionics_power_rail_voltage > 5.4f) { - mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); + mavlink_and_console_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; } } @@ -392,7 +392,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta switch (new_state) { case vehicle_status_s::HIL_STATE_OFF: /* we're in HIL and unexpected things can happen if we disable HIL now */ - mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)"); + mavlink_and_console_log_critical(mavlink_fd, "Not switching off HIL (safety)"); ret = TRANSITION_DENIED; break; @@ -465,7 +465,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta closedir(d); ret = TRANSITION_CHANGED; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state"); } else { /* failed opening dir */ @@ -502,11 +502,11 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta } ret = TRANSITION_CHANGED; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state"); #endif } else { - mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed"); + mavlink_and_console_log_critical(mavlink_fd, "Not switching to HIL when armed"); ret = TRANSITION_DENIED; } break; @@ -759,9 +759,9 @@ int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool preflight_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, reportFailures); - if (status->usb_connected && prearm) { + if (!status->cb_usb && status->usb_connected && prearm) { preflight_ok = false; - if(reportFailures) mavlink_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); + if(reportFailures) mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); } return !preflight_ok; From 0fdc0e28c77f92fba0bfa2541dc6ec0a1a85b498 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:03:14 +0100 Subject: [PATCH 090/106] Messages: Add USB breaker and control state horizontal acceleration --- msg/control_state.msg | 1 + msg/vehicle_status.msg | 1 + 2 files changed, 2 insertions(+) diff --git a/msg/control_state.msg b/msg/control_state.msg index d976fbae8d..4f130353d3 100644 --- a/msg/control_state.msg +++ b/msg/control_state.msg @@ -16,3 +16,4 @@ 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) +float32 horz_acc_mag # Magnitude of the horizontal acceleration diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 6d01b3b58e..c91672c0be 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -182,3 +182,4 @@ bool circuit_breaker_engaged_power_check bool circuit_breaker_engaged_airspd_check bool circuit_breaker_engaged_enginefailure_check bool circuit_breaker_engaged_gpsfailure_check +bool cb_usb From 835ab4f10ff4941e68ee4ded91bc7429d68a6d72 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:03:28 +0100 Subject: [PATCH 091/106] Systemlib: Add USB circuit breaker --- src/modules/systemlib/circuit_breaker.h | 1 + .../systemlib/circuit_breaker_params.c | 13 ++++++++++++ src/modules/systemlib/rc_check.c | 20 ++++++++++--------- 3 files changed, 25 insertions(+), 9 deletions(-) diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index c76e6c37f3..b4d9b5d1c6 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -56,6 +56,7 @@ #define CBRK_FLIGHTTERM_KEY 121212 #define CBRK_ENGINEFAIL_KEY 284953 #define CBRK_GPSFAIL_KEY 240024 +#define CBRK_USB_CHK_KEY 197848 #include diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c index 003b73bb62..a3eaebbb33 100644 --- a/src/modules/systemlib/circuit_breaker_params.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -147,3 +147,16 @@ PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); * @group Circuit Breaker */ PARAM_DEFINE_INT32(CBRK_BUZZER, 0); + +/** + * Circuit breaker for USB link check + * + * Setting this parameter to 197848 will disable the USB connected + * checks in the commander. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 197848 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_USB_CHK, 0); diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 0f5d96f2b8..3456fafc68 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -74,7 +74,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) param_t map_parm = param_find(rc_map_mandatory[j]); if (map_parm == PARAM_INVALID) { - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -87,7 +87,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) param_get(map_parm, &mapping); if (mapping > RC_INPUT_MAX_CHANNELS) { - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -95,7 +95,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) } if (mapping == 0) { - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -148,7 +148,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) if (param_min < RC_INPUT_LOWEST_MIN_US) { count++; - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -157,7 +157,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) if (param_max > RC_INPUT_HIGHEST_MAX_US) { count++; - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -166,7 +166,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) if (param_trim < param_min) { count++; - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -175,7 +175,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) if (param_trim > param_max) { count++; - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -183,7 +183,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) /* assert deadzone is sane */ if (param_dz > RC_INPUT_MAX_DEADZONE_US) { - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -200,9 +200,11 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) if (channels_failed) { sleep(2); - if (report_fail) mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", total_fail_count, (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : ""); + } usleep(100000); } From d3365787af494e41144c138b3238f9e73646b066 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:03:46 +0100 Subject: [PATCH 092/106] sdlog2: Be less verbose --- src/modules/sdlog2/sdlog2.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6b98e64835..f15ec8969a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -425,7 +425,6 @@ int create_log_dir() mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); if (mkdir_ret == 0) { - warnx("log dir created: %s", log_dir); break; } else if (errno != EEXIST) { From 578b2500696ae2dfd97113bd68f90bbe47246249 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:04:52 +0100 Subject: [PATCH 093/106] Add RC check output --- src/modules/systemlib/rc_check.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 3456fafc68..622a3a0603 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -202,8 +202,8 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", - total_fail_count, - (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : ""); + total_fail_count, + (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : ""); } usleep(100000); From 0f489a194fe066848fe92add6cdcdeeca143d0d3 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Thu, 12 Nov 2015 12:18:19 +0530 Subject: [PATCH 094/106] attitude_q : more verbose failsafe output --- .../ecl/validation/data_validator_group.cpp | 2 +- .../attitude_estimator_q_main.cpp | 23 +++++++++++++++---- 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp index ac4bcc8b9b..e82e476416 100644 --- a/src/lib/ecl/validation/data_validator_group.cpp +++ b/src/lib/ecl/validation/data_validator_group.cpp @@ -199,7 +199,7 @@ void DataValidatorGroup::print() { /* print the group's state */ - ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (# %u)", + ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (%u events)", _curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO", _toggle_count); 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 1531e32778..b62c3b6cc5 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -369,12 +369,25 @@ void AttitudeEstimatorQ::task_main() _data_good = true; - if (!_failsafe && (_voter_gyro.failover_count() > 0 || - _voter_accel.failover_count() > 0 || - _voter_mag.failover_count() > 0)) { + if (!_failsafe) { + if (_voter_gyro.failover_count() > 0) { + _failsafe = true; + mavlink_and_console_log_emergency(_mavlink_fd, "Gyro failure!"); + } - _failsafe = true; - mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); + if (_voter_accel.failover_count() > 0) { + _failsafe = true; + mavlink_and_console_log_emergency(_mavlink_fd, "Accel failure!"); + } + + if (_voter_mag.failover_count() > 0) { + _failsafe = true; + mavlink_and_console_log_emergency(_mavlink_fd, "Mag failure!"); + } + + if (_failsafe) { + mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); + } } if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold || From 2d776aca14fdd2bcb97b80cee4e22dae50f90d5e Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Thu, 12 Nov 2015 14:50:49 +0530 Subject: [PATCH 095/106] attitude_q : ignore blank fields --- .../attitude_estimator_q_main.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 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 b62c3b6cc5..bbdf3ccc85 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -347,11 +347,16 @@ void AttitudeEstimatorQ::task_main() _voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]); } - - _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], - sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); - _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], + /* ignore empty fields */ + if (sensors.accelerometer_timestamp[i] > 0) { + _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], + sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); + } + /* ignore empty fields */ + if (sensors.magnetometer_timestamp[i] > 0) { + _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]); + } } int best_gyro, best_accel, best_mag; From 0d7cd22ae7f67452ed1fb1f9ed5c89a64fc1ee37 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Sat, 14 Nov 2015 11:58:51 +0530 Subject: [PATCH 096/106] data validator : verbose error state reporting --- src/lib/ecl/validation/data_validator.cpp | 45 +++++++++-------- src/lib/ecl/validation/data_validator.h | 24 +++++++++- .../ecl/validation/data_validator_group.cpp | 48 +++++++++++++++++-- src/lib/ecl/validation/data_validator_group.h | 14 ++++++ 4 files changed, 108 insertions(+), 23 deletions(-) diff --git a/src/lib/ecl/validation/data_validator.cpp b/src/lib/ecl/validation/data_validator.cpp index 737d9ea1a3..299811ea43 100644 --- a/src/lib/ecl/validation/data_validator.cpp +++ b/src/lib/ecl/validation/data_validator.cpp @@ -43,6 +43,7 @@ #include DataValidator::DataValidator(DataValidator *prev_sibling) : + _error_mask(ERROR_FLAG_NO_ERROR), _time_last(0), _timeout_interval(20000), _event_count(0), @@ -111,39 +112,45 @@ DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, in float DataValidator::confidence(uint64_t timestamp) { + float ret = 1.0f; + /* check if we have any data */ if (_time_last == 0) { - return 0.0f; + _error_mask |= ERROR_FLAG_NO_DATA; + ret = 0.0f; } - - /* check error count limit */ - if (_error_count > NORETURN_ERRCOUNT) { - return 0.0f; + + /* timed out - that's it */ + if (timestamp - _time_last > _timeout_interval) { + _error_mask |= ERROR_FLAG_TIMEOUT; + ret = 0.0f; } /* we got the exact same sensor value N times in a row */ if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) { - return 0.0f; + _error_mask |= ERROR_FLAG_STALE_DATA; + ret = 0.0f; } - - /* timed out - that's it */ - if (timestamp - _time_last > _timeout_interval) { - return 0.0f; + + /* check error count limit */ + if (_error_count > NORETURN_ERRCOUNT) { + _error_mask |= ERROR_FLAG_HIGH_ERRCOUNT; + ret = 0.0f; } /* cap error density counter at window size */ if (_error_density > ERROR_DENSITY_WINDOW) { + _error_mask |= ERROR_FLAG_HIGH_ERRDENSITY; _error_density = ERROR_DENSITY_WINDOW; } - - /* return local error density for last N measurements */ - return 1.0f - (_error_density / ERROR_DENSITY_WINDOW); -} - -int -DataValidator::priority() -{ - return _priority; + + /* no critical errors */ + if(ret > 0.0f) { + /* return local error density for last N measurements */ + ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW); + } + + return ret; } void diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h index dde9cb51aa..31f6d9d989 100644 --- a/src/lib/ecl/validation/data_validator.h +++ b/src/lib/ecl/validation/data_validator.h @@ -91,7 +91,18 @@ public: * Get the priority of this validator * @return the stored priority */ - int priority(); + int priority() { return (_priority); } + + /** + * Get the error state of this validator + * @return the bitmask with the error status + */ + uint32_t state() { return (_error_mask); } + + /** + * Reset the error state of this validator + */ + void reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; } /** * Get the RMS values of this validator @@ -111,9 +122,20 @@ public: * @param timeout_interval_us The timeout interval in microseconds */ void set_timeout(uint64_t timeout_interval_us) { _timeout_interval = timeout_interval_us; } + + /** + * Data validator error states + */ + static constexpr uint32_t ERROR_FLAG_NO_ERROR = (0x00000000U); + static constexpr uint32_t ERROR_FLAG_NO_DATA = (0x00000001U); + static constexpr uint32_t ERROR_FLAG_STALE_DATA = (0x00000001U << 1); + static constexpr uint32_t ERROR_FLAG_TIMEOUT = (0x00000001U << 2); + static constexpr uint32_t ERROR_FLAG_HIGH_ERRCOUNT = (0x00000001U << 3); + static constexpr uint32_t ERROR_FLAG_HIGH_ERRDENSITY = (0x00000001U << 4); private: static const unsigned _dimensions = 3; + uint32_t _error_mask; /**< sensor error state */ uint64_t _time_last; /**< last timestamp */ uint64_t _timeout_interval; /**< interval in which the datastream times out in us */ uint64_t _event_count; /**< total data counter */ diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp index e82e476416..1a22672181 100644 --- a/src/lib/ecl/validation/data_validator_group.cpp +++ b/src/lib/ecl/validation/data_validator_group.cpp @@ -138,11 +138,13 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index) bool true_failsafe = true; - /* check wether the switch was a failsafe or preferring a higher priority sensor */ + /* check whether the switch was a failsafe or preferring a higher priority sensor */ if (pre_check_prio != -1 && pre_check_prio < max_priority && fabsf(pre_check_confidence - max_confidence) < 0.1f) { /* this is not a failover */ true_failsafe = false; + /* reset error flags, this is likely a hotplug sensor coming online late */ + best->reset_state(); } /* if we're no initialized, initialize the bookkeeping but do not count a failsafe */ @@ -203,13 +205,21 @@ DataValidatorGroup::print() _curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO", _toggle_count); - DataValidator *next = _first; unsigned i = 0; while (next != nullptr) { if (next->used()) { - ECL_INFO("sensor #%u, prio: %d", i, next->priority()); + uint32_t flags = next->state(); + + ECL_INFO("sensor #%u, prio: %d, state:%s%s%s%s%s%s", i, next->priority(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " NO_DATA" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE_DATA" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " DATA_TIMEOUT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " HIGH_ERRCOUNT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " HIGH_ERRDENSITY" : ""), + ((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : "")); + next->print(); } next = next->sibling(); @@ -222,3 +232,35 @@ DataValidatorGroup::failover_count() { return _toggle_count; } + +int +DataValidatorGroup::failover_index() +{ + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == _prev_best)) { + return i; + } + next = next->sibling(); + i++; + } + return -1; +} + +uint32_t +DataValidatorGroup::failover_state() +{ + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == _prev_best)) { + return next->state(); + } + next = next->sibling(); + i++; + } + return DataValidator::ERROR_FLAG_NO_ERROR; +} diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h index 3756be2638..855c3ed4ae 100644 --- a/src/lib/ecl/validation/data_validator_group.h +++ b/src/lib/ecl/validation/data_validator_group.h @@ -80,6 +80,20 @@ public: * @return the number of failovers */ unsigned failover_count(); + + /** + * Get the index of the failed sensor in the group + * + * @return index of the failed sensor + */ + int failover_index(); + + /** + * Get the error state of the failed sensor in the group + * + * @return bitmask with erro states of the failed sensor + */ + uint32_t failover_state(); /** * Print the validator value From 5a1f7ca95a29f53eaad39f774b0f9c2b66784179 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Sat, 14 Nov 2015 11:58:34 +0530 Subject: [PATCH 097/106] attitude_q : verbose failure reporting --- .../ecl/validation/data_validator_group.cpp | 4 +-- .../attitude_estimator_q_main.cpp | 35 ++++++++++++++++--- 2 files changed, 32 insertions(+), 7 deletions(-) diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp index 1a22672181..8bf060cf65 100644 --- a/src/lib/ecl/validation/data_validator_group.cpp +++ b/src/lib/ecl/validation/data_validator_group.cpp @@ -240,7 +240,7 @@ DataValidatorGroup::failover_index() unsigned i = 0; while (next != nullptr) { - if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == _prev_best)) { + if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) { return i; } next = next->sibling(); @@ -256,7 +256,7 @@ DataValidatorGroup::failover_state() unsigned i = 0; while (next != nullptr) { - if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == _prev_best)) { + if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) { return next->state(); } next = next->sibling(); 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 bbdf3ccc85..4b4fba54c8 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -347,15 +347,17 @@ void AttitudeEstimatorQ::task_main() _voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]); } + /* ignore empty fields */ if (sensors.accelerometer_timestamp[i] > 0) { _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], - sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); + sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); } + /* ignore empty fields */ if (sensors.magnetometer_timestamp[i] > 0) { _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], - sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]); + sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]); } } @@ -375,19 +377,42 @@ void AttitudeEstimatorQ::task_main() _data_good = true; if (!_failsafe) { + uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR; + if (_voter_gyro.failover_count() > 0) { _failsafe = true; - mavlink_and_console_log_emergency(_mavlink_fd, "Gyro failure!"); + flags = _voter_gyro.failover_state(); + mavlink_and_console_log_emergency(_mavlink_fd, "Gyro #%i failure :%s%s%s%s%s!", + _voter_gyro.failover_index(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); } if (_voter_accel.failover_count() > 0) { _failsafe = true; - mavlink_and_console_log_emergency(_mavlink_fd, "Accel failure!"); + flags = _voter_accel.failover_state(); + mavlink_and_console_log_emergency(_mavlink_fd, "Accel #%i failure :%s%s%s%s%s!", + _voter_accel.failover_index(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); } if (_voter_mag.failover_count() > 0) { _failsafe = true; - mavlink_and_console_log_emergency(_mavlink_fd, "Mag failure!"); + flags = _voter_mag.failover_state(); + mavlink_and_console_log_emergency(_mavlink_fd, "Mag #%i failure :%s%s%s%s%s!", + _voter_mag.failover_index(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); } if (_failsafe) { From 2b50297c2e4b0dd93eb7a883704aa0e56a672db3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 17 Nov 2015 09:14:40 +0100 Subject: [PATCH 098/106] vtol: allow switch to fw mode without airspeed reading to facilitate bench testing --- src/modules/vtol_att_control/standard.cpp | 2 +- src/modules/vtol_att_control/tiltrotor.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index c87ca630ac..05c1a525b1 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -154,7 +154,7 @@ void Standard::update_vtol_state() } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode - if (_airspeed->true_airspeed_m_s >= _params_standard.airspeed_trans) { + if (_airspeed->true_airspeed_m_s >= _params_standard.airspeed_trans || !_armed->armed) { _vtol_schedule.flight_mode = FW_MODE; // we can turn off the multirotor motors now _flag_enable_mc_motors = false; diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 9e170104c4..270aa065a9 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -205,7 +205,8 @@ void Tiltrotor::update_vtol_state() 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) { + // also allow switch if we are not armed for the sake of bench testing + if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans || !_armed->armed) { _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; _vtol_schedule.transition_start = hrt_absolute_time(); } From b90e8a71f03764ec6dc9a7d930535d6a67015520 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 17 Nov 2015 09:28:18 +0100 Subject: [PATCH 099/106] FMU: Fix sensor reset command, by Dmitry Prokhorov --- src/drivers/px4fmu/fmu.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4d82ef9e08..64d9dced4a 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1469,6 +1469,10 @@ PX4FMU::sensor_reset(int ms) stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + stm32_configgpio(GPIO_SPI1_SCK); + stm32_configgpio(GPIO_SPI1_MISO); + stm32_configgpio(GPIO_SPI1_MOSI); + // // XXX bring up the EXTI pins again // stm32_configgpio(GPIO_GYRO_DRDY); // stm32_configgpio(GPIO_MAG_DRDY); From 907848452f412db829311fecaa4a635854868ddc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 17 Nov 2015 09:30:18 +0100 Subject: [PATCH 100/106] Commander: Be less verbose during normal flight ops --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7a65e69e30..8b86b9d7fc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1699,10 +1699,10 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; if (status.condition_landed) { - mavlink_log_critical(mavlink_fd, "LANDING DETECTED"); + mavlink_and_console_log_info(mavlink_fd, "LANDING DETECTED"); } else { - mavlink_log_critical(mavlink_fd, "TAKEOFF DETECTED"); + mavlink_and_console_log_info(mavlink_fd, "TAKEOFF DETECTED"); } } From 81ff8d8060e2584c3b61b952fb8bad3770d126d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 17 Nov 2015 09:31:41 +0100 Subject: [PATCH 101/106] Navigator: Initialize home position at boot time --- src/modules/navigator/navigator.h | 6 ++---- src/modules/navigator/navigator_main.cpp | 13 ++++--------- 2 files changed, 6 insertions(+), 13 deletions(-) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 1ddedddff4..6ecf73a606 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -135,7 +135,7 @@ public: struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; } struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; } struct home_position_s* get_home_position() { return &_home_pos; } - bool home_position_valid() { return _home_position_set; } + bool home_position_valid() { return (_home_pos.timestamp > 0); } struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } struct mission_result_s* get_mission_result() { return &_mission_result; } struct geofence_result_s* get_geofence_result() { return &_geofence_result; } @@ -205,8 +205,6 @@ private: geofence_result_s _geofence_result; vehicle_attitude_setpoint_s _att_sp; - bool _home_position_set; - bool _mission_item_valid; /**< flags if the current mission item is valid */ int _mission_instance_count; /**< instance count for the current mission */ @@ -257,7 +255,7 @@ private: /** * Retrieve home position */ - void home_position_update(); + void home_position_update(bool force=false); /** * Retreive navigation capabilities diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 92f3e2fecb..9131fb16f0 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -126,7 +126,6 @@ Navigator::Navigator() : _pos_sp_triplet{}, _mission_result{}, _att_sp{}, - _home_position_set(false), _mission_item_valid(false), _mission_instance_count(0), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), @@ -206,17 +205,13 @@ Navigator::sensor_combined_update() } void -Navigator::home_position_update() +Navigator::home_position_update(bool force) { bool updated = false; orb_check(_home_pos_sub, &updated); - if (updated) { + if (updated || force) { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); - - if (_home_pos.timestamp > 0) { - _home_position_set = true; - } } } @@ -298,7 +293,7 @@ Navigator::task_main() global_position_update(); gps_position_update(); sensor_combined_update(); - home_position_update(); + home_position_update(true); navigation_capabilities_update(); params_update(); @@ -408,7 +403,7 @@ Navigator::task_main() 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); + bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, home_position_valid()); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; From b6a959ad3e5b2cab864d6a19177916c5c03dcc1f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 17 Nov 2015 12:23:41 +0100 Subject: [PATCH 102/106] Navigator: Code style --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9131fb16f0..e44b8d0be5 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -484,7 +484,7 @@ Navigator::task_main() } /* iterate through navigation modes and set active/inactive for each */ - for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { + for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } From e070f671ae2b61816934628a114e2e63486c6e4f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 17 Nov 2015 12:24:51 +0100 Subject: [PATCH 103/106] Preflight: better reporting --- src/modules/commander/PreflightCheck.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 3423ad8340..8093ef56ba 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -498,7 +498,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro if (checkRC) { if (rc_calibration_check(mavlink_fd, reportFailures) != OK) { if (reportFailures) { - mavlink_log_critical(mavlink_fd, "RC calibration check failed"); + mavlink_and_console_log_critical(mavlink_fd, "RC calibration check failed"); } failed = true; } From 4947e89f52c90929cdf15143b7b5fed53b154582 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 17 Nov 2015 12:25:05 +0100 Subject: [PATCH 104/106] VTOL attitude control: Fix return value on start --- src/modules/vtol_att_control/vtol_att_control_main.cpp | 1 + 1 file changed, 1 insertion(+) 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 e15e950fd1..0248a20727 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -690,6 +690,7 @@ int vtol_att_control_main(int argc, char *argv[]) return 1; } + return 0; } if (!strcmp(argv[1], "stop")) { From 0509a5a9ea64568faee95bd3763d990a12906611 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 17 Nov 2015 12:35:24 +0100 Subject: [PATCH 105/106] Enforce airspeed check for VTOLs --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 981644c342..1956a805c3 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -753,7 +753,7 @@ int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ - if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) { + if (!status->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol) { checkAirspeed = true; } From 092a51426f3cde2e5a04e204a3f3adef82cefeeb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 17 Nov 2015 12:59:45 +0100 Subject: [PATCH 106/106] Build fix and airspeed console cal --- .../commander/airspeed_calibration.cpp | 40 +++++++++---------- src/modules/commander/commander.cpp | 2 + .../commander/state_machine_helper.cpp | 2 +- 3 files changed, 23 insertions(+), 21 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index eb2bcd650e..4139a949e0 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -68,7 +68,7 @@ static const char *sensor_name = "dpress"; static void feedback_calibration_failed(int mavlink_fd) { sleep(5); - mavlink_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); } int do_airspeed_calibration(int mavlink_fd) @@ -78,7 +78,7 @@ int do_airspeed_calibration(int mavlink_fd) const unsigned maxcount = 2400; /* give directions */ - mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); const unsigned calibration_count = (maxcount * 2) / 3; @@ -101,7 +101,7 @@ int do_airspeed_calibration(int mavlink_fd) paramreset_successful = true; } else { - mavlink_log_critical(mavlink_fd, "[cal] airspeed offset zero failed"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset zero failed"); } px4_close(fd); @@ -115,18 +115,18 @@ int do_airspeed_calibration(int mavlink_fd) float analog_scaling = 0.0f; param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); if (fabsf(analog_scaling) < 0.1f) { - mavlink_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd"); goto error_return; } /* set scaling offset parameter */ if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1); goto error_return; } } - mavlink_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind"); usleep(500 * 1000); while (calibration_counter < calibration_count) { @@ -149,7 +149,7 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count); } } else if (poll_ret == 0) { @@ -167,14 +167,14 @@ int do_airspeed_calibration(int mavlink_fd) airscale.offset_pa = diff_pres_offset; if (fd_scale > 0) { if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { - mavlink_log_critical(mavlink_fd, "[cal] airspeed offset update failed"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset update failed"); } px4_close(fd_scale); } if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1); goto error_return; } @@ -183,7 +183,7 @@ int do_airspeed_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); - mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); goto error_return; } @@ -192,12 +192,12 @@ int do_airspeed_calibration(int mavlink_fd) goto error_return; } - mavlink_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset); + mavlink_and_console_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); - mavlink_log_critical(mavlink_fd, "[cal] Create airflow now"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] Create airflow now"); calibration_counter = 0; @@ -222,7 +222,7 @@ int do_airspeed_calibration(int mavlink_fd) if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (calibration_counter % 500 == 0) { - mavlink_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)", + mavlink_and_console_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)", (int)diff_pres.differential_pressure_raw_pa); } continue; @@ -230,26 +230,26 @@ int do_airspeed_calibration(int mavlink_fd) /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)", + mavlink_and_console_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); - mavlink_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!"); + mavlink_and_console_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!"); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ diff_pres_offset = 0.0f; if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1); goto error_return; } /* save */ - mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0); (void)param_save_default(); feedback_calibration_failed(mavlink_fd); goto error_return; } else { - mavlink_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)", + mavlink_and_console_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); break; } @@ -266,9 +266,9 @@ int do_airspeed_calibration(int mavlink_fd) goto error_return; } - mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100); - mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); tune_neutral(true); normal_return: diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8b86b9d7fc..83e79b77fc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -362,6 +362,8 @@ int commander_main(int argc, char *argv[]) calib_ret = do_level_calibration(mavlink_fd); } else if (!strcmp(argv[2], "esc")) { calib_ret = do_esc_calibration(mavlink_fd, &armed); + } else if (!strcmp(argv[2], "airspeed")) { + calib_ret = do_airspeed_calibration(mavlink_fd); } else { warnx("argument %s unsupported.", argv[2]); } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1956a805c3..bcdb5d3f9d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -753,7 +753,7 @@ int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ - if (!status->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol) { + if (!status->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) { checkAirspeed = true; }