From ec2197fd1b228d8eb4855c49d5f9b1365685d01c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 4 Apr 2014 13:02:44 +1100 Subject: [PATCH 1/4] uploader: ignore bad character encodings for older bootloaders this prevents the uploader from throwing an exception with older bootloaders --- Tools/px_uploader.py | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index e4a8b3c054..985e6ffd9d 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -389,18 +389,22 @@ class uploader(object): self.otp_pid = self.otp[12:8:-1] self.otp_coa = self.otp[32:160] # show user: - print("type: " + self.otp_id.decode('Latin-1')) - print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('Latin-1')) - print("vid: " + binascii.hexlify(self.otp_vid).decode('Latin-1')) - print("pid: "+ binascii.hexlify(self.otp_pid).decode('Latin-1')) - print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('Latin-1')) - print("sn: ", end='') - for byte in range(0,12,4): - x = self.__getSN(byte) - x = x[::-1] # reverse the bytes - self.sn = self.sn + x - print(binascii.hexlify(x).decode('Latin-1'), end='') # show user - print('') + try: + print("type: " + self.otp_id.decode('Latin-1')) + print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('Latin-1')) + print("vid: " + binascii.hexlify(self.otp_vid).decode('Latin-1')) + print("pid: "+ binascii.hexlify(self.otp_pid).decode('Latin-1')) + print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('Latin-1')) + print("sn: ", end='') + for byte in range(0,12,4): + x = self.__getSN(byte) + x = x[::-1] # reverse the bytes + self.sn = self.sn + x + print(binascii.hexlify(x).decode('Latin-1'), end='') # show user + print('') + except Exception: + # ignore bad character encodings + pass print("erase...") self.__erase() From 30d1ce3a51caafe959fc048e4d7ca6147bd2ccd6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 11 Mar 2014 12:42:46 +1100 Subject: [PATCH 2/4] hmc5883: properly reset mag to normal state on calibration fail and add current output in "hmc5883 info" --- src/drivers/hmc5883/hmc5883.cpp | 36 +++++++++++++++++---------------- 1 file changed, 19 insertions(+), 17 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 4c85c0cdaa..cb97354ecc 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -169,6 +169,8 @@ private: int _bus; /**< the bus the device is connected to */ + struct mag_report _last_report; /**< used for info() */ + /** * Test whether the device supported by the driver is present at a * specific address. @@ -870,6 +872,8 @@ HMC5883::collect() } } + _last_report = new_report; + /* post a report to the ring */ if (_reports->force(&new_report)) { perf_count(_buffer_overflows); @@ -1042,32 +1046,29 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); - /* set back to normal mode */ - /* Set to 1.1 Gauss */ - if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) { - warnx("failed to set 1.1 Ga range"); - goto out; - } - - if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) { - warnx("failed to disable sensor calibration mode"); - goto out; - } - /* set scaling in device */ mscale_previous.x_scale = scaling[0]; mscale_previous.y_scale = scaling[1]; mscale_previous.z_scale = scaling[2]; - if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { - warn("WARNING: failed to set new scale / offsets for mag"); - goto out; - } - ret = OK; out: + if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { + warn("WARNING: failed to set new scale / offsets for mag"); + } + + /* set back to normal mode */ + /* Set to 1.1 Gauss */ + if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) { + warnx("failed to set 1.1 Ga range"); + } + + if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) { + warnx("failed to disable sensor calibration mode"); + } + if (ret == OK) { if (!check_scale()) { warnx("mag scale calibration successfully finished."); @@ -1221,6 +1222,7 @@ HMC5883::print_info() perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); + printf("output (%.2f %.2f %.2f)\n", (double)_last_report.x, (double)_last_report.y, (double)_last_report.z); printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, From f1f9f452c0225b7ebf65007505e6d74e57216cc1 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Sat, 22 Mar 2014 16:57:32 -0700 Subject: [PATCH 3/4] tone_alarm: Added arming failure tone --- src/drivers/drv_tone_alarm.h | 1 + src/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index cb5fd53a5f..147d7123a4 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -147,6 +147,7 @@ enum { TONE_BATTERY_WARNING_SLOW_TUNE, TONE_BATTERY_WARNING_FAST_TUNE, TONE_GPS_WARNING_TUNE, + TONE_ARMING_FAILURE_TUNE, TONE_NUMBER_OF_TUNES }; diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index f36f2091e3..8ed0de58c6 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -334,6 +334,7 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8"; //battery warning slow _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast _default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow + _default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<< Date: Sat, 5 Apr 2014 12:56:15 +0200 Subject: [PATCH 4/4] Compile hotfix for master --- .../fw_att_pos_estimator/estimator.cpp | 17 ++++++++++- src/modules/fw_att_pos_estimator/estimator.h | 30 +++++++++---------- 2 files changed, 31 insertions(+), 16 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 7ab06e85d3..c312173938 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -106,7 +106,22 @@ void swap_var(float &d1, float &d2) d2 = tmp; } -AttPosEKF::AttPosEKF() +AttPosEKF::AttPosEKF() : + fusionModeGPS(0), + covSkipCount(0), + EAS2TAS(1.0f), + statesInitialised(false), + fuseVelData(false), + fusePosData(false), + fuseHgtData(false), + fuseMagData(false), + fuseVtasData(false), + onGround(true), + staticMode(true), + useAirspeed(true), + useCompass(true), + numericalProtection(true), + storeIndex(0) { } diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 7edb3c714f..e62f1a98a5 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -108,7 +108,7 @@ public: Vector3f dVelIMU; Vector3f dAngIMU; float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) - uint8_t fusionModeGPS = 0; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity + uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity float innovVelPos[6]; // innovation output float varInnovVelPos[6]; // innovation variance output @@ -127,8 +127,8 @@ public: float lonRef; // WGS-84 longitude of reference point (rad) float hgtRef; // WGS-84 height of reference point (m) Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes - uint8_t covSkipCount = 0; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction - float EAS2TAS = 1.0f; // ratio f true to equivalent airspeed + uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction + float EAS2TAS; // ratio f true to equivalent airspeed // GPS input data variables float gpsCourse; @@ -141,25 +141,25 @@ public: // Baro input float baroHgt; - bool statesInitialised = false; + bool statesInitialised; - bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused - bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused - bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused - bool fuseMagData = false; // boolean true when magnetometer data is to be fused - bool fuseVtasData = false; // boolean true when airspeed data is to be fused + bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused + bool fusePosData; // this boolean causes the posNE and velNED obs to be fused + bool fuseHgtData; // this boolean causes the hgtMea obs to be fused + bool fuseMagData; // boolean true when magnetometer data is to be fused + bool fuseVtasData; // boolean true when airspeed data is to be fused - bool onGround = true; ///< boolean true when the flight vehicle is on the ground (not flying) - bool staticMode = true; ///< boolean true if no position feedback is fused - bool useAirspeed = true; ///< boolean true if airspeed data is being used - bool useCompass = true; ///< boolean true if magnetometer data is being used + bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) + bool staticMode; ///< boolean true if no position feedback is fused + bool useAirspeed; ///< boolean true if airspeed data is being used + bool useCompass; ///< boolean true if magnetometer data is being used struct ekf_status_report current_ekf_state; struct ekf_status_report last_ekf_error; - bool numericalProtection = true; + bool numericalProtection; - unsigned storeIndex = 0; + unsigned storeIndex; void UpdateStrapdownEquationsNED();