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 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 42e4b835fc..b2faa43e1d 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -77,30 +77,48 @@ #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_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 +#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_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC +#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_BLOCK_ACCESS 0x44 + +#define BATT_SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC #ifndef CONFIG_SCHED_WORKQUEUE # 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: @@ -133,6 +151,62 @@ 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); + + /** + * 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 @@ -166,18 +240,41 @@ 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); + + /** + * 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 @@ -187,6 +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) + 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 @@ -198,15 +300,27 @@ 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(); +int solo_battery_check(); + 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), + _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... memset(&_work, 0, sizeof(_work)); @@ -223,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 @@ -297,8 +423,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); } } @@ -343,6 +469,89 @@ 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; +} + +bool +BATT_SMBUS::is_solo_battery() +{ + check_if_solo_battery(); + return _is_solo_battery; +} + int BATT_SMBUS::probe() { @@ -381,11 +590,53 @@ 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; } + 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; @@ -403,7 +654,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 | @@ -426,14 +677,42 @@ 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) { + 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"); } } @@ -453,7 +732,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 @@ -480,6 +759,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 +820,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 +853,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,17 +906,133 @@ 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; +} + +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 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 +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[]) { @@ -649,6 +1110,41 @@ 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(); + solo_battery_check(); + exit(0); + } + batt_smbus_usage(); exit(0); } 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, diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d442ce65ac..4fa37baf90 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,