From 90276fa3a7b9c0f409599f007bebc6612904b922 Mon Sep 17 00:00:00 2001 From: sander Date: Wed, 3 Feb 2016 22:12:39 +0100 Subject: [PATCH 1/9] VTOL: check feasibility as rotary wing --- src/modules/navigator/mission.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 929e08ae02..ef2c787400 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -257,7 +257,7 @@ Mission::update_offboard_mission() * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); - failed = !_missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing, + failed = !_missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol), dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt, _navigator->home_position_valid(), _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, @@ -1027,7 +1027,7 @@ Mission::check_mission_valid() dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); - _navigator->get_mission_result()->valid = _missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing, + _navigator->get_mission_result()->valid = _missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol), dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt, _navigator->home_position_valid(), _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, From 7fc0b0925c60979cc3d647c04ad9d9585a87a73b Mon Sep 17 00:00:00 2001 From: ksschwabe Date: Wed, 30 Dec 2015 15:18:57 +0100 Subject: [PATCH 2/9] batt_smbus: Adds ManufacturerAccess, write_reg, and write_block functions Also fixes the the PEC calculation for writing. See http://cache.freescale.com/files/32bit/doc/app_note/AN4471.pdf and http://www.ti.com/lit/an/sloa132/sloa132.pdf for more details for more details on SMBus reading and writing including with and without PECs. --- src/drivers/batt_smbus/batt_smbus.cpp | 138 ++++++++++++++++++++++---- 1 file changed, 119 insertions(+), 19 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 42e4b835fc..79861fdabb 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -77,25 +77,29 @@ #include #include -#define BATT_SMBUS_ADDR_MIN 0x08 ///< lowest possible address -#define BATT_SMBUS_ADDR_MAX 0x7F ///< highest possible address +#define BATT_SMBUS_ADDR_MIN 0x08 ///< lowest possible address +#define BATT_SMBUS_ADDR_MAX 0x7F ///< highest possible address -#define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION -#define BATT_SMBUS_ADDR 0x0B ///< I2C address -#define BATT_SMBUS_TEMP 0x08 ///< temperature register -#define BATT_SMBUS_VOLTAGE 0x09 ///< voltage register +#define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION +#define BATT_SMBUS_ADDR 0x0B ///< I2C address +#define BATT_SMBUS_TEMP 0x08 ///< temperature register +#define BATT_SMBUS_VOLTAGE 0x09 ///< voltage register #define BATT_SMBUS_REMAINING_CAPACITY 0x0f ///< predicted remaining battery capacity as a percentage #define BATT_SMBUS_FULL_CHARGE_CAPACITY 0x10 ///< capacity when fully charged -#define BATT_SMBUS_DESIGN_CAPACITY 0x18 ///< design capacity register -#define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register -#define BATT_SMBUS_SERIALNUM 0x1c ///< serial number register -#define BATT_SMBUS_MANUFACTURE_NAME 0x20 ///< manufacturer name -#define BATT_SMBUS_MANUFACTURE_INFO 0x25 ///< cell voltage register -#define BATT_SMBUS_CURRENT 0x2a ///< current register -#define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) ///< time in microseconds, measure at 10hz -#define BATT_SMBUS_TIMEOUT_MS 10000000 ///< timeout looking for battery 10seconds after startup +#define BATT_SMBUS_DESIGN_CAPACITY 0x18 ///< design capacity register +#define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register +#define BATT_SMBUS_SERIALNUM 0x1c ///< serial number register +#define BATT_SMBUS_MANUFACTURE_NAME 0x20 ///< manufacturer name +#define BATT_SMBUS_MANUFACTURE_INFO 0x25 ///< cell voltage register +#define BATT_SMBUS_CURRENT 0x2a ///< current register +#define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) ///< time in microseconds, measure at 10Hz +#define BATT_SMBUS_TIMEOUT_MS 10000000 ///< timeout looking for battery 10seconds after startup -#define BATT_SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC +#define BATT_SMBUS_MANUFACTURER_ACCESS 0x00 +#define BATT_SMBUS_MANUFACTURER_DATA 0x23 +#define BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS 0x44 + +#define BATT_SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC #ifndef CONFIG_SCHED_WORKQUEUE # error This requires CONFIG_SCHED_WORKQUEUE. @@ -166,18 +170,35 @@ private: */ int read_reg(uint8_t reg, uint16_t &val); + /** + * Write a word to specified register + */ + int write_reg(uint8_t reg, uint16_t val); + /** * Read block from bus * @return returns number of characters read if successful, zero if unsuccessful */ uint8_t read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_zero); + /** + * Write block to the bus + * @return the number of characters sent if successful, zero if unsuccessful + */ + uint8_t write_block(uint8_t reg, uint8_t *data, uint8_t len); + /** * Calculate PEC for a read or write from the battery * @param buff is the data that was read or will be written */ uint8_t get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const; + /** + * Write a word to Manufacturer Access register (0x00) + * @param cmd the word to be written to Manufacturer Access + */ + uint8_t ManufacturerAccess(uint16_t cmd); + // internal variables bool _enabled; ///< true if we have successfully connected to battery work_s _work; ///< work queue for scheduling reads @@ -480,6 +501,27 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) return ret; } +int +BATT_SMBUS::write_reg(uint8_t reg, uint16_t val) +{ + uint8_t buff[4]; // reg + 2 bytes of data + PEC + + buff[0] = reg; + buff[2] = uint8_t(val << 8) & 0xff; + buff[1] = (uint8_t)val; + buff[3] = get_PEC(reg, false, &buff[1], 2); // Append PEC + + // write bytes to register + int ret = transfer(buff, 3, nullptr, 0); + + if (ret != OK) { + debug("Register write error"); + } + + // return success or failure + return ret; +} + uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_zero) { @@ -520,6 +562,31 @@ BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_ return bufflen; } +uint8_t +BATT_SMBUS::write_block(uint8_t reg, uint8_t *data, uint8_t len) +{ + uint8_t buff[len + 3]; // buffer to hold results + + usleep(1); + + buff[0] = reg; + buff[1] = len; + memcpy(&buff[2], data, len); + buff[len + 2] = get_PEC(reg, false, &buff[1], len + 1); // Append PEC + + // send bytes + int ret = transfer(buff, len + 3, nullptr, 0); + + // return zero on failure + if (ret != OK) { + debug("Block write error\n"); + return 0; + } + + // return success + return len; +} + uint8_t BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const { @@ -528,12 +595,32 @@ BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len return 0; } - // prepare temp buffer for calcing crc - uint8_t tmp_buff[len + 3]; + /** + * Note: The PEC is calculated on all the message bytes. See http://cache.freescale.com/files/32bit/doc/app_note/AN4471.pdf + * and http://www.ti.com/lit/an/sloa132/sloa132.pdf for more details + */ + + // prepare temp buffer for calculating crc + uint8_t tmp_buff_len; + + if (reading) { + tmp_buff_len = len + 3; + + } else { + tmp_buff_len = len + 2; + } + + uint8_t tmp_buff[tmp_buff_len]; tmp_buff[0] = (uint8_t)get_address() << 1; tmp_buff[1] = cmd; - tmp_buff[2] = tmp_buff[0] | (uint8_t)reading; - memcpy(&tmp_buff[3], buff, len); + + if (reading) { + tmp_buff[2] = tmp_buff[0] | (uint8_t)reading; + memcpy(&tmp_buff[3], buff, len); + + } else { + memcpy(&tmp_buff[2], buff, len); + } // initialise crc to zero uint8_t crc = 0; @@ -561,6 +648,19 @@ BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len return crc; } +uint8_t +BATT_SMBUS::ManufacturerAccess(uint16_t cmd) +{ + // write bytes to Manufacturer Access + int ret = write_reg(BATT_SMBUS_MANUFACTURER_ACCESS, cmd); + + if (ret != OK) { + debug("Manufacturer Access error"); + } + + return ret; +} + ///////////////////////// shell functions /////////////////////// void From 6ac26c680d994d8947424ad6baf7cc1c0316bb8d Mon Sep 17 00:00:00 2001 From: Angus Peart Date: Thu, 27 Aug 2015 10:51:12 -0700 Subject: [PATCH 3/9] batt_smbus: read button status from the smart battery This is a cherry-pick of commit 8bd17a4b0cd9c1432cb57a9a80a215692f532370 in 3drobotics/PX4Firmware-solo Conflicts: src/drivers/batt_smbus/batt_smbus.cpp src/drivers/drv_irlock.h src/lib/ecl --- msg/battery_status.msg | 1 + src/drivers/batt_smbus/batt_smbus.cpp | 51 ++++++++++++++++++++------- 2 files changed, 39 insertions(+), 13 deletions(-) diff --git a/msg/battery_status.msg b/msg/battery_status.msg index 4ed56dd038..75b144c6e8 100644 --- a/msg/battery_status.msg +++ b/msg/battery_status.msg @@ -3,3 +3,4 @@ float32 voltage_v # Battery voltage in volts, 0 if unknown float32 voltage_filtered_v # Battery voltage in volts, filtered, 0 if unknown float32 current_a # Battery current in amperes, -1 if unknown float32 discharged_mah # Discharged amount in mAh, -1 if unknown +bool is_powering_off # Power off event imminent indication, false if unknown diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 79861fdabb..c6b5057062 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -90,13 +90,15 @@ #define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register #define BATT_SMBUS_SERIALNUM 0x1c ///< serial number register #define BATT_SMBUS_MANUFACTURE_NAME 0x20 ///< manufacturer name +#define BATT_SMBUS_MANUFACTURER_DATA 0x23 ///< manufacturer data #define BATT_SMBUS_MANUFACTURE_INFO 0x25 ///< cell voltage register #define BATT_SMBUS_CURRENT 0x2a ///< current register -#define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) ///< time in microseconds, measure at 10Hz -#define BATT_SMBUS_TIMEOUT_MS 10000000 ///< timeout looking for battery 10seconds after startup +#define BATT_SMBUS_MEASUREMENT_INTERVAL_US (1000000 / 10) ///< time in microseconds, measure at 10Hz +#define BATT_SMBUS_TIMEOUT_US 10000000 ///< timeout looking for battery 10seconds after startup + +#define BATT_SMBUS_BUTTON_DEBOUNCE_MS 300 ///< button holds longer than this time will cause a power off event #define BATT_SMBUS_MANUFACTURER_ACCESS 0x00 -#define BATT_SMBUS_MANUFACTURER_DATA 0x23 #define BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS 0x44 #define BATT_SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC @@ -208,6 +210,7 @@ private: orb_id_t _batt_orb_id; ///< uORB battery topic ID uint64_t _start_time; ///< system time we first attempt to communicate with battery uint16_t _batt_capacity; ///< battery's design capacity in mAh (0 means unknown) + uint8_t _button_press_counts; ///< count of button presses detected }; namespace @@ -220,14 +223,15 @@ void batt_smbus_usage(); extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : - I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 400000), + I2C("batt_smbus", BATT_SMBUS0_DEVICE_PATH, bus, batt_smbus_addr, 100000), _enabled(false), _work{}, _reports(nullptr), - _batt_topic(nullptr), + _batt_topic(-1), _batt_orb_id(nullptr), _start_time(0), - _batt_capacity(0) + _batt_capacity(0), + _button_press_counts(0) { // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -318,8 +322,8 @@ BATT_SMBUS::test() if (updated) { if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) { - warnx("V=%4.2f C=%4.2f DismAh=%4.2f Cap:%d", (float)status.voltage_v, (float)status.current_a, - (float)status.discharged_mah, (int)_batt_capacity); + warnx("V=%4.2f C=%4.2f DismAh=%4.2f Cap:%d Shutdown:%d", (double)status.voltage_v, (double)status.current_a, + (double)status.discharged_mah, (int)_batt_capacity, (int)status.is_powering_off); } } @@ -402,7 +406,7 @@ BATT_SMBUS::cycle() uint64_t now = hrt_absolute_time(); // exit without rescheduling if we have failed to find a battery after 10 seconds - if (!_enabled && (now - _start_time > BATT_SMBUS_TIMEOUT_MS)) { + if (!_enabled && (now - _start_time > BATT_SMBUS_TIMEOUT_US)) { warnx("did not find smart battery"); return; } @@ -424,7 +428,7 @@ BATT_SMBUS::cycle() new_report.voltage_v = ((float)tmp) / 1000.0f; // read current - uint8_t buff[4]; + uint8_t buff[6]; if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) { new_report.current_a = -(float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 | @@ -447,14 +451,35 @@ BATT_SMBUS::cycle() } } + // read the button press indicator + if (read_block(BATT_SMBUS_MANUFACTURER_DATA, buff, 6, false) == 6) { + bool pressed = (buff[1] >> 3) & 0x01; + + if(_button_press_counts >= ((BATT_SMBUS_BUTTON_DEBOUNCE_MS * 1000) / BATT_SMBUS_MEASUREMENT_INTERVAL_US)) { + // battery will power off + new_report.is_powering_off = true; + // warn only once + if(_button_press_counts++ == ((BATT_SMBUS_BUTTON_DEBOUNCE_MS * 1000) / BATT_SMBUS_MEASUREMENT_INTERVAL_US)) { + warnx("system is shutting down NOW..."); + } + } else if(pressed) { + // battery will power off if the button is held + _button_press_counts++; + } else { + // button released early, reset counters + _button_press_counts = 0; + new_report.is_powering_off = false; + } + } + // publish to orb - if (_batt_topic != nullptr) { + if (_batt_topic != -1) { orb_publish(_batt_orb_id, _batt_topic, &new_report); } else { _batt_topic = orb_advertise(_batt_orb_id, &new_report); - if (_batt_topic == nullptr) { + if (_batt_topic < 0) { errx(1, "ADVERT FAIL"); } } @@ -474,7 +499,7 @@ BATT_SMBUS::cycle() // schedule a fresh cycle call when the measurement is done work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, - USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_MS)); + USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_US)); } int From e3f2467b915eb9bf4c0388f261775fb3e51e7640 Mon Sep 17 00:00:00 2001 From: ksschwabe Date: Fri, 1 Jan 2016 17:40:52 +0100 Subject: [PATCH 4/9] batt_smbus: Commentted out button press detection The button press detection relies on reading the MANUFACTURER_DATA register. The implementation of what information is returned when reading this register differs from manufacturer to manufacturer. This has been commented out until an elegant solution can be found to detect the battery shutdown on different batteries manufactured by different manufacturers. --- src/drivers/batt_smbus/batt_smbus.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index c6b5057062..31b9c72913 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -451,6 +451,7 @@ BATT_SMBUS::cycle() } } + /* // read the button press indicator if (read_block(BATT_SMBUS_MANUFACTURER_DATA, buff, 6, false) == 6) { bool pressed = (buff[1] >> 3) & 0x01; @@ -471,6 +472,7 @@ BATT_SMBUS::cycle() new_report.is_powering_off = false; } } + */ // publish to orb if (_batt_topic != -1) { From 27a645353bf5be2ddb4b21a4f9121ab1681afb21 Mon Sep 17 00:00:00 2001 From: ksschwabe Date: Fri, 1 Jan 2016 18:17:00 +0100 Subject: [PATCH 5/9] batt_smbus: Adds functions for reading out the SBS Info Functions added for reading out the following info: - Manufacturer Name - Device Name - Serial Number - Device Chemistry --- src/drivers/batt_smbus/batt_smbus.cpp | 294 ++++++++++++++++++++++++-- 1 file changed, 272 insertions(+), 22 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 31b9c72913..f9214834c8 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -88,8 +88,11 @@ #define BATT_SMBUS_FULL_CHARGE_CAPACITY 0x10 ///< capacity when fully charged #define BATT_SMBUS_DESIGN_CAPACITY 0x18 ///< design capacity register #define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register -#define BATT_SMBUS_SERIALNUM 0x1c ///< serial number register -#define BATT_SMBUS_MANUFACTURE_NAME 0x20 ///< manufacturer name +#define BATT_SMBUS_MANUFACTURE_DATE 0x1B ///< manufacture date register +#define BATT_SMBUS_SERIAL_NUMBER 0x1C ///< serial number register +#define BATT_SMBUS_MANUFACTURER_NAME 0x20 ///< manufacturer name +#define BATT_SMBUS_DEVICE_NAME 0x21 ///< device name register +#define BATT_SMBUS_DEVICE_CHEMISTRY 0x22 ///< device chemistry register #define BATT_SMBUS_MANUFACTURER_DATA 0x23 ///< manufacturer data #define BATT_SMBUS_MANUFACTURE_INFO 0x25 ///< cell voltage register #define BATT_SMBUS_CURRENT 0x2a ///< current register @@ -139,6 +142,57 @@ public: */ int search(); + /** + * Get the SBS manufacturer name of the battery device + * + * @param manufacturer_name pointer a buffer into which the manufacturer name is to be written + * @param max_length the maximum number of bytes to attempt to read from the manufacturer name register, including the null character that is appended to the end + * + * @return the number of bytes read + */ + uint8_t manufacturer_name(uint8_t *man_name, uint8_t max_length); + + /** + * Return the SBS manufacture date of the battery device + * + * @return the date in the following format: + * see Smart Battery Data Specification, Revision 1.1 + * http://sbs-forum.org/specs/sbdat110.pdf for more details + * Date as uint16_t = (year-1980) * 512 + month * 32 + day + * | Field | Bits | Format | Allowable Values | + * | ----- | ---- | ------------------ | ------------------------------------------ | + * | Day 0-4 5-bit binary value 1-31 (corresponds to day) | + * | Month 5-8 4-bit binary value 1-12 (corresponds to month number) | + * | Year 9-15 7-bit binary value 0-127 (corresponds to year biased by 1980) | + * otherwise, return 0 on failure + */ + uint16_t manufacture_date(); + + /** + * Get the SBS device name of the battery device + * + * @param dev_name pointer a buffer into which the device name is to be written + * @param max_length the maximum number of bytes to attempt to read from the device name register, including the null character that is appended to the end + * + * @return the number of bytes read + */ + uint8_t device_name(uint8_t *dev_name, uint8_t max_length); + + /** + * Return the SBS serial number of the battery device + */ + uint16_t serial_number(); + + /** + * Get the SBS device chemistry of the battery device + * + * @param dev_chem pointer a buffer into which the device chemistry is to be written + * @param max_length the maximum number of bytes to attempt to read from the device chemistry register, including the null character that is appended to the end + * + * @return the number of bytes read + */ + uint8_t device_chemistry(uint8_t *dev_chem, uint8_t max_length); + protected: /** * Check if the device can be contacted @@ -222,6 +276,12 @@ void batt_smbus_usage(); extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); +int manufacturer_name(); +int manufacture_date(); +int device_name(); +int serial_number(); +int device_chemistry(); + BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : I2C("batt_smbus", BATT_SMBUS0_DEVICE_PATH, bus, batt_smbus_addr, 100000), _enabled(false), @@ -368,6 +428,82 @@ BATT_SMBUS::search() return OK; } +uint8_t +BATT_SMBUS::manufacturer_name(uint8_t *man_name, uint8_t max_length) +{ + uint8_t len = read_block(BATT_SMBUS_MANUFACTURER_NAME, man_name, max_length, false); + + if (len > 0) { + if (len >= max_length - 1) { + man_name[max_length - 1] = 0; + + } else { + man_name[len] = 0; + } + } + + return len; +} + +uint16_t +BATT_SMBUS::manufacture_date() +{ + uint16_t man_date; + + if (read_reg(BATT_SMBUS_MANUFACTURE_DATE, man_date) == OK) { + return man_date; + } + + // Return 0 if could not read the date correctly + return 0; +} + +uint8_t +BATT_SMBUS::device_name(uint8_t *dev_name, uint8_t max_length) +{ + uint8_t len = read_block(BATT_SMBUS_DEVICE_NAME, dev_name, max_length, false); + + if (len > 0) { + if (len >= max_length - 1) { + dev_name[max_length - 1] = 0; + + } else { + dev_name[len] = 0; + } + } + + return len; +} + +uint16_t +BATT_SMBUS::serial_number() +{ + uint16_t serial_num; + + if (read_reg(BATT_SMBUS_SERIAL_NUMBER, serial_num) == OK) { + return serial_num; + } + + return -1; +} + +uint8_t +BATT_SMBUS::device_chemistry(uint8_t *dev_chem, uint8_t max_length) +{ + uint8_t len = read_block(BATT_SMBUS_DEVICE_CHEMISTRY, dev_chem, max_length, false); + + if (len > 0) { + if (len >= max_length - 1) { + dev_chem[max_length - 1] = 0; + + } else { + dev_chem[len] = 0; + } + } + + return len; +} + int BATT_SMBUS::probe() { @@ -451,28 +587,28 @@ BATT_SMBUS::cycle() } } - /* - // read the button press indicator - if (read_block(BATT_SMBUS_MANUFACTURER_DATA, buff, 6, false) == 6) { - bool pressed = (buff[1] >> 3) & 0x01; + /* + // read the button press indicator + if (read_block(BATT_SMBUS_MANUFACTURER_DATA, buff, 6, false) == 6) { + bool pressed = (buff[1] >> 3) & 0x01; - if(_button_press_counts >= ((BATT_SMBUS_BUTTON_DEBOUNCE_MS * 1000) / BATT_SMBUS_MEASUREMENT_INTERVAL_US)) { - // battery will power off - new_report.is_powering_off = true; - // warn only once - if(_button_press_counts++ == ((BATT_SMBUS_BUTTON_DEBOUNCE_MS * 1000) / BATT_SMBUS_MEASUREMENT_INTERVAL_US)) { - warnx("system is shutting down NOW..."); + if(_button_press_counts >= ((BATT_SMBUS_BUTTON_DEBOUNCE_MS * 1000) / BATT_SMBUS_MEASUREMENT_INTERVAL_US)) { + // battery will power off + new_report.is_powering_off = true; + // warn only once + if(_button_press_counts++ == ((BATT_SMBUS_BUTTON_DEBOUNCE_MS * 1000) / BATT_SMBUS_MEASUREMENT_INTERVAL_US)) { + warnx("system is shutting down NOW..."); + } + } else if(pressed) { + // battery will power off if the button is held + _button_press_counts++; + } else { + // button released early, reset counters + _button_press_counts = 0; + new_report.is_powering_off = false; } - } else if(pressed) { - // battery will power off if the button is held - _button_press_counts++; - } else { - // button released early, reset counters - _button_press_counts = 0; - new_report.is_powering_off = false; } - } - */ + */ // publish to orb if (_batt_topic != -1) { @@ -693,12 +829,92 @@ BATT_SMBUS::ManufacturerAccess(uint16_t cmd) void batt_smbus_usage() { - warnx("missing command: try 'start', 'test', 'stop', 'search'"); + warnx("missing command: try 'start', 'test', 'stop', 'search', 'man_name', 'man_date', 'dev_name', 'serial_num', 'dev_chem', 'sbs_info'"); warnx("options:"); warnx(" -b i2cbus (%d)", BATT_SMBUS_I2C_BUS); warnx(" -a addr (0x%x)", BATT_SMBUS_ADDR); } +int +manufacturer_name() +{ + uint8_t man_name[21]; + uint8_t len = g_batt_smbus->manufacturer_name(man_name, sizeof(man_name)); + + if (len > 0) { + warnx("The manufacturer name: %s", man_name); + return OK; + + } else { + warnx("Unable to read manufacturer name."); + } + + return -1; +} + +int +manufacture_date() +{ + uint16_t man_date = g_batt_smbus->manufacture_date(); + + if (man_date > 0) { + // Convert the uint16_t into human-readable date format + uint16_t year = ((man_date >> 9) & 0xFF) + 1980; + uint8_t month = (man_date >> 5) & 0xF; + uint8_t day = man_date & 0x1F; + warnx("The manufacturer date is: %d which is %4d-%02d-%02d", man_date, year, month, day); + return OK; + + } else { + warnx("Unable to read the manufacturer date."); + } + + return -1; +} + +int +device_name() +{ + uint8_t device_name[21]; + uint8_t len = g_batt_smbus->device_name(device_name, sizeof(device_name)); + + if (len > 0) { + warnx("The device name: %s", device_name); + return OK; + + } else { + warnx("Unable to read device name."); + } + + return -1; +} + +int +serial_number() +{ + uint16_t serial_num = g_batt_smbus->serial_number(); + warnx("The serial number: 0x%04x (%d in decimal)", serial_num, serial_num); + + return OK; +} + +int +device_chemistry() +{ + uint8_t device_chemistry[5]; + uint8_t len = g_batt_smbus->device_chemistry(device_chemistry, sizeof(device_chemistry)); + + if (len > 0) { + warnx("The device chemistry: %s", device_chemistry); + return OK; + + } else { + warnx("Unable to read device chemistry."); + } + + return -1; +} + int batt_smbus_main(int argc, char *argv[]) { @@ -776,6 +992,40 @@ batt_smbus_main(int argc, char *argv[]) exit(0); } + if (!strcmp(verb, "man_name")) { + manufacturer_name(); + exit(0); + } + + if (!strcmp(verb, "man_date")) { + manufacture_date(); + exit(0); + } + + if (!strcmp(verb, "dev_name")) { + device_name(); + exit(0); + } + + if (!strcmp(verb, "serial_num")) { + serial_number(); + exit(0); + } + + if (!strcmp(verb, "dev_chem")) { + device_chemistry(); + exit(0); + } + + if (!strcmp(verb, "sbs_info")) { + manufacturer_name(); + manufacture_date(); + device_name(); + serial_number(); + device_chemistry(); + exit(0); + } + batt_smbus_usage(); exit(0); } From 4b6d0ef0a6e0dc0a37212df728a2b74ce0637e87 Mon Sep 17 00:00:00 2001 From: ksschwabe Date: Sun, 24 Jan 2016 18:33:40 +0100 Subject: [PATCH 6/9] batt_smbus: Adds check for if the battery detected is a 3DR solo battery Also, if the battery is a 3DR Solo battery, it checks for the button press on the battery which sets the is_powering_off flag. The check for whether the battery is a 3DR Solo battery is based on whether the Manufacturer Name, Device Name and Device Chemistry all match the that of the standard 3DR Solo battery. --- src/drivers/batt_smbus/batt_smbus.cpp | 133 ++++++++++++++++++++++++-- 1 file changed, 126 insertions(+), 7 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index f9214834c8..b2faa43e1d 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -110,6 +110,15 @@ # error This requires CONFIG_SCHED_WORKQUEUE. #endif +struct battery_type { + char *ManufacturerName; + char *DeviceName; + char *DeviceChemistry; +}; + +// Declaration of the solo battery data, as determined by reading out the data from multiple 3DR Solo batteries +const struct battery_type solo_battery = {(char *)"BMTPOW", (char *)"MA03", (char *)"LIon"}; + class BATT_SMBUS : public device::I2C { public: @@ -193,6 +202,11 @@ public: */ uint8_t device_chemistry(uint8_t *dev_chem, uint8_t max_length); + /** + * Checks whether the current SBS battery data corresponds to a 3DR Solo battery + */ + bool is_solo_battery(); + protected: /** * Check if the device can be contacted @@ -255,6 +269,12 @@ private: */ uint8_t ManufacturerAccess(uint16_t cmd); + /** + * Checks if the battery that has been detected is a 3DR Solo Battery. If it is, it sets + * the private variable _is_solo_battery to be true + */ + void check_if_solo_battery(); + // internal variables bool _enabled; ///< true if we have successfully connected to battery work_s _work; ///< work queue for scheduling reads @@ -264,7 +284,11 @@ private: orb_id_t _batt_orb_id; ///< uORB battery topic ID uint64_t _start_time; ///< system time we first attempt to communicate with battery uint16_t _batt_capacity; ///< battery's design capacity in mAh (0 means unknown) - uint8_t _button_press_counts; ///< count of button presses detected + char *_manufacturer_name; ///< The name of the battery manufacturer + char *_device_name; ///< The name of the battery device + char *_device_chemistry; ///< The battery chemistry + bool _is_solo_battery; ///< Boolean as to whether the battery detected is a 3DR Solo Battery or not + uint8_t _button_press_counts; ///< count of button presses detected on 3DR Solo Battery }; namespace @@ -281,6 +305,7 @@ int manufacture_date(); int device_name(); int serial_number(); int device_chemistry(); +int solo_battery_check(); BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : I2C("batt_smbus", BATT_SMBUS0_DEVICE_PATH, bus, batt_smbus_addr, 100000), @@ -291,6 +316,10 @@ BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : _batt_orb_id(nullptr), _start_time(0), _batt_capacity(0), + _manufacturer_name(nullptr), + _device_name(nullptr), + _device_chemistry(nullptr), + _is_solo_battery(false), _button_press_counts(0) { // work_cancel in the dtor will explode if we don't do this... @@ -308,6 +337,18 @@ BATT_SMBUS::~BATT_SMBUS() if (_reports != nullptr) { delete _reports; } + + if (_manufacturer_name != nullptr) { + delete _manufacturer_name; + } + + if (_device_name != nullptr) { + delete _device_name; + } + + if (_device_chemistry != nullptr) { + delete _device_chemistry; + } } int @@ -504,6 +545,13 @@ BATT_SMBUS::device_chemistry(uint8_t *dev_chem, uint8_t max_length) return len; } +bool +BATT_SMBUS::is_solo_battery() +{ + check_if_solo_battery(); + return _is_solo_battery; +} + int BATT_SMBUS::probe() { @@ -547,6 +595,48 @@ BATT_SMBUS::cycle() return; } + bool perform_solo_battry_check = false; // Only check if it is a solo battery if changes have been made to the SBS data + + // Try and get battery SBS info + if (_manufacturer_name == nullptr) { + char man_name[21]; + uint8_t len = manufacturer_name((uint8_t *)man_name, sizeof(man_name)); + + if (len > 0) { + _manufacturer_name = new char[len]; + strcpy(_manufacturer_name, man_name); + perform_solo_battry_check = true; + } + } + + if (_device_name == nullptr) { + char dev_name[21]; + uint8_t len = device_name((uint8_t *)dev_name, sizeof(dev_name)); + + if (len > 0) { + _device_name = new char[len]; + strcpy(_device_name, dev_name); + perform_solo_battry_check = true; + } + } + + if (_device_chemistry == nullptr) { + char dev_chem[21]; + uint8_t len = device_chemistry((uint8_t *)dev_chem, sizeof(dev_chem)); + + if (len > 0) { + _device_chemistry = new char[len]; + strcpy(_device_chemistry, dev_chem); + perform_solo_battry_check = true; + } + } + + // If necessary, check if the battery is a 3DR Solo Battery + if (perform_solo_battry_check) { + warnx("Checking solo battery"); + check_if_solo_battery(); + } + // read data from sensor struct battery_status_s new_report; @@ -587,28 +677,33 @@ BATT_SMBUS::cycle() } } - /* + // if it is a solo battery, check for shutdown on button press + if (_is_solo_battery) { // read the button press indicator - if (read_block(BATT_SMBUS_MANUFACTURER_DATA, buff, 6, false) == 6) { + if (read_block(BATT_SMBUS_MANUFACTURER_DATA, buff, 6, false) == 6) { bool pressed = (buff[1] >> 3) & 0x01; - if(_button_press_counts >= ((BATT_SMBUS_BUTTON_DEBOUNCE_MS * 1000) / BATT_SMBUS_MEASUREMENT_INTERVAL_US)) { + if (_button_press_counts >= ((BATT_SMBUS_BUTTON_DEBOUNCE_MS * 1000) / BATT_SMBUS_MEASUREMENT_INTERVAL_US)) { // battery will power off new_report.is_powering_off = true; + // warn only once - if(_button_press_counts++ == ((BATT_SMBUS_BUTTON_DEBOUNCE_MS * 1000) / BATT_SMBUS_MEASUREMENT_INTERVAL_US)) { + if (_button_press_counts++ == ((BATT_SMBUS_BUTTON_DEBOUNCE_MS * 1000) / BATT_SMBUS_MEASUREMENT_INTERVAL_US)) { warnx("system is shutting down NOW..."); } - } else if(pressed) { + + } else if (pressed) { // battery will power off if the button is held _button_press_counts++; + } else { // button released early, reset counters _button_press_counts = 0; new_report.is_powering_off = false; } } - */ + } + // publish to orb if (_batt_topic != -1) { @@ -824,6 +919,16 @@ BATT_SMBUS::ManufacturerAccess(uint16_t cmd) return ret; } +void +BATT_SMBUS::check_if_solo_battery() +{ + // Check if the SBS information corresponds to that of a 3DR Solo Battery. If, yes, set the solo_battery flag to true; + if (!strcmp(_manufacturer_name, solo_battery.ManufacturerName) && !strcmp(_device_name, solo_battery.DeviceName) + && !strcmp(_device_chemistry, solo_battery.DeviceChemistry)) { + _is_solo_battery = true; + } +} + ///////////////////////// shell functions /////////////////////// void @@ -915,6 +1020,19 @@ device_chemistry() return -1; } +int +solo_battery_check() +{ + if (g_batt_smbus->is_solo_battery()) { + warnx("The battery corresponds to a 3DR Solo Battery"); + + } else { + warnx("The battery does not correspond to a 3DR Solo Battery"); + } + + return OK; +} + int batt_smbus_main(int argc, char *argv[]) { @@ -1023,6 +1141,7 @@ batt_smbus_main(int argc, char *argv[]) device_name(); serial_number(); device_chemistry(); + solo_battery_check(); exit(0); } From eb11c9173326961bfec1095db802f151d2c035de Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Feb 2016 22:53:43 +0100 Subject: [PATCH 7/9] Fix airspeed check in simulation --- src/modules/sensors/sensors.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d442ce65ac..24ae7c7705 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1228,7 +1228,12 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) /* push data into validator */ _airspeed_validator.put(_airspeed.timestamp, _diff_pres.differential_pressure_raw_pa, _diff_pres.error_count, 100); + +#ifdef __PX4_POSIX + _airspeed.confidence = 1.0f; +#else _airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time()); +#endif /* don't risk to feed negative airspeed into the system */ _airspeed.indicated_airspeed_m_s = math::max(0.0f, From 7fbd4a0b5bd0b7cbfd55f25cd417706901f33ab2 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 18 Feb 2016 21:20:26 -0500 Subject: [PATCH 8/9] sensors fix code style --- 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 24ae7c7705..4fa37baf90 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1228,7 +1228,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) /* push data into validator */ _airspeed_validator.put(_airspeed.timestamp, _diff_pres.differential_pressure_raw_pa, _diff_pres.error_count, 100); - + #ifdef __PX4_POSIX _airspeed.confidence = 1.0f; #else From 7c91aaee641016392de7779ca57a09f3f133aca4 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 18 Feb 2016 17:30:25 -0700 Subject: [PATCH 9/9] add generic 250 quad_x config tuned for acro mode --- ROMFS/px4fmu_common/init.d/4050_generic_250 | 39 +++++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/4050_generic_250 diff --git a/ROMFS/px4fmu_common/init.d/4050_generic_250 b/ROMFS/px4fmu_common/init.d/4050_generic_250 new file mode 100644 index 0000000000..3aa1388e1d --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4050_generic_250 @@ -0,0 +1,39 @@ +#!nsh +# +# @name generic_250 +# tuned for acro mode on Spedix S250AQ (3S, stock ESCs and 2300KV motors, 5x4.5 bullnose props) +# +# @type Quadrotor x +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Mark Whitehorn +# + +sh /etc/init.d/4001_quad_x + +if [ $AUTOCNF == yes ] +then + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_I 0.1 + param set MC_ROLLRATE_D 0.005 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_I 0.2 + param set MC_PITCHRATE_D 0.005 + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.1 + param set MC_YAWRATE_D 0.0 + param set PWM_MIN 980 + param set MPC_THR_MIN 0.06 + param set MPC_MANTHR_MIN 0.06 + param set MC_ROLLRATE_MAX 360.0 + param set MC_PITCHRATE_MAX 360.0 + param set MC_YAWRATE_MAX 200.0 + # att_estimator_q gyro bias estimates cause problems with acro mode + param set ATT_BIAS_MAX 0.0 +fi