mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-17 20:53:35 +08:00
added an SMBus base class
This commit is contained in:
Jake Dahl
@@ -34,3 +34,4 @@
|
||||
add_subdirectory(airspeed)
|
||||
add_subdirectory(device)
|
||||
add_subdirectory(led)
|
||||
add_subdirectory(smbus)
|
||||
|
||||
@@ -0,0 +1,34 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2018 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(drivers__smbus SMBus.cpp)
|
||||
@@ -0,0 +1,163 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "SMBus.hpp"
|
||||
|
||||
SMBus::SMBus(int bus_num, uint16_t address) :
|
||||
I2C("BATT_SMBUS_I2C", nullptr, bus_num, address, 100000)
|
||||
{
|
||||
init();
|
||||
}
|
||||
|
||||
SMBus::~SMBus()
|
||||
{
|
||||
}
|
||||
|
||||
int SMBus::read_word(const uint8_t cmd_code, void *data)
|
||||
{
|
||||
// 2 data bytes + pec byte
|
||||
int result = transfer(&cmd_code, 1, (uint8_t *)data, 3);
|
||||
|
||||
if (result == PX4_OK) {
|
||||
// Check PEC.
|
||||
uint8_t addr = get_device_address() << 1;
|
||||
uint8_t full_data_packet[5];
|
||||
full_data_packet[0] = addr | 0x00;
|
||||
full_data_packet[1] = cmd_code;
|
||||
full_data_packet[2] = addr | 0x01;
|
||||
memcpy(&full_data_packet[3], data, 2);
|
||||
|
||||
uint8_t pec = get_pec(full_data_packet, sizeof(full_data_packet) / sizeof(full_data_packet[0]));
|
||||
|
||||
if (pec == ((uint8_t *)data)[2]) {
|
||||
return PX4_OK;
|
||||
|
||||
} else {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
int SMBus::block_read(const uint8_t cmd_code, void *data, const uint8_t length)
|
||||
{
|
||||
unsigned byte_count = 0;
|
||||
// Length of data (32max). byte_count(1), cmd_code(2), pec(1)
|
||||
uint8_t rx_data[32 + 4];
|
||||
|
||||
transfer(&cmd_code, 1, (uint8_t *)rx_data, length + 2);
|
||||
|
||||
byte_count = rx_data[0];
|
||||
|
||||
// addr1, addr2, byte_count,
|
||||
memcpy(data, &rx_data[1], byte_count);
|
||||
|
||||
// addr(wr), cmd_code, addr(r), byte_count, rx_data[]
|
||||
uint8_t device_address = get_device_address();
|
||||
uint8_t full_data_packet[32 + 4] = {0};
|
||||
|
||||
full_data_packet[0] = (device_address << 1) | 0x00;
|
||||
full_data_packet[1] = cmd_code;
|
||||
full_data_packet[2] = (device_address << 1) | 0x01;
|
||||
full_data_packet[3] = byte_count;
|
||||
|
||||
memcpy(&full_data_packet[4], &rx_data[1], byte_count);
|
||||
|
||||
uint8_t pec = get_pec(full_data_packet, byte_count + 4);
|
||||
|
||||
// First byte is byte count, followed by data.
|
||||
if (pec != ((uint8_t *)rx_data)[byte_count + 1]) {
|
||||
PX4_INFO("bad PEC from block_read");
|
||||
return PX4_ERROR;
|
||||
|
||||
} else {
|
||||
return PX4_OK;
|
||||
}
|
||||
}
|
||||
|
||||
int SMBus::block_write(const uint8_t cmd_code, void *data, const uint8_t byte_count)
|
||||
{
|
||||
// cmd code, byte count, data[byte_count], pec
|
||||
uint8_t buf[byte_count + 2];
|
||||
|
||||
buf[0] = cmd_code;
|
||||
buf[1] = (uint8_t)byte_count;
|
||||
memcpy(&buf[2], data, byte_count);
|
||||
|
||||
uint8_t pec = get_pec(buf, sizeof(buf));
|
||||
buf[byte_count + 2] = pec;
|
||||
|
||||
unsigned i = 0;
|
||||
|
||||
// If block_write fails, try up to 10 times.
|
||||
while (i < 10) {
|
||||
if (PX4_OK != transfer((uint8_t *)buf, sizeof(buf), nullptr, 0)) {
|
||||
i++;
|
||||
PX4_WARN("block_write failed: %d", i);
|
||||
usleep(100000);
|
||||
|
||||
} else {
|
||||
return PX4_OK;
|
||||
}
|
||||
}
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
uint8_t SMBus::get_pec(uint8_t *buff, const uint8_t len)
|
||||
{
|
||||
// Initialise CRC to zero.
|
||||
uint8_t crc = 0;
|
||||
uint8_t shift_register = 0;
|
||||
bool invert_crc;
|
||||
|
||||
// Calculate crc for each byte in the stream.
|
||||
for (uint8_t i = 0; i < len; i++) {
|
||||
// Load next data byte into the shift register
|
||||
shift_register = buff[i];
|
||||
|
||||
// Calculate crc for each bit in the current byte.
|
||||
for (uint8_t j = 0; j < 8; j++) {
|
||||
invert_crc = (crc ^ shift_register) & 0x80;
|
||||
crc <<= 1;
|
||||
shift_register <<= 1;
|
||||
|
||||
if (invert_crc) {
|
||||
crc ^= SMBUS_PEC_POLYNOMIAL;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <drivers/device/i2c.h>
|
||||
|
||||
#include <string.h>
|
||||
|
||||
#define SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC
|
||||
|
||||
class SMBus : public device::I2C
|
||||
{
|
||||
public:
|
||||
SMBus(int bus_num, uint16_t address);
|
||||
~SMBus();
|
||||
|
||||
/**
|
||||
* @brief Sends a block write command.
|
||||
* @param cmd_code The command code.
|
||||
* @param data The data to be written.
|
||||
* @param length The number of bytes being written.
|
||||
* @return Returns PX4_OK on success, PX4_ERROR on failure.
|
||||
*/
|
||||
int block_write(const uint8_t cmd_code, void *data, const uint8_t byte_count);
|
||||
|
||||
/**
|
||||
* @brief Sends a block read command.
|
||||
* @param cmd_code The command code.
|
||||
* @param data The returned data.
|
||||
* @param length The number of bytes being read
|
||||
* @return Returns PX4_OK on success, PX4_ERROR on failure.
|
||||
*/
|
||||
int block_read(const uint8_t cmd_code, void *data, const uint8_t length);
|
||||
|
||||
/**
|
||||
* @brief Sends a read word command.
|
||||
* @param cmd_code The command code.
|
||||
* @param data The 2 bytes of returned data plus a 1 byte CRC if used.
|
||||
* @return Returns PX4_OK on success, PX4_ERROR on failure.
|
||||
*/
|
||||
int read_word(const uint8_t cmd_code, void *data);
|
||||
|
||||
/**
|
||||
* @brief Calculates the PEC from the data.
|
||||
* @param buffer The buffer that stores the data to perform the CRC over.
|
||||
* @param length The number of bytes being written.
|
||||
* @return Returns PX4_OK on success, PX4_ERROR on failure.
|
||||
*/
|
||||
uint8_t get_pec(uint8_t *buffer, uint8_t length);
|
||||
|
||||
};
|
||||
Reference in New Issue
Block a user