diff --git a/acoustics/acoustics_data_record/CMakeLists.txt b/acoustics/acoustics_data_record/CMakeLists.txt new file mode 100644 index 00000000..ee711f38 --- /dev/null +++ b/acoustics/acoustics_data_record/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.8) +project(acoustics_data_record) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(rclpy REQUIRED) +find_package(std_msgs REQUIRED) + +# Install launch files. +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS + ${PROJECT_NAME}/acoustics_data_record_lib.py + ${PROJECT_NAME}/acoustics_data_record_node.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/acoustics/acoustics_data_record/README b/acoustics/acoustics_data_record/README new file mode 100644 index 00000000..5e245109 --- /dev/null +++ b/acoustics/acoustics_data_record/README @@ -0,0 +1,6 @@ +A handy ROS2 node to record data from acoustic PCB +Has the ability to display data live in real time +OBS!: Make sure the Acoustics Interface Node is running before starting acoustics recording node, otherwise you will be recording nothing :P + +Use utilities/display_acoustics_data_live.py to display data in real time while it is being saved into the .csv file through the ROS2 node +(Verry cool to look at, would recoment ;DDD) diff --git a/acoustics/acoustics_data_record/acoustics_data/.gitignore b/acoustics/acoustics_data_record/acoustics_data/.gitignore new file mode 100644 index 00000000..f59ec20a --- /dev/null +++ b/acoustics/acoustics_data_record/acoustics_data/.gitignore @@ -0,0 +1 @@ +* \ No newline at end of file diff --git a/acoustics/acoustics_data_record/acoustics_data_record/__init__.py b/acoustics/acoustics_data_record/acoustics_data_record/__init__.py new file mode 100755 index 00000000..e69de29b diff --git a/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_lib.py b/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_lib.py new file mode 100755 index 00000000..d148f75d --- /dev/null +++ b/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_lib.py @@ -0,0 +1,78 @@ +# Python Libraries +import time +import csv +from datetime import datetime + + + +class AcousticsDataRecordLib: + def __init__(self, + ROS2_PACKAGE_DIRECTORY = "" + ): + # Global variables for .csv file manipulation ---------- + # Get the path for the directory where we will store our data + self.acoustics_data_directory = ROS2_PACKAGE_DIRECTORY + "acoustics_data/" + + timestamp = time.strftime('%Y-%m-%d_%H:%M:%S') + data_file_name = 'acoustics_data_' + timestamp + '.csv' + self.data_file_location = self.acoustics_data_directory + data_file_name + + self.csv_headers = [ + "Time", + + "Hydrophone1", + "Hydrophone2", + "Hydrophone3", + "Hydrophone4", + "Hydrophone5", + + "FilterResponse", + "FFT", + "Peaks", + + "TDOA", + "Position", + ] + + # Make new .csv file for loging blackbox data ---------- + with open(self.data_file_location, mode="w", newline="") as csv_file: + writer = csv.writer(csv_file) + writer.writerow(self.csv_headers) + + # Methods for external uses ---------- + def log_data_to_csv_file(self, + hydrophone1 = [0], + hydrophone2 = [0], + hydrophone3 = [0], + hydrophone4 = [0], + hydrophone5 = [0], + + filter_response = [0], + fft = [0], + peaks = [0], + + tdoa = [0.0], + position = [0.0], + ): + # Get current time in hours, minutes, seconds and miliseconds + current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3] + + # Write to .csv file + with open(self.data_file_location, mode="a", newline="") as csv_file: + writer = csv.writer(csv_file) + writer.writerow([ + current_time, + + hydrophone1, + hydrophone2, + hydrophone3, + hydrophone4, + hydrophone5, + + filter_response, + fft, + peaks, + + tdoa, + position, + ]) \ No newline at end of file diff --git a/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_node.py b/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_node.py new file mode 100755 index 00000000..61a1e9d5 --- /dev/null +++ b/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_node.py @@ -0,0 +1,197 @@ +#!/usr/bin/env python3 + +# ROS2 libraries +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float32MultiArray, Int32MultiArray +from ament_index_python.packages import get_package_share_directory + +# Python libraries +import array + +# Custom libraries +from acoustics_data_record_lib import AcousticsDataRecordLib + + + +class AcousticsDataRecordNode(Node): + def __init__(self): + # Variables for seting upp loging correctly + hydrophoneDataSize = (2**10) * 3 # 1 hydrophone buffer is 2^10 long, Each hydrophone data has 3 buffers full of this data + DSPDataSize = 2**10 # DSP (Digital Signal Processing) has 2^10 long data + TDOADataSize = 5 # TDOA (Time Difference Of Arrival) has 5 hydrophones it has times for + positionDataSize = 3 # position only has X, Y, Z basicaly 3 elements + + # Initialize ROS2 node + super().__init__('acoustics_data_record_node') + + # Initialize Subscribers ---------- + # Start listening to Hydrophone data + self.subscriberHydrophone1 = self.create_subscription( + Int32MultiArray, + '/acoustics/hydrophone1', + self.hydrophone1_callback, + 5) + self.hydropone1Data = array.array("i", [0] * hydrophoneDataSize) + + self.subscriberHydrophone2 = self.create_subscription( + Int32MultiArray, + '/acoustics/hydrophone2', + self.hydrophone2_callback, + 5) + self.hydropone2Data = array.array("i", [0] * hydrophoneDataSize) + + self.subscriberHydrophone3 = self.create_subscription( + Int32MultiArray, + '/acoustics/hydrophone3', + self.hydrophone3_callback, + 5) + self.hydropone3Data = array.array("i", [0] * hydrophoneDataSize) + + self.subscriberHydrophone4 = self.create_subscription( + Int32MultiArray, + '/acoustics/hydrophone4', + self.hydrophone4_callback, + 5) + self.hydropone4Data = array.array("i", [0] * hydrophoneDataSize) + + self.subscriberHydrophone5 = self.create_subscription( + Int32MultiArray, + '/acoustics/hydrophone5', + self.hydrophone5_callback, + 5) + self.hydropone5Data = array.array("i", [0] * hydrophoneDataSize) + + # Start listening to DSP (Digital Signal Processing) data + self.subscriberFilterResponse = self.create_subscription( + Int32MultiArray, + '/acoustics/filter_response', + self.filter_response_callback, + 5) + self.filterResponseData = array.array("i", [0] * DSPDataSize) + + self.subscriberFFT = self.create_subscription( + Int32MultiArray, + '/acoustics/fft', + self.fft_callback, + 5) + self.FFTData = array.array("i", [0] * DSPDataSize) + + self.subscriberPeaks = self.create_subscription( + Int32MultiArray, + '/acoustics/peaks', + self.peaks_callback, + 5) + self.peaksData = array.array("i", [0] * DSPDataSize) + + # Start listening to Multilateration data + self.subscriberTDOAResponse = self.create_subscription( + Float32MultiArray, + '/acoustics/time_difference_of_arrival', + self.tdoa_callback, + 5) + self.TDOAData = array.array("f", [0.0] * TDOADataSize) + + self.subscriberPositionResponse = self.create_subscription( + Float32MultiArray, + '/acoustics/position', + self.position_callback, + 5) + self.positionData = array.array("f", [0.0] * positionDataSize) + + # Initialize logger ---------- + # Get package directory location + ros2_package_directory_location = get_package_share_directory("acoustics_data_record") + ros2_package_directory_location = ros2_package_directory_location + "/../../../../" # go back to workspace + ros2_package_directory_location = ros2_package_directory_location + "src/vortex-asv/acoustics/acoustics_data_record/" # Navigate to this package + + # Make blackbox loging file + self.acoustics_data_record = AcousticsDataRecordLib( + ROS2_PACKAGE_DIRECTORY = ros2_package_directory_location + ) + + # Logs all the newest data 1 time(s) per second + self.declare_parameter("acoustics.data_logging_rate", 1.0) # Providing a default value 1.0 => 1 samplings per second, verry slow + DATA_LOGING_RATE = self.get_parameter("acoustics.data_logging_rate").get_parameter_value().double_value + timer_period = 1.0/DATA_LOGING_RATE + self.logger_timer = self.create_timer(timer_period, self.logger) + + # Debuging ---------- + self.get_logger().info( + "Started logging data for topics: \n" + "/acoustics/hydrophone1 [Int32MultiArray] \n" + "/acoustics/hydrophone2 [Int32MultiArray] \n" + "/acoustics/hydrophone3 [Int32MultiArray] \n" + "/acoustics/hydrophone4 [Int32MultiArray] \n" + "/acoustics/hydrophone5 [Int32MultiArray] \n" + "/acoustics/filter_response [Int32MultiArray] \n" + "/acoustics/fft [Int32MultiArray] \n" + "/acoustics/peaks [Int32MultiArray] \n" + "/acoustics/time_difference_of_arrival [Float32MultiArray] \n" + "/acoustics/position [Float32MultiArray] \n" + ) + + # Callback methods for diffrenet topics + def hydrophone1_callback(self, msg): + self.hydropone1Data = msg.data + + def hydrophone2_callback(self, msg): + self.hydropone2Data = msg.data + + def hydrophone3_callback(self, msg): + self.hydropone3Data = msg.data + + def hydrophone4_callback(self, msg): + self.hydropone4Data = msg.data + + def hydrophone5_callback(self, msg): + self.hydropone5Data = msg.data + + def filter_response_callback(self, msg): + self.filterResponseData = msg.data + + def fft_callback(self, msg): + self.FFTData = msg.data + + def peaks_callback(self, msg): + self.peaksData = msg.data + + def tdoa_callback(self, msg): + self.TDOAData = msg.data + + def position_callback(self, msg): + self.positionData = msg.data + + # The logger that logs all the data + def logger(self): + self.acoustics_data_record.log_data_to_csv_file( + hydrophone1=self.hydropone1Data, + hydrophone2=self.hydropone2Data, + hydrophone3=self.hydropone3Data, + hydrophone4=self.hydropone4Data, + hydrophone5=self.hydropone5Data, + + filter_response=self.filterResponseData, + fft=self.FFTData, + peaks=self.peaksData, + + tdoa=self.TDOAData, + position=self.positionData, + ) + + + +def main(): + # Initialize ROS2 + rclpy.init() + + # Start the ROS2 node and continue forever until exit + acoustics_data_record_node = AcousticsDataRecordNode() + rclpy.spin(acoustics_data_record_node) + + # Destroy the node explicitly once ROS2 stops runing + acoustics_data_record_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/acoustics/acoustics_data_record/launch/__init__.py b/acoustics/acoustics_data_record/launch/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py b/acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py new file mode 100644 index 00000000..508f088b --- /dev/null +++ b/acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py @@ -0,0 +1,24 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + # Path to the YAML file + yaml_file_path = os.path.join( + get_package_share_directory("blackbox"), + "../../../../", # Go to the workspace + "src/vortex-asv/asv_setup/config/robots/", # Go inside where yamal files are located at + 'freya.yaml' + ) + + return LaunchDescription([ + Node( + package='acoustics_data_record', + namespace='acoustics_data_record', + executable='acoustics_data_record_node.py', + name='acoustics_data_record_node', + output='screen', + parameters=[yaml_file_path], + ), + ]) \ No newline at end of file diff --git a/acoustics/acoustics_data_record/package.xml b/acoustics/acoustics_data_record/package.xml new file mode 100644 index 00000000..22c36c11 --- /dev/null +++ b/acoustics/acoustics_data_record/package.xml @@ -0,0 +1,19 @@ + + + + acoustics_data_record + 1.0.0 + Records data from the acoustics + vortex + MIT + + + rclpy + std_msgs + ros2launch + + + + ament_cmake + + diff --git a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py new file mode 100644 index 00000000..b1c1e3de --- /dev/null +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -0,0 +1,275 @@ +#!/usr/bin/env python3 + +# Libraries for file manipulation +import os +import sys +import ast +import glob + +# Libraries for handling data structures +import pandas as pd +import numpy as np +import array + +# Libraries for anmation +import matplotlib.animation as animation +import matplotlib.gridspec as gridspec +import matplotlib.pyplot as plt + + + +# Variables for seting upp data structures correctly +hydrophoneDataSize = (2**10) * 3 # 1 hydrophone buffer is 2^10 long, Each hydrophone data has 3 buffers full of this data +DSPDataSize = 2**10 # DSP (Digital Signal Processing) has 2^10 long data +TDOADataSize = 5 # TDOA (Time Difference Of Arrival) has 5 hydrophones it has times for +positionDataSize = 3 # position only has X, Y, Z basicaly 3 elements + +# Important variables for later processing of data +SAMPLE_RATE = 430_000 # 430 kHz +MAX_FREQUENCY_TO_SHOW = 60_000 # 60 kHz +FPS = 1 + + + +# Make a good plot layout ================================================== +fig = plt.figure() +# Create an outer GridSpec for the two columns +outer_gs = gridspec.GridSpec(1, 2, figure=fig, width_ratios=[1, 1]) +# Create an inner GridSpec for the first column +gs_hydrophone = gridspec.GridSpecFromSubplotSpec( + 5, 1, subplot_spec=outer_gs[0], hspace=0.1 +) +# Create an inner GridSpec for the second column, with height ratios for the 70%/30% split +gs_dsp = gridspec.GridSpecFromSubplotSpec( + 2, 1, subplot_spec=outer_gs[1], height_ratios=[7, 3], hspace=0.3 +) + +hydrophoneAxis = [None] * 5 + +# Add subplots in the first column for hydrophone data +for i in range(5): + hydrophoneAxis[i] = fig.add_subplot( + gs_hydrophone[i, 0], sharex=hydrophoneAxis[0] if i else None + ) + hydrophoneAxis[i].label_outer() +fig.text(0.25, 0.965, "Hydrophone Data", ha="center") + +# Add subplots in the second column +FFTAxis = fig.add_subplot(gs_dsp[0]) +filterAxis = fig.add_subplot(gs_dsp[1]) + +# Plot type so that the size is dynamic +plt.tight_layout() + +# Select nice color pallet for graphs +colorSoftPurple = (168 / 255, 140 / 255, 220 / 255) +colorSoftBlue = (135 / 255, 206 / 255, 250 / 255) +colorSoftGreen = (122 / 255, 200 / 255, 122 / 255) + + + +# .CSV Setup ================================================== +# Get Directory of the .csv files +PACKAGE_DIR = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) +ACOUSTICS_CSV_FILE_DIR = PACKAGE_DIR + "/acoustics_data" + +# List of all the acoustic files +acousticsCSVFiles = csv_files = glob.glob(ACOUSTICS_CSV_FILE_DIR + "/acoustics_data_" + "*.csv") + +# Get the latest csv file name for acoustics data +acousticsCSVFile = max(acousticsCSVFiles, key=os.path.getctime) + + + +def convertPandasObjectToIntArray(pandasObject): + pandasString = pandasObject.iloc[0].strip("array('i', ").rstrip(')') + pandasIntArray = [int(x.strip()) for x in pandasString.strip('[]').split(',')] + + return pandasIntArray + +def convertPandasObjectToFloatArray(pandasObject): + pandasString = pandasObject.iloc[0].strip("array('f', ").rstrip(')') + pandasFloatArray = [float(x.strip()) for x in pandasString.strip('[]').split(',')] + + return pandasFloatArray + + + +def getAcousticsData(): + # Variables that will be filled with latest acoustics data ---------- + hydrophone1 = [0] * hydrophoneDataSize + hydrophone2 = [0] * hydrophoneDataSize + hydrophone3 = [0] * hydrophoneDataSize + hydrophone4 = [0] * hydrophoneDataSize + hydrophone5 = [0] * hydrophoneDataSize + + unfilteredData = [0] * DSPDataSize + filteredData = [0] * DSPDataSize + FFTData = [0] * DSPDataSize + peaksData = [0] * DSPDataSize + FFTAmplitudeData = [0] * DSPDataSize + FFTFrequencyData = [0] * DSPDataSize + peaksAmplitudeData = [0] * DSPDataSize + peaksFrequencyData = [0] * DSPDataSize + + tdoaData = [0.0] * TDOADataSize + positonData = [0.0] * positionDataSize + + # Read latest acoustics data ---------- + acousticsDataFrame = pd.read_csv(acousticsCSVFile) + latestAcousticsData = acousticsDataFrame.tail(1) + + try: + # Get latest hydrophone data + hydrophone1 = convertPandasObjectToIntArray(latestAcousticsData["Hydrophone1"]) + hydrophone2 = convertPandasObjectToIntArray(latestAcousticsData["Hydrophone2"]) + hydrophone3 = convertPandasObjectToIntArray(latestAcousticsData["Hydrophone3"]) + hydrophone4 = convertPandasObjectToIntArray(latestAcousticsData["Hydrophone4"]) + hydrophone5 = convertPandasObjectToIntArray(latestAcousticsData["Hydrophone5"]) + + # Unfiltered data is special as it is the same as Hydrohone 1 first 1024 values + # This is because Acoustics PCB uses Hydrophone 1 to perform DSP + # Hydrohones have a ring buffer the size of 3 buffers each containing 1024 values (2^10) + # We always use the first ring buffer of Hydrophone 1 to performe DSP + # That is why unfiltered data is the same as Hydrphne 1 first buffer + unfilteredData = hydrophone1[0:1024] + + # Get DSP data + filteredData = convertPandasObjectToIntArray(latestAcousticsData["FilterResponse"]) # Also known as Filter response to the raw unfiltered data + FFTData = convertPandasObjectToIntArray(latestAcousticsData["FFT"]) + peaksData = convertPandasObjectToIntArray(latestAcousticsData["Peaks"]) + + # Get multilateration data + tdoaData = convertPandasObjectToFloatArray(latestAcousticsData["TDOA"]) + positonData = convertPandasObjectToFloatArray(latestAcousticsData["Position"]) + except: + print("ERROR: Coulden't read acoustics data") + + # Post process DSP data to desired scale and amount ---------- + # 1. Convert FFTData to its corresponding frequency amount + # 2. Cut out big FFT frequencies out as they are not relevant + # 3. Cut out big peak frequencies as they are not relevant + sampleLength = len(FFTData) + maxFrequencyIndex = int(MAX_FREQUENCY_TO_SHOW * sampleLength / SAMPLE_RATE) + + FFTAmplitudeData = FFTData[0:maxFrequencyIndex] + FFTFrequencyData = [(i * (SAMPLE_RATE / sampleLength)) for i in range(sampleLength)] + FFTFrequencyData = FFTFrequencyData[0:maxFrequencyIndex] + + # Peaks data is special as each peak data value is a array of [Amplitude, Frequency, Phase] of the peak + # We want to get amplitude and frequency, dont really care about the phase + try: + tempAmplitude = [] + tempFrequency = [] + for i in range(1, len(peaksData), 3): + if peaksData[i] < MAX_FREQUENCY_TO_SHOW: + tempAmplitude += [peaksData[i - 1]] + tempFrequency += [peaksData[i]] + + peaksAmplitudeData = tempAmplitude + peaksFrequencyData = tempFrequency + except: + print("ERROR processing DSP data") + + # return processed data ---------- + return [ + hydrophone1, + hydrophone2, + hydrophone3, + hydrophone4, + hydrophone5, + + unfilteredData, + + filteredData, + FFTAmplitudeData, + FFTFrequencyData, + peaksAmplitudeData, + peaksFrequencyData, + + tdoaData, + positonData, + ] + + + +def display_live_data(frame): + # Get latest acoustics data + acousticsData = getAcousticsData() + + # Set the lates acoustics data in apropriate variables + hydrophoneData = [ + acousticsData[0], # Hydrophone 1 + acousticsData[1], # Hydrophone 2 + acousticsData[2], # Hydrophone 3 + acousticsData[3], # Hydrophone 4 + acousticsData[4], # Hydrophone 5 + ] + + unfilteredData = acousticsData[5] + + filterData = acousticsData[6] + FFTAmplitudeData = acousticsData[7] + FFTFrequencyData = acousticsData[8] + peaksAmplitudeData = acousticsData[9] + peaksFrequencyData = acousticsData[10] + + tdoaData = acousticsData[11] # Currently not in use + positionData = acousticsData[12] # Currently not in use + + # Plot hydrophone data + for i in range(5): + xHydrophone = list(range(len(hydrophoneData[i][::]))) + hydrophoneAxis[i].clear() + hydrophoneAxis[i].plot( + xHydrophone, + hydrophoneData[i], + label=f"Hydrophone {i + 1}", + color=colorSoftBlue, + alpha=1, + ) + hydrophoneAxis[i].legend(loc="upper right", fontsize="xx-small") + + # Plot Filter response + xRaw = list(range(len(unfilteredData))) + xFilter = list(range(len(filterData))) + filterAxis.clear() + filterAxis.set_title("Filter response") + filterAxis.plot(xRaw, unfilteredData, label="Raw", color=colorSoftBlue, alpha=0.5) + filterAxis.plot( + xFilter, filterData, label="Filter", color=colorSoftGreen, alpha=0.7 + ) + filterAxis.legend(loc="upper right", fontsize="xx-small") + + # Plot FFT data + FFTAxis.clear() + FFTAxis.set_title("FFT") + FFTAxis.set_xlabel("Frequency [Hz]") + FFTAxis.set_ylabel("Amplitude") + FFTAxis.bar( + FFTFrequencyData, + FFTAmplitudeData, + label="FFT", + color=colorSoftPurple, + alpha=1, + width=500, + ) + FFTAxis.scatter( + peaksFrequencyData, + peaksAmplitudeData, + label="Peaks", + color="red", + alpha=0.7, + s=30, + linewidths=1.4, + marker="x", + ) + FFTAxis.legend(loc="upper right", fontsize="xx-small") + + # Print out the unused Multilateration data + print(f"TDOA Data: {tdoaData} | Position Data: {positionData}") + +# Plotting live data +ani = animation.FuncAnimation(fig, display_live_data, interval=1000/FPS) +plt.show() + diff --git a/acoustics/acoustics_interface/CMakeLists.txt b/acoustics/acoustics_interface/CMakeLists.txt new file mode 100644 index 00000000..82d472cb --- /dev/null +++ b/acoustics/acoustics_interface/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.8) +project(acoustics_interface) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(rclpy REQUIRED) +find_package(std_msgs REQUIRED) + +ament_python_install_package(acoustics_interface) + +# Install launch files. +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + +# Executables +install(PROGRAMS + ${PROJECT_NAME}/acoustics_interface_node.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/acoustics/acoustics_interface/README b/acoustics/acoustics_interface/README new file mode 100644 index 00000000..25c1a8ea --- /dev/null +++ b/acoustics/acoustics_interface/README @@ -0,0 +1,2 @@ +Interface to comunicate with acoustics PCB and Teensy 4.1 MCU +OBS!: Make sure to connect Teensy 4.1 MCU to the ethernet \ No newline at end of file diff --git a/acoustics/acoustics_interface/acoustics_interface/__init__.py b/acoustics/acoustics_interface/acoustics_interface/__init__.py new file mode 100755 index 00000000..e69de29b diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py new file mode 100755 index 00000000..90224a9c --- /dev/null +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -0,0 +1,214 @@ +# Setting up libraries +import os +import sys +from socket import * +import netifaces as ni +from enum import Enum +import errno +import time + + +class TeensyCommunicationUDP: + # Teensy networking Setup + TEENSY_IP = "10.0.0.111" + TEENSY_PORT = 8888 + MY_PORT = 9999 + MAX_PACKAGE_SIZE_RECEIVED = 65536 + TIMEOUT = 1 + address = (TEENSY_IP, TEENSY_PORT) + + # Code words for communication + INITIALIZATION_MESSAGE = "HELLO :D" # This is a message only sent once to establish 2 way communication between Teensy and client + + clientSocket = socket(AF_INET, SOCK_DGRAM) + + _timeoutMax = 10 + _data_string = "" + _data_target = "" + acoustics_data = { + "HYDROPHONE_1": [0], + "HYDROPHONE_2": [0], + "HYDROPHONE_3": [0], + "HYDROPHONE_4": [0], + "HYDROPHONE_5": [0], + "SAMPLES_FILTERED": [0], + "FFT": [0], + "PEAK": [0], + "TDOA": [0], + "LOCATION": [0] + } + + @classmethod + def init_communication(cls, frequenciesOfInterest): + assert len(frequenciesOfInterest) == 10, "Frequency list has to have exactly 10 entries" + + _frequenciesOfInterest = frequenciesOfInterest + + cls.MY_IP = cls.get_ip() + + # Socket setup + cls.clientSocket.settimeout(cls.TIMEOUT) + cls.clientSocket.bind((cls.MY_IP, cls.MY_PORT)) + cls.clientSocket.setblocking(False) + + cls.send_acknowledge_signal() + timeStart = time.time() + + # Wait for READY signal + while not cls.check_if_available(): + """ + IMPORTANT! + DO NOT have "time.sleep(x)" value SMALLER than 1 second!!! + This will interrupt sampling by asking teensy if its available to many times + If less than 1 second you risc crashing teensy to PC communication O_O + """ + + print("Did not receive READY signal. Will wait.") + time.sleep(1) + + if time.time() - timeStart > cls._timeoutMax: + print("Gave up on receiving READY. Sending acknowledge signal again") + # Start over + timeStart = time.time() + cls.send_acknowledge_signal() + + print("READY signal received, sending frequencies...") + cls.send_frequencies_of_interest(frequenciesOfInterest) + + @classmethod + def fetch_data(cls): + i = 0 + + while True: + data = cls.get_raw_data() + + if data == None: + return + + if data not in cls.acoustics_data.keys(): + cls._data_string += data + else: + cls.write_to_target() + cls._data_target = data + + # Ah yes, my good friend code safety + i += 1 + + if i > 1000: + i = 0 + print("Max tries exceeded") + break + + @classmethod + def write_to_target(cls): + if cls._data_target == "TDOA" or cls._data_target == "LOCATION": + data = cls.parse_data_string(is_float=True) + else: + data = cls.parse_data_string(is_float=False) + + if data == None: + cls._data_string = "" + return + + cls.acoustics_data[cls._data_target] = data + + cls._data_string = "" + + @classmethod + def get_raw_data(cls) -> str | None: + try: + rec_data, _ = cls.clientSocket.recvfrom(cls.MAX_PACKAGE_SIZE_RECEIVED) + messageReceived = rec_data.decode() + return messageReceived + except error as e: # `error` is really `socket.error` + if e.errno == errno.EWOULDBLOCK: + pass + else: + print("Socket error: ", e) + + @classmethod + def parse_data_string(cls, is_float: bool): + if cls._data_string == '': return + + try: + # Format data from CSV string to floats, ignore last value + if is_float: + return list(map(float, cls._data_string.split(",")[:-1])) + else: + return list(map(int, cls._data_string.split(",")[:-1])) + except Exception as e: + print(f"The string '{cls._data_string}' caused an error when parsing") + print(f"The exception was: {e}") + + # stackoverflow <3 + # https://stackoverflow.com/questions/166506/finding-local-ip-addresses-using-pythons-stdlib + @classmethod + def get_ip(cls): + s = socket(AF_INET, SOCK_DGRAM) + s.settimeout(0) + try: + # doesn't even have to be reachable + s.connect((cls.TEENSY_IP, 1)) + IP = s.getsockname()[0] + except Exception: + IP = '127.0.0.1' + finally: + s.close() + + return IP + + @classmethod + def send_acknowledge_signal(cls): + try: + cls.clientSocket.sendto(cls.INITIALIZATION_MESSAGE.encode(), cls.address) + print("DEBUGING: Sent acknowledge package") + except Exception as e: + print("Error from send_acknowledge_signal") + print(e) + pass + + @classmethod + def check_if_available(cls): + try: + i = 0 + while True: + # Read data + message = cls.get_raw_data() + # Check if there is no more data left + if message == None: + return False + + # Check if correct signal was sent + if message == "READY": + return True + + i += 1 + + if i > 200: + i = 0 + print("Max tries exceeded") + break + except Exception as e: + print(f"check_if_available rased exception: {e}") + return False + + @classmethod + def send_frequencies_of_interest(cls, frequenciesOfInterest: list[tuple[float, float]]): + """ + To ignore entries in the frequency list, just make the frequency entry -1, and make the variance 0 + """ + try: + + # Format (CSV): xxx,x,xx,x...,x (frequency list comes first, then variances) + assert len(frequenciesOfInterest) == 10, "List of frequencies has to be ten entries long!" + + # ten messages in total, one message for each entry to work around the max packet size + for (frequency, variance) in frequenciesOfInterest: + frequency_variance_msg = f"{str(frequency)},{str(variance)}," + + # print(self.address); + cls.clientSocket.sendto(frequency_variance_msg.encode(), cls.address) + except: + print("Couldn't send Frequency data") + + diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py new file mode 100755 index 00000000..d7260939 --- /dev/null +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py @@ -0,0 +1,85 @@ +#!/usr/bin/python3 + +import rclpy +from rclpy.node import Node +import rclpy.logging +from std_msgs.msg import Int32MultiArray, Float32MultiArray +from acoustics_interface.acoustics_interface_lib import TeensyCommunicationUDP + +class AcousticsInterfaceNode(Node): + def __init__(self) -> None: + super().__init__('acoustics_interface') + rclpy.logging.initialize() + + self._hydrophone_1_publisher = self.create_publisher(Int32MultiArray, '/acoustics/hydrophone1', 5) + self._hydrophone_2_publisher = self.create_publisher(Int32MultiArray, '/acoustics/hydrophone2', 5) + self._hydrophone_3_publisher = self.create_publisher(Int32MultiArray, '/acoustics/hydrophone3', 5) + self._hydrophone_4_publisher = self.create_publisher(Int32MultiArray, '/acoustics/hydrophone4', 5) + self._hydrophone_5_publisher = self.create_publisher(Int32MultiArray, '/acoustics/hydrophone5', 5) + + self._filter_response_publisher = self.create_publisher(Int32MultiArray, '/acoustics/filter_response', 5) + self._fft_publisher = self.create_publisher(Int32MultiArray, '/acoustics/fft', 5) + self._peak_publisher = self.create_publisher(Int32MultiArray, '/acoustics/peaks', 5) + self._tdoa_publisher = self.create_publisher(Float32MultiArray, '/acoustics/time_difference_of_arrival', 5) + self._position_publisher = self.create_publisher(Float32MultiArray, '/acoustics/position', 5) + + + # Logs all the newest data + self.declare_parameter("acoustics.data_logging_rate", 1.0) # Providing a default value 1.0 => 1 samplings per second, verry slow + DATA_LOGING_RATE = self.get_parameter("acoustics.data_logging_rate").get_parameter_value().double_value + timer_period = 1.0/DATA_LOGING_RATE + + self._timer_data_update = self.create_timer(0.001, self.data_update) + self._timer_data_publisher = self.create_timer(timer_period, self.data_publisher) + + # Declare Frequency of interest parameters to send to Acoustics PCB to look out for + # This list has to be exactly 10 entries long (20 elements (10 frequencies + 10 variances)) + # format [(FREQUENCY, FREQUENCY_VARIANCE), ...] + self.declare_parameter("acoustics.frequencies_of_interest", [0] * 20) + FREQUENCIES_OF_INTEREST_PARAMETERS = self.get_parameter("acoustics.frequencies_of_interest").get_parameter_value().integer_array_value + + frequenciesOfInterest = [] + for i in range(0, len(FREQUENCIES_OF_INTEREST_PARAMETERS), 2): + frequenciesOfInterest += [(FREQUENCIES_OF_INTEREST_PARAMETERS[i], FREQUENCIES_OF_INTEREST_PARAMETERS[i + 1])] + + # Initialize comunication with Acoustics PCB + self.get_logger().info("Initializing comunication with Acoustics") + self.get_logger().info("Acoustics PCB MCU IP: 10.0.0.111") + self.get_logger().info("Trying to connect...") + + TeensyCommunicationUDP.init_communication(frequenciesOfInterest) + + self.get_logger().info("Sucsefully connected to Acoustics PCB MCU :D") + + + def data_update(self): + TeensyCommunicationUDP.fetch_data() + + def data_publisher(self): + self._hydrophone_1_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_1"])) + self._hydrophone_2_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_2"])) + self._hydrophone_3_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_3"])) + self._hydrophone_4_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_4"])) + self._hydrophone_5_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_5"])) + + self._filter_response_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["SAMPLES_FILTERED"])) + self._fft_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["FFT"])) + self._peak_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["PEAK"])) + + self._tdoa_publisher.publish(Float32MultiArray(data=TeensyCommunicationUDP.acoustics_data["TDOA"])) + self._position_publisher.publish(Float32MultiArray(data=TeensyCommunicationUDP.acoustics_data["LOCATION"])) + +def main(args=None): + rclpy.init(args=args) + + node = AcousticsInterfaceNode() + + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() + pass + +if __name__ == "__main__": + main() + \ No newline at end of file diff --git a/acoustics/acoustics_interface/launch/__init__.py b/acoustics/acoustics_interface/launch/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/acoustics/acoustics_interface/launch/acoustics_interface_launch.py b/acoustics/acoustics_interface/launch/acoustics_interface_launch.py new file mode 100644 index 00000000..0388f506 --- /dev/null +++ b/acoustics/acoustics_interface/launch/acoustics_interface_launch.py @@ -0,0 +1,24 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + # Path to the YAML file + yaml_file_path = os.path.join( + get_package_share_directory("blackbox"), + "../../../../", # Go to the workspace + "src/vortex-asv/asv_setup/config/robots/", # Go inside where yamal files are located at + 'freya.yaml' + ) + + return LaunchDescription([ + Node( + package='acoustics_interface', + namespace='acoustics_interface', + executable='acoustics_interface_node.py', + name='acoustics_interface_node', + output='screen', + parameters=[yaml_file_path], + ), + ]) \ No newline at end of file diff --git a/acoustics/acoustics_interface/package.xml b/acoustics/acoustics_interface/package.xml new file mode 100644 index 00000000..fdb44caf --- /dev/null +++ b/acoustics/acoustics_interface/package.xml @@ -0,0 +1,19 @@ + + + + acoustics_interface + 1.0.0 + Communicates and gets data from acoustics PCB + vortex + MIT + + + rclpy + std_msgs + ros2launch + + + + ament_cmake + + diff --git a/asv_setup/README.md b/asv_setup/README.md index b3344e0c..e0664cd3 100644 --- a/asv_setup/README.md +++ b/asv_setup/README.md @@ -8,7 +8,7 @@ The config folder contains physical parameters related to the AUV and the enviro There are currently no physical parameters ### Launch -This package contains a launchfile for each specific ASV. Additionally the pc.launch file is to +This package contains a launchfile for each specific ASV. Additionally the shoreside.launch.py file is to be used on the topside computer that the joystick is connected to, for ROV operations. diff --git a/asv_setup/config/robots/freya.yaml b/asv_setup/config/robots/freya.yaml index 5ed6cd4b..cc960e68 100644 --- a/asv_setup/config/robots/freya.yaml +++ b/asv_setup/config/robots/freya.yaml @@ -32,18 +32,18 @@ yaw: true thrusters: num: 4 - min: -100 - max: 100 + min: -200 + max: 200 configuration_matrix: #NED [0.70711, 0.70711, 0.70711, 0.70711, -0.70711, 0.70711, -0.70711, 0.70711, - 0.27738, 0.27738, -0.27738, -0.27738] + -0.8485, -0.8485, 0.8485, 0.8485] rate_of_change: max: 0 # Maximum rate of change in newton per second for a thruster thruster_to_pin_mapping: [3, 2, 1, 0] # I.e. if thruster_to_pin = [1, 3, 2, 0] then thruster 0 is pin 1 etc.. - thruster_direction: [1,-1,-1, 1] # Disclose during thruster mapping (+/- 1) + thruster_direction: [-1, 1, 1,-1] # Disclose during thruster mapping (+/- 1) thruster_PWM_offset: [0, 0, 0, 0] # Disclose during thruster mapping, Recomended: [0, 0, 0, 0] thruster_PWM_min: [1100, 1100, 1100, 1100] # Minimum PWM value, Recomended: [1100, 1100, 1100, 1100] thruster_PWM_max: [1900, 1900, 1900, 1900] # Maximum PWM value, Recomended: [1900, 1900, 1900, 1900] @@ -52,4 +52,24 @@ max: [150.0, 150.0, 150.0, 150.0, 150.0, 150.0] scaling: [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + blackbox: + data_logging_rate: 5.0 # [logings/second], Recomended: 5.0 logings per second + + internal_status: + psm_read_rate: 2.0 # [readings/second], Recomended 2.0 readings per second OBS! Try keeping under 2.0 as otherwise I2C comunication to Power Sense Module might become bad as psm needs time to sample data + + acoustics: + frequencies_of_interest: # MUST be 20 elements [FREQUENCY [Hz], FREQUENCY_VARIANCE [Hz]...] Recoended: Frequency 1 000 - 50 000 Hz, Variance 1 000 - 10 000 + [10000, 1000, + 50000, 3000, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0] + data_logging_rate: 1.0 # [logings/second], Recomended: 1.0 logings per second + computer: "pc-debug" diff --git a/asv_setup/launch/freya.launch.py b/asv_setup/launch/freya.launch.py index a412647b..f9b3ec16 100644 --- a/asv_setup/launch/freya.launch.py +++ b/asv_setup/launch/freya.launch.py @@ -1,4 +1,4 @@ -import os +from os import path from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.actions import SetEnvironmentVariable, IncludeLaunchDescription @@ -12,18 +12,17 @@ def generate_launch_description(): value='[${severity}] [${time}] [${node}]: ${message}' ) - # Thruster Allocator launch thruster_allocator_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('thruster_allocator'), 'launch/thruster_allocator.launch.py') + path.join(get_package_share_directory('thruster_allocator'),'launch','thruster_allocator.launch.py') ) ) #Thruster Interface launch thruster_interface_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('thruster_interface'), 'launch/thruster_interface.launch.py') + path.join(get_package_share_directory('thruster_interface_asv'),'launch','thruster_interface_asv.launch.py') ) ) diff --git a/asv_setup/launch/pc.launch.py b/asv_setup/launch/shoreside.launch.py similarity index 100% rename from asv_setup/launch/pc.launch.py rename to asv_setup/launch/shoreside.launch.py diff --git a/asv_setup/launch/tf.launch.py b/asv_setup/launch/tf.launch.py index 401c1911..47a7de37 100644 --- a/asv_setup/launch/tf.launch.py +++ b/asv_setup/launch/tf.launch.py @@ -37,9 +37,9 @@ def generate_launch_description(): '--z' , '-0.392425', '--roll' , str(math.pi), '--pitch' , '0', - '--yaw' , '0', + '--yaw' , str(math.pi), '--frame-id' , 'base_link', - '--child-frame-id' , 'os_lidar'], + '--child-frame-id' , 'os_sensor'], ) # base_link to zed2i_camera_center tf. diff --git a/guidance/d_star_lite/CMakeLists.txt b/guidance/d_star_lite/CMakeLists.txt new file mode 100644 index 00000000..7badbfb7 --- /dev/null +++ b/guidance/d_star_lite/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.8) +project(d_star_lite) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(std_msgs REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS + d_star_lite/ros_d_star_lite_node.py + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + ament_add_pytest_test(python_tests tests) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) +endif() + +ament_package() \ No newline at end of file diff --git a/guidance/d_star_lite/README.md b/guidance/d_star_lite/README.md new file mode 100644 index 00000000..cf7a35f6 --- /dev/null +++ b/guidance/d_star_lite/README.md @@ -0,0 +1,24 @@ +# D* lite +Given a map of obstacles and a minimum safe distance to the obstacles, this algorithm will compute the shortest path from the start point to the end point. This is an example of an obstacle map: + +![Obstacle Map](https://drive.google.com/uc?export=download&id=1MohnPBQMoQHHbLaDkbUe43MjBPp5sTiF) + +And when using this map when running the D* lite algorithm with safe distance 4.5 we get this path: + +![Path](https://drive.google.com/uc?export=download&id=1il-i2aJM3pkQacTO8Jg77_2zDVcZF1ty) + +A D* lite object consists of these parameters: + +``` +dsl_object = DStarLite(obstacle_x, obstacle_y, start, goal, safe_dist_to_obstacle, origin, height, width) +``` + +where `obstacle_x` and `obstacle_y` are lists containg the x and y coordinates of the obstacles. `start` and `goal` are the start and goal node for the selected mission. `origin`, `height` and `width` are parameters to create the world boundaries and are used to compute `x_min`, `x_max`, `y_min` and `y_max`. See the figures below for visual representation. + +![World Grid](https://drive.google.com/uc?export=download&id=1RYXcRTjFWMFRhYBMRx5ILmheNvEKahrY) + +![Obstacle](https://drive.google.com/uc?export=download&id=1M43ohD3wpKwmgkjJ44Ut1uOT3f5MlSQI) + +# D* lite ROS2 node +The node is responsible for gathering the waypoints found from the algorithm and send them to the waypoint manager. The node receives the mission parameters from the mission planner. It is both a client and a server. + diff --git a/guidance/d_star_lite/d_star_lite/__init__.py b/guidance/d_star_lite/d_star_lite/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/guidance/d_star_lite/d_star_lite/d_star_lite.py b/guidance/d_star_lite/d_star_lite/d_star_lite.py new file mode 100755 index 00000000..d1d918b1 --- /dev/null +++ b/guidance/d_star_lite/d_star_lite/d_star_lite.py @@ -0,0 +1,403 @@ +import numpy as np +import math +from d_star_lite.d_star_lite_node import DSLNode +from geometry_msgs.msg import Point + +# Link to the original code: +# https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/DStarLite/d_star_lite.py + +# Link to theory: +# http://idm-lab.org/bib/abstracts/papers/aaai02b.pdf + + +class DStarLite: + """ + Implements the D* Lite algorithm for path planning in a grid. + + This class manages the pathfinding grid, obstacles and calculates the shortest path from a start DSLNode to a goal DSLNode. + + Methods: + ------------------------------------------------------------------------------------------- + __init__(ox: list, oy: list, min_dist_to_obstacle: float = 4.5): Initializes a new instance of the DStarLite class. + + create_grid(val: float) -> np.ndarray: Creates a grid initialized with a specific value. + + is_obstacle(DSLNode: DSLNode) -> bool: Check if the DSLNode is considered an obstacle or is too close to an obstacle. + + movement_cost(node1: DSLNode, node2: DSLNode) -> float: Calculates the cost of moving from node1 to node2. + + heuristic_distance(s: DSLNode) -> float: Calculates the heuristic distance from DSLNode s to the goal using the Euclidean distance. + + calculate_key(s: DSLNode) -> tuple: Calculates the priority key for a DSLNode 's' based on the D* Lite algorithm. + + is_valid(DSLNode: DSLNode) -> bool: Determines if a DSLNode is within the grid boundaries. + + pred(u: DSLNode) -> list[DSLNode]: Retrieves the predecessors of a DSLNode 'u'. + + initialize(start: DSLNode, goal: DSLNode): Initializes the grid and the D* Lite algorithm. + + update_vertex(u: DSLNode): Updates the vertex in the priority queue and the rhs value of the DSLNode 'u'. + + detect_and_update_waypoints(current_point: DSLNode, next_point: DSLNode): Updates the waypoints based on the current and next points. + + compare_keys(key_pair1: tuple[float, float], key_pair2: tuple[float, float]) -> bool: Compares the priority keys of two nodes. + + compute_shortest_path(): Computes or recomputes the shortest path from the start to the goal using the D* Lite algorithm. + + compute_current_path() -> list[DSLNode]: Computes the current path from the start to the goal. + + get_WP() -> list[list[int]]: Retrieves the waypoints and adjusts their coordinates to the original coordinate system. + + dsl_main(start: DSLNode, goal: DSLNode) -> tuple[bool, list[int], list[int]]: Main function to run the D* Lite algorithm. + """ + + possible_motions = [ # Represents the possible motions in the grid and corresponding costs + DSLNode(1, 0, 1), DSLNode(0, 1, 1), DSLNode(-1, 0, 1), DSLNode(0, -1, 1), + DSLNode(1, 1, math.sqrt(2)), DSLNode(1, -1, math.sqrt(2)), + DSLNode(-1, 1, math.sqrt(2)), DSLNode(-1, -1, math.sqrt(2)) + ] + + def __init__(self, obstacles: list[Point], start: Point, goal: Point, min_dist_to_obstacle: float = 4.5, origin: Point = Point(x=0.0,y=0.0), height: int = 25, width: int = 25): + """ + Initializes a new instance of the DStarLite class. + + Args: + obstacles (list): A list of Point objects representing the obstacles. + start (DSLNode): The start node. + goal (DSLNode): The goal node. + min_dist_to_obstacle (float): The minimum distance a DSLNode must be from any obstacle to be considered valid. Defaults to 4.5. + origin (tuple): The origin of the grid. Defaults to (0, 0). + height (int): The height of the grid. Defaults to 25. + width (int): The width of the grid. Defaults to 25. + """ + if len(obstacles) == 0: # If no obstacles are provided + self.obstacles = [] + + else: + self.obstacles = [DSLNode(int(point.x), int(point.y)) for point in obstacles] # The obstacles as nodes + self.obstacles_xy = np.array( # The obstacles as xy coordinates + [[obstacle.x, obstacle.y] for obstacle in self.obstacles] + ) + + self.start = DSLNode(int(start.x), int(start.y)) # The start DSLNode + self.goal = DSLNode(int(goal.x), int(goal.y)) # The goal DSLNode + self.world_grid_origin = (int(origin.x), int(origin.y)) # The origin of the world grid + self.world_grid_height = height/2 # The height of the world grid + self.world_grid_width = width/2 # The width of the world grid + + # Compute the min and max values for the world boundaries + self.x_min = int(origin.x-self.world_grid_width) + self.y_min = int(origin.y-self.world_grid_height) + self.x_max = int(origin.x+self.world_grid_width) + self.y_max = int(origin.y+self.world_grid_height) + + self.priority_queue = [] # Priority queue + self.key_min = 0.0 # The minimum key in priority queue + self.key_old = 0.0 # The old minimum key in priority queue + self.rhs = self.create_grid(float("inf")) # The right hand side values + self.g = self.create_grid(float("inf")) # The g values + self.initialized = False # Whether the grid has been initialized + self.waypoints = [] # The waypoints + self.min_dist_to_obstacle = min_dist_to_obstacle # The minimum distance a DSLNode must be from any obstacle to be considered valid + + def create_grid(self, val: float) -> np.ndarray: + """ + Creates a grid initialized with a specific value. + + Args: + val (float): The value to initialize the grid with. + + Returns: + np.ndarray: A 2D numpy array representing the initialized grid. + """ + return np.full((self.x_max - self.x_min, self.y_max - self.y_min), val) + + def is_obstacle(self, dslnode: DSLNode) -> bool: + """ + Check if the DSLNode is considered an obstacle or is too close to an obstacle. + + Args: + DSLNode (DSLNode): The DSLNode to check. + + Returns: + bool: True if the DSLNode is too close to an obstacle or is an obstacle, False otherwise. + """ + # If there are no obstacles, return False + if len(self.obstacles) == 0: + return False + + # Convert the DSLNode's coordinates to a numpy array for efficient distance computation + node_xy = np.array([dslnode.x, dslnode.y]) + + # Compute the euclidean distances from the DSLNode to all obstacles + distances = np.sqrt(np.sum((self.obstacles_xy - node_xy) ** 2, axis=1)) + + # Check if any distance is less than the minimum distance (default: 4.5) + return np.any(distances < self.min_dist_to_obstacle) + + def movement_cost(self, node1: DSLNode, node2: DSLNode) -> float: + """ + Calculates the cost of moving from node1 to node2. Returns infinity if node2 is an obstacle or if no valid motion is found. + + Args: + node1 (DSLNode): The starting DSLNode. + node2 (DSLNode): The ending DSLNode. + + Returns: + float: The cost of moving from node1 to node2. + """ + if self.is_obstacle(node2): + return math.inf + + movement_vector = DSLNode(node1.x - node2.x, node1.y - node2.y) + for motion in self.possible_motions: + if motion == movement_vector: + return motion.cost + return math.inf + + def heuristic_distance(self, s: DSLNode) -> float: + """ + Calculates the heuristic distance from DSLNode s to the goal using the Euclidean distance. + + Args: + s (DSLNode): The DSLNode to calculate the heuristic distance from. + + Returns: + float: The heuristic distance from DSLNode s to the goal. + """ + return DSLNode.distance_between_nodes(s, self.goal) # Euclidean distance + + def calculate_key(self, s: DSLNode) -> tuple: + """ + Calculates the priority key for a DSLNode 's' based on the D* Lite algorithm. + + The key is a tuple consisting of two parts: + 1. The estimated total cost from the start to the goal through 's', combining the minimum of g(s) and rhs(s), + the heuristic distance to the goal and a constant key_min. + 2. The minimum of g(s) and rhs(s) representing the best known cost to reach 's'. + + Args: + s (DSLNode): The DSLNode to calculate the key for. + + Returns: + tuple: A tuple of two floats representing the priority key for the DSLNode. + """ + return (min(self.g[s.x][s.y], self.rhs[s.x][s.y]) + self.heuristic_distance(s) + self.key_min, + min(self.g[s.x][s.y], self.rhs[s.x][s.y])) + + def is_valid(self, DSLNode: DSLNode) -> bool: + """ + Determines if a DSLNode is within the grid boundaries. + + Args: + DSLNode (DSLNode): The DSLNode to check. + + Returns: + bool: True if the DSLNode is within the grid boundaries, False otherwise. + """ + return self.x_min <= DSLNode.x < self.x_max and self.y_min <= DSLNode.y < self.y_max + + def pred(self, u: DSLNode) -> list[DSLNode]: + """ + Retrieves the predecessors of a DSLNode 'u'. In this case, the predecessors are the valid neighbours of the DSLNode. + + Args: + u (DSLNode): The DSLNode to retrieve predecessors for. + + Returns: + list: A list of predecessors of the DSLNode 'u'. + """ + return [u + motion for motion in self.possible_motions if self.is_valid(u + motion)] + + def initialize(self): + """ + Initializes the grid and the D* Lite algorithm. + This function adjusts the coordinates of the start and goal nodes based on the grid's minimum world coordinates, + sets up the cost and right-hand side (rhs) grids, and initializes the priority queue. This setup is required + before the algorithm begins pathfinding. The initialization will only occur once; subsequent calls to this + function will have no effect. + + Args: + start (DSLNode): The start DSLNode. + goal (DSLNode): The goal DSLNode. + """ + + if not self.initialized: + self.initialized = True + print("Initializing") + self.priority_queue = [] + self.key_min = 0.0 + self.rhs = self.create_grid(math.inf) + self.g = self.create_grid(math.inf) + self.rhs[self.goal.x][self.goal.y] = 0 + self.priority_queue.append((self.goal, self.calculate_key(self.goal))) + + def update_vertex(self, u: DSLNode): + """ + Updates the vertex in the priority queue and the rhs value of the DSLNode 'u'. + + This method adjusts the right-hand side (rhs) value for a DSLNode unless it's the goal. It also ensures that the + DSLNode's priority in the queue reflects its current g and rhs values, reordering the queue as necessary. + + Args: + u (DSLNode): The DSLNode to update. + + """ + if not u == self.goal: + self.rhs[u.x][u.y] = min([self.movement_cost(u, sprime) + self.g[sprime.x][sprime.y] for sprime in self.pred(u)]) + + # Update the priority queue + if any([u == dslnode for dslnode, key in self.priority_queue]): + self.priority_queue = [(dslnode, key) for dslnode, key in self.priority_queue if not u == dslnode] + self.priority_queue.sort(key=lambda x: x[1]) + if self.g[u.x][u.y] != self.rhs[u.x][u.y]: + self.priority_queue.append((u, self.calculate_key(u))) + + # Resort the priority queue + self.priority_queue.sort(key=lambda x: x[1]) + + def detect_and_update_waypoints(self, current_point: DSLNode, next_point: DSLNode): + """ + Updates the waypoints based on the current and next points. + + This function checks the direction between the last waypoint and the current point, and the direction + between the current point and the next point. If there is a change in direction, indicating a turn or + deviation in the path, the current point is added to the list of waypoints. + + Args: + current_point (DSLNode): The current point. + next_point (DSLNode): The next point. + """ + if not self.waypoints: # If the waypoint list is empty + self.waypoints.append(current_point) + else: + # Get the last waypoint + last_wp = self.waypoints[-1] + # Determine directions + last_direction = DSLNode.get_direction(last_wp, current_point) + current_direction = DSLNode.get_direction(current_point, next_point) + + # If there's a change in direction, add the current point to waypoints + if current_direction != last_direction: + #print("Change in direction detected") + self.waypoints.append(current_point) + + def compare_keys(self, key_pair1: tuple[float, float], key_pair2: tuple[float, float]) -> bool: + """ + Compares the priority keys of two nodes. + + Args: + key_pair1 (tuple): The first key pair to compare. + key_pair2 (tuple): The second key pair to compare. + + Returns: + bool: True if `key_pair1` should precede `key_pair2` in sorting order, False otherwise. + """ + return key_pair1[0] < key_pair2[0] or (key_pair1[0] == key_pair2[0] and key_pair1[1] < key_pair2[1]) + + def compute_shortest_path(self): + """ + Computes or recomputes the shortest path from the start to the goal using the D* Lite algorithm. + + This method iteratively updates the priorities and costs of nodes based on the graph's current state, + adjusting the path as necessary until the start DSLNode's key does not precede the smallest key in the + priority queue and the start DSLNode's rhs and g values are equal. + """ + + # Loop until the priority queue is empty, indicating no more nodes to process + while self.priority_queue: + # Sort the priority queue based on the keys to ensure the node with the smallest key is processed first + self.priority_queue.sort(key=lambda x: x[1]) + + # Extract the smallest key and its corresponding node + k_old = self.priority_queue[0][1] + u = self.priority_queue[0][0] + + # Double-check conditions to potentially exit the loop + has_elements = len(self.priority_queue) > 0 + start_key = self.calculate_key(self.start) + + # Determine if the start node's key is outdated or not, affecting loop continuation + start_key_not_updated = has_elements and self.compare_keys(self.priority_queue[0][1], start_key) + + # Check if the rhs value and g value for the start node are equal, indicating optimality reached for the start node + rhs_not_equal_to_g = self.rhs[self.start.x][self.start.y] != self.g[self.start.x][self.start.y] + + # Exit the loop if no updates are required + if not start_key_not_updated and not rhs_not_equal_to_g: + break + + # Remove the processed node from the priority queue + self.priority_queue.pop(0) + + # If the current node's old key is outdated, reinsert it with the updated key + if self.compare_keys(k_old, self.calculate_key(u)): + self.priority_queue.append((u, self.calculate_key(u))) + + # If the g value is greater than the rhs value, update it to achieve consistency + elif self.g[u.x][u.y] > self.rhs[u.x][u.y]: + self.g[u.x][u.y] = self.rhs[u.x][u.y] + + # Update all predecessors of the current node as their cost might have changed + for s in self.pred(u): + self.update_vertex(s) + + # If no optimal path is known (g value is infinity), set the current node's g value to + # infinity and update its predecessors + else: + self.g[u.x][u.y] = float('inf') + + # Update the current node and its predecessors + for s in self.pred(u) + [u]: + self.update_vertex(s) + + def compute_current_path(self) -> list[DSLNode]: + """ + Computes the current path from the start to the goal. + + Returns: + list: A list of DSLNode objects representing the current path from the start to the goal. + """ + path = list() + current_point = DSLNode(self.start.x, self.start.y) + last_point = None + + while not current_point == self.goal: + if last_point is not None: + self.detect_and_update_waypoints(last_point, current_point) + path.append(current_point) + last_point = current_point + current_point = min(self.pred(current_point), key = lambda sprime: self.movement_cost(current_point, sprime) + self.g[sprime.x][sprime.y]) + path.append(self.goal) + self.waypoints.append(self.goal) + + return path + + + def get_WP(self) -> list[Point]: + """ + Retrieves the waypoints and adjusts their coordinates to the original coordinate system. + + Returns: + list: A list of waypoints with their coordinates adjusted to the original coordinate system. + """ + WP_list = [] + for wp in self.waypoints: + WP_list.append(Point(x=float(wp.x), y=float(wp.y), z=0.0)) + return WP_list + + + def dsl_main(self) -> None: + """ + Main function to run the D* Lite algorithm. + + Args: + start (DSLNode): The start DSLNode. + goal (DSLNode): The goal DSLNode. + """ + self.initialize() + self.compute_shortest_path() + self.compute_current_path() + print("Path found") + + \ No newline at end of file diff --git a/guidance/d_star_lite/d_star_lite/d_star_lite_node.py b/guidance/d_star_lite/d_star_lite/d_star_lite_node.py new file mode 100644 index 00000000..1555992e --- /dev/null +++ b/guidance/d_star_lite/d_star_lite/d_star_lite_node.py @@ -0,0 +1,81 @@ +import math + +class DSLNode: + """ + Represents a DSLNode in the grid. + + Attributes: + x (int): The x-coordinate of the DSLNode. + y (int): The y-coordinate of the DSLNode. + cost (float): The cost of moving to the DSLNode. + + """ + def __init__(self, x: int = 0, y: int = 0, cost: float = 0.0): + """ + Initializes a new instance of the DSLNode class. + + Args: + x (int): The x-coordinate of the DSLNode. Defaults to 0. + y (int): The y-coordinate of the DSLNode. Defaults to 0. + cost (float): The cost of moving to the DSLNode. Defaults to 0.0. + """ + self.x = x + self.y = y + self.cost = cost + + def __add__(self, other: 'DSLNode') -> 'DSLNode': + """ + Adds two DSLNode objects together. + + Args: + other (DSLNode): The DSLNode to add to the current DSLNode. + + Returns: + DSLNode: A new DSLNode object with the combined x and y coordinates and costs. + """ + return DSLNode(self.x + other.x, self.y + other.y, self.cost + other.cost) + + def __eq__(self, other: 'DSLNode') -> bool: + """ + Checks if two DSLNode objects have the same x and y coordinates. + + Args: + other (DSLNode): The DSLNode to compare to the current DSLNode. + + Returns: + bool: True if the nodes have the same x and y coordinates, False otherwise. + """ + return self.x == other.x and self.y == other.y + + @staticmethod + def distance_between_nodes(node1: 'DSLNode', node2: 'DSLNode') -> float: + """ + Computes the Euclidean distance between two DSLNode objects. + + Args: + node1 (DSLNode): The first DSLNode. + node2 (DSLNode): The second DSLNode. + + Returns: + float: The Euclidean distance between the two nodes. + """ + return math.sqrt((node1.x - node2.x)**2 + (node1.y - node2.y)**2) + + @staticmethod + def get_direction(node1: 'DSLNode', node2: 'DSLNode') -> tuple: + """ + Calculates the direction from node1 to node2. + + Args: + node1 (DSLNode): The starting DSLNode. + node2 (DSLNode): The ending DSLNode. + + Returns: + tuple: A tuple of two integers representing the direction from node1 to node2. + """ + dx = node2.x - node1.x + dx = dx/abs(dx) if dx != 0 else 0 + dy = node2.y - node1.y + dy = dy/abs(dy) if dy != 0 else 0 + return dx, dy + \ No newline at end of file diff --git a/guidance/d_star_lite/d_star_lite/ros_d_star_lite_node.py b/guidance/d_star_lite/d_star_lite/ros_d_star_lite_node.py new file mode 100755 index 00000000..ff03c8f3 --- /dev/null +++ b/guidance/d_star_lite/d_star_lite/ros_d_star_lite_node.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from d_star_lite.d_star_lite import DStarLite +from vortex_msgs.srv import MissionParameters, Waypoint + +class DStarLiteNode(Node): + """ + A ROS2 node implementing the D* Lite algorithm. + + The node offers a mission planner service to calculate the optimal waypoints, which are then sent to the waypoint service. + """ + def __init__(self): + """ + Initialize the DStarLiteNode, creating necessary services and clients + for mission planning and waypoint submission. + """ + super().__init__('d_star_lite_node') + self.obstacle_srv = self.create_service(MissionParameters, 'mission_parameters', self.d_star_lite_callback) + self.waypoint_client = self.create_client(Waypoint, 'waypoint') + self.get_logger().info('D Star Lite Node has been started') + + + def d_star_lite_callback(self, request, response): + """ + Callback for the mission planner service. + + Args: + request: start and goal coordinates, the obstacle coordinates and the world boundaries + response: success flag + + Returns: + The modified response object with success status + """ + self.get_logger().info('D Star Lite Service has been called') + obstacles = request.obstacles + start = request.start + goal = request.goal + origin = request.origin + height = request.height + width = request.width + + dsl = DStarLite(obstacles, start, goal, origin=origin, height=height, width=width) + dsl.dsl_main() # Run the main function to generate path + + # Get waypoints + self.waypoints = dsl.get_WP() + + # Send waypoints to waypoint service + self.send_waypoints_request() + + response.success = True + + return response + + def send_waypoints_request(self): + """ + Sends the computed waypoints to the waypoint service. + """ + if not self.waypoint_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('Waypoint service not available, waiting again...') + return None # Return None to indicate the service is not available. + request = Waypoint.Request() + request.waypoint = self.waypoints + future = self.waypoint_client.call_async(request) + future.add_done_callback(self.waypoint_response_callback) # Handle response + + def waypoint_response_callback(self, future): + try: + response = future.result() + if response.success: + self.get_logger().info('Waypoints successfully submitted.') + else: + self.get_logger().error('Waypoint submission failed.') + except Exception as e: + self.get_logger().error('Service call failed %r' % (e,)) + +def main(args=None): + rclpy.init(args=args) + node = DStarLiteNode() + rclpy.spin(node) + + # Cleanup and shutdown + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/guidance/d_star_lite/launch/d_star_lite.launch.py b/guidance/d_star_lite/launch/d_star_lite.launch.py new file mode 100644 index 00000000..95ffb0c5 --- /dev/null +++ b/guidance/d_star_lite/launch/d_star_lite.launch.py @@ -0,0 +1,17 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + d_star_lite_node = Node( + package='d_star_lite', + executable='d_star_lite_node.py', + name='d_star_lite_node', + output='screen' + ) + + return LaunchDescription([ + d_star_lite_node + ]) \ No newline at end of file diff --git a/guidance/d_star_lite/package.xml b/guidance/d_star_lite/package.xml new file mode 100644 index 00000000..4a564bba --- /dev/null +++ b/guidance/d_star_lite/package.xml @@ -0,0 +1,30 @@ + + + + d_star_lite + 0.0.0 + d_star_lite algorithm for path planning + andeshog + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + launch_testing_ament_cmake + launch_testing_ros + + rclpy + std_msgs + vortex_msgs + geometry_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_cmake + + diff --git a/guidance/d_star_lite/resource/D_star_lite b/guidance/d_star_lite/resource/D_star_lite new file mode 100644 index 00000000..e69de29b diff --git a/guidance/d_star_lite/tests/test_d_star_lite.py b/guidance/d_star_lite/tests/test_d_star_lite.py new file mode 100644 index 00000000..5f42d30a --- /dev/null +++ b/guidance/d_star_lite/tests/test_d_star_lite.py @@ -0,0 +1,100 @@ +import unittest +import pytest +from d_star_lite.d_star_lite import DStarLite +from d_star_lite.d_star_lite_node import DSLNode +from geometry_msgs.msg import Point +import numpy as np + +class TestDStarLite(unittest.TestCase): + + def setUp(self): + # Create example DSLNodes + self.DSLNode1 = DSLNode(0, 0, 2.0) + self.DSLNode2 = DSLNode(1, 1, 3.4) + self.DSLNode5 = DSLNode(-1, -1, -5.8) + + # Create example obstacle coordinates + self.obstacles = [Point(x=5.0, y=5.0, z=0.0)] + + # Create start and goal + self.start = Point(x=0.0, y=0.0, z=0.0) + self.goal = Point(x=10.0, y=10.0, z=0.0) + + # Create example dsl object + self.dsl = DStarLite(self.obstacles, self.start, self.goal, min_dist_to_obstacle=3, origin=Point(x=0.0, y=0.0, z=0.0), height=30, width=30) + + def tearDown(self): + pass + + # Test node addition + def test_combine_DSLNodes_with_positive_values(self): + self.assertEqual(self.DSLNode1 + self.DSLNode2, DSLNode(1, 1, 5.4)) + + def test_combine_DSLNodes_with_negative_values(self): + self.assertEqual(self.DSLNode5 + self.DSLNode5, DSLNode(-2, -2, -11.6)) + + def test_combine_DSLNodes_with_positive_and_negative_values(self): + self.assertEqual(self.DSLNode1 + self.DSLNode5, DSLNode(-1, -1, -3.8)) + + # Test node comparison + def test_compare_coordinates_is_false_for_two_different_nodes(self): + + self.assertEqual(self.DSLNode1 == self.DSLNode2, False) + + def test_compare_coordinates_is_true_for_two_same_nodes(self): + self.assertEqual(self.DSLNode1 == self.DSLNode1, True) + + # Test the distance function + def test_distance_between_two_nodes_yields_correct_euclidean_distance(self): + + result = DSLNode.distance_between_nodes(self.DSLNode1, self.DSLNode2) + self.assertEqual(result, np.sqrt(2)) + + result = DSLNode.distance_between_nodes(self.DSLNode2, self.DSLNode5) + self.assertEqual(result, np.sqrt(8)) + + # Test the is_obstacle function + def test_is_obstacle_is_true_when_node_is_obstacle(self): + + self.assertEqual(self.dsl.is_obstacle(DSLNode(5, 5)), True) + + def test_is_obstacle_is_true_when_node_is_in_safe_distance_from_obstacle(self): + self.assertEqual(self.dsl.is_obstacle(DSLNode(4, 4)), True) + + def test_is_obstacle_is_false_when_node_is_not_obstacle(self): + self.assertEqual(self.dsl.is_obstacle(DSLNode(10, 0)), False) + + # Test the movement_cost function + def test_movement_cost_when_moving_straight(self): + + self.assertEqual(self.dsl.movement_cost(DSLNode(10, 0), DSLNode(11, 0)), 1.0) + + def test_movement_cost_when_moving_diagonally(self): + self.assertEqual(self.dsl.movement_cost(DSLNode(10, 10), DSLNode(11, 11)), np.sqrt(2)) + + def test_movement_cost_when_moving_to_obstacle(self): + self.assertEqual(self.dsl.movement_cost(DSLNode(3, 3), DSLNode(4, 4)), np.inf) + + # Test the heuristic_distance function (distance to target node) + def test_heuristic_distance(self): + self.assertEqual(self.dsl.heuristic_distance(DSLNode(0, 0)), 2*np.sqrt(50)) + self.assertEqual(self.dsl.heuristic_distance(DSLNode(5, 5)), np.sqrt(50)) + self.assertEqual(self.dsl.heuristic_distance(DSLNode(10, 10)), 0) + # Test the get_direction function + def test_get_direction_when_moving_in_positive_x_direction(self): + self.assertEqual(DSLNode.get_direction(DSLNode(0, 0), DSLNode(1, 0)), (1, 0)) + + def test_get_direction_when_moving_positive_diagonal(self): + self.assertEqual(DSLNode.get_direction(DSLNode(0, 0), DSLNode(2, 2)), (1, 1)) + + def test_get_direction_when_moving_in_negative_x_direction(self): + self.assertEqual(DSLNode.get_direction(DSLNode(0, 0), DSLNode(-1, 0)), (-1, 0)) + + def test_get_direction_when_moving_in_negative_y_direction(self): + self.assertEqual(DSLNode.get_direction(DSLNode(0, 0), DSLNode(0, -1)), (0, -1)) + + def test_get_direction_when_moving_in_negative_diagonal(self): + self.assertEqual(DSLNode.get_direction(DSLNode(0, 0), DSLNode(-2, -2)), (-1, -1)) + +if __name__ == '__main__': + unittest.main() \ No newline at end of file diff --git a/mission/blackbox/README b/mission/blackbox/README new file mode 100644 index 00000000..23183a8d --- /dev/null +++ b/mission/blackbox/README @@ -0,0 +1,25 @@ +## Balackbox + +Logs data of all the important topics such as: +/asv/power_sense_module/current +/asv/power_sense_module/voltage +/asv/temperature/ESC1 +/asv/temperature/ESC2 +/asv/temperature/ESC3 +/asv/temperature/ESC4 +/asv/temperature/ambient1 +/asv/temperature/ambient2 +internal/status/bms0 +internal/status/bms1 +/thrust/thruster_forces +/pwm + + +## Service file bootup + +To start the blackbox loging automaticaly every time on bootup just run this command: +``` +./vortex-asv/add_service_files_to_bootup_sequence.sh +``` + +Enjoy :) \ No newline at end of file diff --git a/mission/blackbox/blackbox/__init__.py b/mission/blackbox/blackbox/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/mission/blackbox/blackbox/blackbox_log_data.py b/mission/blackbox/blackbox/blackbox_log_data.py new file mode 100644 index 00000000..b4f432a5 --- /dev/null +++ b/mission/blackbox/blackbox/blackbox_log_data.py @@ -0,0 +1,235 @@ +# Python Libraries +import os +import re +import time +import csv +from datetime import datetime, timedelta + + + +class BlackBoxLogData: + def __init__(self, + ROS2_PACKAGE_DIRECTORY = "" + ): + # Global variables for .csv file manipulation ---------- + # Get the path for the directory where we will store our data + self.blackbox_data_directory = ROS2_PACKAGE_DIRECTORY + "blackbox_data/" + + timestamp = time.strftime('%Y-%m-%d_%H:%M:%S') + data_file_name = 'blackbox_data_' + timestamp + '.csv' + self.data_file_location = self.blackbox_data_directory + data_file_name + + self.csv_headers = [ + "Time", + + "Power Sense Module Current", + "Power Sense Module Voltage", + + "Temperature ESC1", + "Temperature ESC2", + "Temperature ESC3", + "Temperature ESC4", + + "Temperature Ambiant1", + "Temperature Ambiant2", + + "BMS0 Voltage", + "BMS0 Current", + "BMS0 Battery Percentage", + "BMS0 Cell Temperature 1", + "BMS0 Cell Temperature 2", + "BMS0 Cell Temperature 3", + + "BMS1 Voltage", + "BMS1 Current", + "BMS1 Battery Percentage", + "BMS1 Cell Temperature 1", + "BMS1 Cell Temperature 2", + "BMS1 Cell Temperature 3", + + "Thruster Forces 1", + "Thruster Forces 2", + "Thruster Forces 3", + "Thruster Forces 4", + + "PWM 1", + "PWM 2", + "PWM 3", + "PWM 4", + + ] + + # Manage csv files for blackbox data ---------- + # If there are stale old .csv files => Delete oldes ones + # If .csv files take up to much space => Delte oldest ones + self.manage_csv_files() + + # Make new .csv file for loging blackbox data ---------- + with open(self.data_file_location, mode="w", newline="") as csv_file: + writer = csv.writer(csv_file) + writer.writerow(self.csv_headers) + + # Methods for inside use of the class ---------- + def manage_csv_files( + self, + max_file_age_in_days=1, + max_size_kb=3_000_000, + ): #adjust the max size before you start deleting old files (1 000 000 kb = 1 000 mb = 1 gb) + current_time = datetime.now() + older_than_time = current_time - timedelta(days=max_file_age_in_days) + + # Compile a regular expression pattern for matching the expected filename format + pattern = re.compile( + r'blackbox_data_(\d{4}-\d{2}-\d{2}_\d{2}:\d{2}:\d{2})\.csv') + + # List all .csv files in the blackbox data directory + csv_files = [ + f for f in os.listdir(self.blackbox_data_directory) + if f.endswith('.csv') and f.startswith('blackbox_data_') + ] + + for csv_file in csv_files: + match = pattern.match(csv_file) + # Skip files that do not match the expected format + if match is None: + print(f'Invalid filename format, skipping file: {csv_file}') + continue + + try: + file_time = datetime.strptime(match.group(1), + '%Y-%m-%d_%H:%M:%S') + except ValueError as e: + print( + f'Error parsing file timestamp, skipping file: {csv_file}. Error: {e}' + ) + continue + + if file_time < older_than_time: + file_path = os.path.join(self.blackbox_data_directory, + csv_file) + os.remove(file_path) + print(f'Deleted old csv file: {file_path}') + + # Calculate the total size of remaining .csv files + total_size_kb = sum( + os.path.getsize(os.path.join(self.blackbox_data_directory, f)) + for f in os.listdir(self.blackbox_data_directory) + if f.endswith('.csv')) / 1024 + + csv_files = [ + f for f in os.listdir(self.blackbox_data_directory) + if f.endswith('.csv') and f.startswith('blackbox_data_') + and pattern.match(f) + ] + # Delete oldest files if total size exceeds max_size_kb + while total_size_kb > max_size_kb: + # Sort .csv files by timestamp (oldest first) + csv_files_sorted = sorted( + csv_files, + key=lambda x: datetime.strptime( + pattern.match(x).group(1), '%Y-%m-%d_%H:%M:%S')) + + if not csv_files_sorted: + print('No .csv files to delete.') + break + + oldest_file = csv_files_sorted[0] + oldest_file_path = os.path.join(self.blackbox_data_directory, + oldest_file) + os.remove(oldest_file_path) + print(f'Deleted the oldest csv file: {oldest_file_path}') + + # Recalculate the total size of remaining .csv files + total_size_kb = sum( + os.path.getsize(os.path.join(self.blackbox_data_directory, f)) + for f in os.listdir(self.blackbox_data_directory) + if f.endswith('.csv')) / 1024 + csv_files.remove( + oldest_file + ) # Ensure the deleted file is removed from the list + print( + f'Now the total size of .csv files is: {total_size_kb:.2f} KB') + + # Methods for external uses ---------- + def log_data_to_csv_file(self, + psm_current = 0.0, + psm_voltage = 0.0, + + temperature_ESC1 = 0.0, + temperature_ESC2 = 0.0, + temperature_ESC3 = 0.0, + temperature_ESC4 = 0.0, + + temperature_ambient1 = 0.0, + temperature_ambient2 = 0.0, + + bms0_voltage = 0.0, + bms0_current = 0.0, + bms0_percentage = 0.0, + bms0_cell_temperature_1 = 0.0, + bms0_cell_temperature_2 = 0.0, + bms0_cell_temperature_3 = 0.0, + + bms1_voltage = 0.0, + bms1_current = 0.0, + bms1_percentage = 0.0, + bms1_cell_temperature_1 = 0.0, + bms1_cell_temperature_2 = 0.0, + bms1_cell_temperature_3 = 0.0, + + thruster_forces_1 = 0.0, + thruster_forces_2 = 0.0, + thruster_forces_3 = 0.0, + thruster_forces_4 = 0.0, + + pwm_1 = 0, + pwm_2 = 0, + pwm_3 = 0, + pwm_4 = 0, + + + ): + # Get current time in hours, minutes, seconds and miliseconds + current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3] + + # Write to .csv file + with open(self.data_file_location, mode="a", newline="") as csv_file: + writer = csv.writer(csv_file) + writer.writerow([ + current_time, + + psm_current, + psm_voltage, + + temperature_ESC1, + temperature_ESC2, + temperature_ESC3, + temperature_ESC4, + + temperature_ambient1, + temperature_ambient2, + + bms0_voltage, + bms0_current, + bms0_percentage, + bms0_cell_temperature_1, + bms0_cell_temperature_2, + bms0_cell_temperature_3, + + bms1_voltage, + bms1_current, + bms1_percentage, + bms1_cell_temperature_1, + bms1_cell_temperature_2, + bms1_cell_temperature_3, + + thruster_forces_1, + thruster_forces_2, + thruster_forces_3, + thruster_forces_4, + + pwm_1, + pwm_2, + pwm_3, + pwm_4, + ]) diff --git a/mission/blackbox/blackbox/blackbox_node.py b/mission/blackbox/blackbox/blackbox_node.py new file mode 100644 index 00000000..51babca5 --- /dev/null +++ b/mission/blackbox/blackbox/blackbox_node.py @@ -0,0 +1,247 @@ +# ROS2 Libraries +import rclpy +import array +from rclpy.node import Node +from ament_index_python.packages import get_package_share_directory +from sensor_msgs.msg import BatteryState + +# ROS2 Topic Libraries +from std_msgs.msg import Float32MultiArray, Float32, Int16MultiArray + +# Custom Libraries +from blackbox.blackbox_log_data import BlackBoxLogData + + + +class BlackBoxNode(Node): + def __init__(self): + # Start the ROS2 Node ---------- + super().__init__("blackbox_node") + + # Initialize sunscribers ---------- + self.psm_current_subscriber = self.create_subscription( + Float32, + "/asv/power_sense_module/current", + self.psm_current_callback, + 1) + self.psm_current_data = 0.0 + + self.psm_voltage_subscriber = self.create_subscription( + Float32, + "/asv/power_sense_module/voltage", + self.psm_voltage_callback, + 1) + self.psm_voltage_data = 0.0 + + self.temperature_ESC1_subscriber = self.create_subscription( + Float32, + "/asv/temperature/ESC1", + self.temperature_ESC1_callback, + 1) + self.temperature_ESC1_data = 0.0 + + self.temperature_ESC2_subscriber = self.create_subscription( + Float32, + "/asv/temperature/ESC2", + self.temperature_ESC2_callback, + 1) + self.temperature_ESC2_data = 0.0 + + self.temperature_ESC3_subscriber = self.create_subscription( + Float32, + "/asv/temperature/ESC3", + self.temperature_ESC3_callback, + 1) + self.temperature_ESC3_data = 0.0 + + self.temperature_ESC4_subscriber = self.create_subscription( + Float32, + "/asv/temperature/ESC4", + self.temperature_ESC4_callback, + 1) + self.temperature_ESC4_data = 0.0 + + self.temperature_ambient1_subscriber = self.create_subscription( + Float32, + "/asv/temperature/ambient1", + self.temperature_ambient1_callback, + 1) + self.temperature_ambient1_data = 0.0 + + self.temperature_ambient2_subscriber = self.create_subscription( + Float32, + "/asv/temperature/ambient2", + self.temperature_ambient2_callback, + 1) + self.temperature_ambient2_data = 0.0 + + self.bms0_subscriber = self.create_subscription( + BatteryState, + "/internal/status/bms0", + self.bms0_callback, + 1) + self.bms0_voltage_data = 0.0 + self.bms0_current_data = 0.0 + self.bms0_percentage_data = 0.0 + self.bms0_cell_temperature_data = array.array('f', [0.0, 0.0, 0.0]) + + self.bms1_subscriber = self.create_subscription( + BatteryState, + "/internal/status/bms1", + self.bms1_callback, + 1) + self.bms1_voltage_data = 0.0 + self.bms1_current_data = 0.0 + self.bms1_percentage_data = 0.0 + self.bms1_cell_temperature_data = array.array('f', [0.0, 0.0, 0.0]) + + self.thruster_forces = self.create_subscription( + Float32MultiArray, + "/thrust/thruster_forces", + self.thruster_forces_callback, + 1) + self.thruster_forces_data = array.array('f', [0.0, 0.0, 0.0, 0.0]) + + self.pwm = self.create_subscription( + Int16MultiArray, + "/pwm", + self.pwm_callback, + 1) + self.pwm_data = array.array('i', [0, 0, 0, 0]) + + + + # Initialize logger ---------- + # Get package directory location + ros2_package_directory_location = get_package_share_directory("blackbox") + ros2_package_directory_location = ros2_package_directory_location + "/../../../../" # go back to workspace + ros2_package_directory_location = ros2_package_directory_location + "src/vortex-asv/mission/blackbox/" # Navigate to this package + + # Make blackbox loging file + self.blackbox_log_data = BlackBoxLogData( + ROS2_PACKAGE_DIRECTORY = ros2_package_directory_location + ) + + # Logs all the newest data 10 times per second + self.declare_parameter("blackbox.data_logging_rate", 1.0) # Providing a default value 1.0 => 1 samplings per second, verry slow + DATA_LOGING_RATE = self.get_parameter("blackbox.data_logging_rate").get_parameter_value().double_value + timer_period = 1.0/DATA_LOGING_RATE + self.logger_timer = self.create_timer(timer_period, self.logger) + + # Debuging ---------- + self.get_logger().info( + "Started logging data for topics: \n" + "/asv/power_sense_module/current [Float32] \n" + "/asv/power_sense_module/voltage [Float32] \n" + "/asv/temperature/ESC1 [Float32] \n" + "/asv/temperature/ESC2 [Float32] \n" + "/asv/temperature/ESC3 [Float32] \n" + "/asv/temperature/ESC4 [Float32] \n" + "/asv/temperature/ambient1 [Float32] \n" + "/asv/temperature/ambient2 [Float32] \n" + "/internal/status/bms0 [Float32] \n" + "/internal/status/bms1 [Float32] \n" + "/thrust/thruster_forces [Float32] \n" + "/pwm [Float32] \n" + ) + + # Callback Methods ---------- + def psm_current_callback(self, msg): + self.psm_current_data = msg.data + + def psm_voltage_callback(self, msg): + self.psm_voltage_data = msg.data + + def temperature_ESC1_callback(self,msg): + self.temperature_ESC1_data = msg.data + + def temperature_ESC2_callback(self,msg): + self.temperature_ESC2_data = msg.data + + def temperature_ESC3_callback(self,msg): + self.temperature_ESC3_data = msg.data + + def temperature_ESC4_callback(self,msg): + self.temperature_ESC4_data = msg.data + + def temperature_ambient1_callback(self,msg): + self.temperature_ambient1_data = msg.data + + def temperature_ambient2_callback(self,msg): + self.temperature_ambient2_data = msg.data + + def bms0_callback(self,msg): + self.bms0_voltage_data = msg.voltage + self.bms0_current_data = msg.current + self.bms0_percentage_data = msg.percentage + self.bms0_cell_temperature_data = msg.cell_temperature + + def bms1_callback(self,msg): + self.bms1_voltage_data = msg.voltage + self.bms1_current_data = msg.current + self.bms1_percentage_data = msg.percentage + self.bms1_cell_temperature_data = msg.cell_temperature + + def thruster_forces_callback(self,msg): + self.thruster_forces_data = msg.data + + def pwm_callback(self,msg): + self.pwm_data = msg.data + + + + def logger(self): + self.blackbox_log_data.log_data_to_csv_file( + psm_current=self.psm_current_data, + psm_voltage=self.psm_voltage_data, + + temperature_ESC1 =self.temperature_ESC1_data, + temperature_ESC2 =self.temperature_ESC2_data, + temperature_ESC3 =self.temperature_ESC3_data, + temperature_ESC4 =self.temperature_ESC4_data, + + temperature_ambient1 =self.temperature_ambient1_data, + temperature_ambient2 =self.temperature_ambient2_data, + + bms0_voltage =self.bms0_voltage_data, + bms0_current =self.bms0_current_data, + bms0_percentage =self.bms0_percentage_data, + bms0_cell_temperature_1 =self.bms0_cell_temperature_data[0], + bms0_cell_temperature_2 =self.bms0_cell_temperature_data[1], + bms0_cell_temperature_3 =self.bms0_cell_temperature_data[2], + + bms1_voltage =self.bms1_voltage_data, + bms1_current =self.bms1_current_data, + bms1_percentage =self.bms1_percentage_data, + bms1_cell_temperature_1 =self.bms1_cell_temperature_data[0], + bms1_cell_temperature_2 =self.bms1_cell_temperature_data[1], + bms1_cell_temperature_3 =self.bms1_cell_temperature_data[2], + + thruster_forces_1 =self.thruster_forces_data[0], + thruster_forces_2 =self.thruster_forces_data[1], + thruster_forces_3 =self.thruster_forces_data[2], + thruster_forces_4 =self.thruster_forces_data[3], + + pwm_1 =self.pwm_data[0], + pwm_2 =self.pwm_data[1], + pwm_3 =self.pwm_data[2], + pwm_4 =self.pwm_data[3], + + ) + + + +def main(args=None): + # Initialize ROS2 + rclpy.init(args=args) + + # Start ROS2 node + blackbox_node = BlackBoxNode() + rclpy.spin(blackbox_node) + + # Destroy the node once ROS2 ends + blackbox_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/mission/blackbox/blackbox_data/.gitignore b/mission/blackbox/blackbox_data/.gitignore new file mode 100644 index 00000000..f88f9871 --- /dev/null +++ b/mission/blackbox/blackbox_data/.gitignore @@ -0,0 +1 @@ +blackbox_data_* \ No newline at end of file diff --git a/mission/blackbox/launch/blackbox.launch.py b/mission/blackbox/launch/blackbox.launch.py new file mode 100644 index 00000000..a7b54bce --- /dev/null +++ b/mission/blackbox/launch/blackbox.launch.py @@ -0,0 +1,24 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + # Path to the YAML file + yaml_file_path = os.path.join( + get_package_share_directory("blackbox"), + "../../../../", # Go to the workspace + "src/vortex-asv/asv_setup/config/robots/", # Go inside where yamal files are located at + 'freya.yaml' + ) + + return LaunchDescription([ + Node( + package='blackbox', + namespace='blackbox', + executable='blackbox_node', + name='blackbox_node', + output='screen', + parameters=[yaml_file_path], + ), + ]) \ No newline at end of file diff --git a/mission/blackbox/package.xml b/mission/blackbox/package.xml new file mode 100644 index 00000000..45b4b1f2 --- /dev/null +++ b/mission/blackbox/package.xml @@ -0,0 +1,24 @@ + + + + blackbox + 1.0.0 + Logs all ROS2 data and other internal statuses to a data file for use in analizing data latern + vortex + MIT + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + rclpy + sensor_msgs + ros2launch + + blackbox_log_data + + + ament_python + + diff --git a/mission/blackbox/resource/blackbox b/mission/blackbox/resource/blackbox new file mode 100644 index 00000000..e69de29b diff --git a/mission/blackbox/setup.cfg b/mission/blackbox/setup.cfg new file mode 100644 index 00000000..5008ed0d --- /dev/null +++ b/mission/blackbox/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/blackbox +[install] +install_scripts=$base/lib/blackbox diff --git a/mission/blackbox/setup.py b/mission/blackbox/setup.py new file mode 100644 index 00000000..65fe2c06 --- /dev/null +++ b/mission/blackbox/setup.py @@ -0,0 +1,33 @@ +import os +from glob import glob +from setuptools import find_packages, setup +from ament_index_python.packages import get_package_share_directory + +package_name = 'blackbox' +yaml_file_dir = "asv_setup/" + +setup( + name=package_name, + version='1.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), + (os.path.join('share', yaml_file_dir, 'config/robots'), glob('robots/*.yaml')) + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='vortex', + maintainer_email='rose.j.kapps@gmail.com', + description='Logs all ROS2 data and other internal statuses to a data file for use in analizing data later', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'blackbox_node = blackbox.blackbox_node:main', + ], + }, +) diff --git a/mission/blackbox/startup_scripts/blackbox.service b/mission/blackbox/startup_scripts/blackbox.service new file mode 100644 index 00000000..4733ea95 --- /dev/null +++ b/mission/blackbox/startup_scripts/blackbox.service @@ -0,0 +1,17 @@ +[Unit] +Description=Launch internal status node +After=network.target + +[Service] +# Use the wrapper script for ExecStart +# DONT CHANGE THIS LINE!!!! +# Use vortex-asv/scripts/add_service_files_to_bootup_sequence.sh to automaticaly set everything up +ExecStart=/bin/bash 'blackbox.sh' +WorkingDirectory=/home/vortex/ +StandardOutput=journal+console +User=vortex +Restart=no +RestartSec=2 + +[Install] +WantedBy=multi-user.target \ No newline at end of file diff --git a/mission/blackbox/startup_scripts/blackbox.sh b/mission/blackbox/startup_scripts/blackbox.sh new file mode 100644 index 00000000..d70454eb --- /dev/null +++ b/mission/blackbox/startup_scripts/blackbox.sh @@ -0,0 +1,7 @@ +# Determine the directory where the script is located +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" + +cd $SCRIPT_DIR +cd ../../../../.. # Go back to workspace +source install/setup.bash +ros2 launch blackbox blackbox.launch.py diff --git a/mission/internal_status/README.md b/mission/internal_status/README.md index 491c159d..3e8660b0 100644 --- a/mission/internal_status/README.md +++ b/mission/internal_status/README.md @@ -17,6 +17,15 @@ This ros node just publishes the current and voltage data we get from the lib to * /asv/power_sense_module/current for the current data [A] * /asv/power_sense_module/voltage for the voltage data [V] +## Service file bootup + +To start the internal_status publishing automaticaly every time on bootup just run this command: +``` +./vortex-asv/add_service_files_to_bootup_sequence.sh +``` + +Enjoy :) + diff --git a/mission/internal_status/internal_status/__init__.py b/mission/internal_status/internal_status/__init__.py old mode 100644 new mode 100755 diff --git a/mission/internal_status/internal_status/power_sense_module_lib.py b/mission/internal_status/internal_status/power_sense_module_lib.py old mode 100644 new mode 100755 index e128a02f..2036c1a2 --- a/mission/internal_status/internal_status/power_sense_module_lib.py +++ b/mission/internal_status/internal_status/power_sense_module_lib.py @@ -1,3 +1,4 @@ +import time import smbus from MCP342x import MCP342x @@ -11,6 +12,7 @@ def __init__(self): # init of I2C bus communication self.bus = smbus.SMBus(1) + time.sleep(1) # A short pause because sometimes I2C is slow to conect self.channel_voltage = MCP342x(self.bus, self.i2c_adress, channel=1, @@ -18,7 +20,7 @@ def __init__(self): self.channel_current = MCP342x(self.bus, self.i2c_adress, channel=0, - resolution=18) # current + resolution=18) # current # Convertion ratios taken from PSM datasheet at: https://bluerobotics.com/store/comm-control-power/control/psm-asm-r2-rp/ @@ -34,8 +36,8 @@ def get_voltage(self): self.psm_to_battery_voltage) return system_voltage - except IOError: - return + except IOError as e: + return e def get_current(self): try: @@ -45,8 +47,9 @@ def get_current(self): return system_current - except IOError: - return - + except IOError as e: + return e + def shutdown(self): self.bus.close() + diff --git a/mission/internal_status/internal_status/power_sense_module_publisher.py b/mission/internal_status/internal_status/power_sense_module_publisher.py old mode 100644 new mode 100755 index fa5303dc..79665581 --- a/mission/internal_status/internal_status/power_sense_module_publisher.py +++ b/mission/internal_status/internal_status/power_sense_module_publisher.py @@ -8,11 +8,17 @@ class MinimalPublisher(Node): def __init__(self): + # Node setup ---------- super().__init__('PSM_publisher') self.PSM = internal_status.power_sense_module_lib.PowerSenseModule() - self.publisher_current = self.create_publisher(Float32, '/asv/power_sense_module/current', 1) - self.publisher_voltage = self.create_publisher(Float32, '/asv/power_sense_module/voltage', 1) - timer_period = 0.5 + + self.publisher_current = self.create_publisher(Float32, '/asv/power_sense_module/current', 5) + self.publisher_voltage = self.create_publisher(Float32, '/asv/power_sense_module/voltage', 5) + + # Data gathering cycle ---------- + self.declare_parameter("internal_status.psm_read_rate", 1.0) # Providing a default value 1.0 => 1 second delay per data gathering + psm_read_rate = self.get_parameter("internal_status.psm_read_rate").get_parameter_value().double_value + timer_period = 1.0/psm_read_rate self.timer = self.create_timer(timer_period, self.timer_callback) @@ -28,6 +34,7 @@ def timer_callback(self): # self.get_logger().info('Publishing PSM voltage: "%s"' % voltage.data) # Comented out because anoying on the info screen on ROS2 + def main(args=None): rclpy.init(args=args) diff --git a/mission/internal_status/launch/internal_status_launch.py b/mission/internal_status/launch/internal_status_launch.py index 18c26015..f29fa153 100644 --- a/mission/internal_status/launch/internal_status_launch.py +++ b/mission/internal_status/launch/internal_status_launch.py @@ -1,10 +1,22 @@ from launch import LaunchDescription from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os def generate_launch_description(): + # Path to the YAML file + yaml_file_path = os.path.join( + get_package_share_directory("blackbox"), + "../../../../", # Go to the workspace + "src/vortex-asv/asv_setup/config/robots/", # Go inside where yamal files are located at + 'freya.yaml' + ) + return LaunchDescription([ Node( package = 'internal_status', - executable = 'power_sense_module_publisher' + executable = 'power_sense_module_publisher', + output='screen', + parameters=[yaml_file_path], ) ]) \ No newline at end of file diff --git a/mission/internal_status/setup.py b/mission/internal_status/setup.py index 8f4ee25f..71179627 100644 --- a/mission/internal_status/setup.py +++ b/mission/internal_status/setup.py @@ -3,6 +3,7 @@ from setuptools import find_packages, setup package_name = 'internal_status' +yaml_file_dir = "asv_setup/" setup( name=package_name, @@ -12,7 +13,8 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))) + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), + (os.path.join('share', yaml_file_dir, 'config/robots'), glob('robots/*.yaml')), ], install_requires=['setuptools'], zip_safe=True, diff --git a/mission/internal_status/startup_scripts/internal_status.service b/mission/internal_status/startup_scripts/internal_status.service new file mode 100644 index 00000000..897d3a2c --- /dev/null +++ b/mission/internal_status/startup_scripts/internal_status.service @@ -0,0 +1,17 @@ +[Unit] +Description=Launch internal status node +After=network.target + +[Service] +# Use the wrapper script for ExecStart +# DONT CHANGE THIS LINE!!!! +# Use vortex-asv/scripts/add_service_files_to_bootup_sequence.sh to automaticaly set everything up +ExecStart=/bin/bash 'internal_status.sh' +WorkingDirectory=/home/vortex/ +StandardOutput=journal+console +User=vortex +Restart=no +RestartSec=2 + +[Install] +WantedBy=multi-user.target \ No newline at end of file diff --git a/mission/internal_status/startup_scripts/internal_status.sh b/mission/internal_status/startup_scripts/internal_status.sh new file mode 100644 index 00000000..bebebfaa --- /dev/null +++ b/mission/internal_status/startup_scripts/internal_status.sh @@ -0,0 +1,8 @@ +# Determine the directory where the script is located +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" + +cd $SCRIPT_DIR +cd ../../../../.. # Go back to workspace +source install/setup.bash +ros2 launch internal_status internal_status_launch.py + diff --git a/mission/joystick_interface/config/param_joystick_interface.yaml b/mission/joystick_interface/config/param_joystick_interface.yaml index 45d9bbd6..965e3dd8 100755 --- a/mission/joystick_interface/config/param_joystick_interface.yaml +++ b/mission/joystick_interface/config/param_joystick_interface.yaml @@ -1,7 +1,7 @@ joystick_interface: ros__parameters: - surge_scale_factor: 50.0 - sway_scale_factor: 50.0 - yaw_scale_factor: 50.0 + surge_scale_factor: 100.0 + sway_scale_factor: 100.0 + yaw_scale_factor: 60.0 diff --git a/mission/joystick_interface/launch/joystick_interface.launch.py b/mission/joystick_interface/launch/joystick_interface.launch.py old mode 100644 new mode 100755 diff --git a/mission/joystick_interface/package.xml b/mission/joystick_interface/package.xml index 08e812e9..63b031fe 100755 --- a/mission/joystick_interface/package.xml +++ b/mission/joystick_interface/package.xml @@ -9,8 +9,6 @@ MIT - ament_cmake_python - rclpy geometry_msgs sensor_msgs diff --git a/mission/mission_planner/CMakeLists.txt b/mission/mission_planner/CMakeLists.txt new file mode 100644 index 00000000..827ef381 --- /dev/null +++ b/mission/mission_planner/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.8) +project(mission_planner) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(vortex_msgs REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS + mission_planner/mission_planner.py + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/mission/mission_planner/launch/mission_planner.launch.py b/mission/mission_planner/launch/mission_planner.launch.py new file mode 100644 index 00000000..c80e0391 --- /dev/null +++ b/mission/mission_planner/launch/mission_planner.launch.py @@ -0,0 +1,17 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + mission_planner_node = Node( + package='mission_planner', + executable='mission_planner.py', + name='mission_planner', + output='screen' + ) + + return LaunchDescription([ + mission_planner_node + ]) \ No newline at end of file diff --git a/mission/mission_planner/mission_planner/__init__.py b/mission/mission_planner/mission_planner/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/mission/mission_planner/mission_planner/mission_planner.py b/mission/mission_planner/mission_planner/mission_planner.py new file mode 100755 index 00000000..5b7ffc2a --- /dev/null +++ b/mission/mission_planner/mission_planner/mission_planner.py @@ -0,0 +1,77 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from vortex_msgs.srv import MissionParameters +from geometry_msgs.msg import Point + +class MissionPlannerClient(Node): + """ + A ROS2 client node for interacting with the MissionPlanner service. + """ + def __init__(self): + """ + Initializes the client node, creates a client for the MissionPlanner service, + and waits for the service to become available. + """ + super().__init__('mission_planner_client') + self.client = self.create_client(MissionParameters, 'mission_parameters') + while not self.client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('Service not available, waiting again...') + self.req = MissionParameters.Request() + + def send_request(self, obstacles: list[Point], start: Point, goal: Point, origin: Point, height: int, width: int): + """ + Sends an asynchronous request to the MissionPlanner service with obstacle locations, + start, and goal positions. + + Args: + obstacles (list[Point]): The list of obstacle points. + start (Point): The start point. + goal (Point): The goal point. + origin (Point): The origin point of the world. + height (int): The height of the world. + width (int): The width of the world. + """ + self.req.obstacles = obstacles + self.req.start = start + self.req.goal = goal + self.req.origin = origin + self.req.height = height + self.req.width = width + + self.future = self.client.call_async(self.req) + self.get_logger().info('MissionPlanner service has been called') + +def main(args=None): + """ + Main function with example usage of the MissionPlannerClient. + + """ + rclpy.init(args=args) + mission_planner_client = MissionPlannerClient() + # Test data + obstacles = [Point(x=5.0, y=5.0)] + start = Point(x=0.0, y=0.0) + goal = Point(x=10.0, y=10.0) + mission_planner_client.send_request(obstacles, start, goal, Point(x=0.0, y=0.0), 30, 30) + + while rclpy.ok(): + rclpy.spin_once(mission_planner_client) + if mission_planner_client.future.done(): + try: + response = mission_planner_client.future.result() + mission_planner_client.get_logger().info(f'Success: {response.success}') + except Exception as e: + mission_planner_client.get_logger().info(f'Service call failed {e}') + break # Break out of the loop if the service call failed + else: + # This else block should be executed if no exceptions were raised + mission_planner_client.get_logger().info('Successfully created path') + break # Ensure to break out of the loop once processing is done + rclpy.shutdown() + +if __name__ == '__main__': + main() + + \ No newline at end of file diff --git a/mission/mission_planner/package.xml b/mission/mission_planner/package.xml new file mode 100644 index 00000000..3906016a --- /dev/null +++ b/mission/mission_planner/package.xml @@ -0,0 +1,20 @@ + + + + mission_planner + 0.0.0 + Mission planner for path planning + andeshog + MIT + + ament_lint_auto + ament_lint_common + + rclpy + std_msgs + vortex_msgs + + + ament_cmake + + diff --git a/motion/thruster_allocator/CMakeLists.txt b/motion/thruster_allocator/CMakeLists.txt index de9e0920..3b282d0c 100644 --- a/motion/thruster_allocator/CMakeLists.txt +++ b/motion/thruster_allocator/CMakeLists.txt @@ -13,8 +13,8 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(vortex_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) find_package(Eigen3 REQUIRED) include_directories(include) @@ -27,8 +27,8 @@ add_executable(${PROJECT_NAME}_node ament_target_dependencies(${PROJECT_NAME}_node rclcpp - geometry_msgs - vortex_msgs + geometry_msgs + std_msgs Eigen3 ) diff --git a/motion/thruster_allocator/README.md b/motion/thruster_allocator/README.md index 5cbc8f0a..208c913c 100644 --- a/motion/thruster_allocator/README.md +++ b/motion/thruster_allocator/README.md @@ -1,5 +1,5 @@ ## Thruster allocator -This package takes a thrust vector and calculates the corresponding thrust required from each individual thruster. The resulting calculation is published as a vortex_msgs ThrusterForces. +This package takes a thrust vector and calculates the corresponding thrust required from each individual thruster. The resulting calculation is published as a std_msgs::msg::Float32MultiArray. The calculation itself is based on the 'unweighted pseudoinverse-based allocator' as described in Fossen (2011): Handbook of Marine Craft Hydrodynamics and Motion Control (chapter 12.3.2). \ No newline at end of file diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp index 22f2d5cf..4aed04cd 100644 --- a/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp +++ b/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp @@ -14,7 +14,7 @@ #include #include -#include +#include using namespace std::chrono_literals; @@ -46,8 +46,10 @@ class ThrusterAllocator : public rclcpp::Node { * @return True if the vector is healthy, false otherwise. */ bool healthyWrench(const Eigen::VectorXd &v) const; - rclcpp::Publisher::SharedPtr publisher_; - rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Publisher::SharedPtr + thrust_publisher_; + rclcpp::Subscription::SharedPtr + wrench_subscriber_; rclcpp::TimerBase::SharedPtr timer_; size_t count_; int num_dof_; diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp index 739e447e..f36a4b49 100644 --- a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp +++ b/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp @@ -12,7 +12,7 @@ #include #include -#include +#include /** * @brief Check if the matrix has any NaN or INF elements. @@ -81,19 +81,18 @@ inline bool saturateVectorValues(Eigen::VectorXd &vec, double min, double max) { } /** - * @brief Converts an Eigen VectorXd to a vortex_msgs::msg::ThrusterForces + * @brief Converts an Eigen VectorXd to a std_msgs::msg::Float32MultiArray * message. * * @param u The Eigen VectorXd to be converted. - * @param msg The vortex_msgs::msg::ThrusterForces message to store the + * @param msg The std_msgs::msg::Float32MultiArray message to store the * converted values. */ +#include + inline void arrayEigenToMsg(const Eigen::VectorXd &u, - vortex_msgs::msg::ThrusterForces &msg) { - int r = u.size(); - std::vector u_vec(r); - std::copy_n(u.begin(), r, u_vec.begin()); - msg.thrust = u_vec; + std_msgs::msg::Float32MultiArray &msg) { + msg.data.assign(u.data(), u.data() + u.size()); } /** diff --git a/motion/thruster_allocator/launch/thruster_allocator.launch.py b/motion/thruster_allocator/launch/thruster_allocator.launch.py old mode 100644 new mode 100755 index cffa7ed8..6f3e8615 --- a/motion/thruster_allocator/launch/thruster_allocator.launch.py +++ b/motion/thruster_allocator/launch/thruster_allocator.launch.py @@ -1,16 +1,19 @@ -import os +from os import path from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node def generate_launch_description(): + config = path.join(get_package_share_directory('asv_setup'),'config','robots','freya.yaml') + thruster_allocator_node = Node( - package='thruster_allocator', - executable='thruster_allocator_node', - name='thruster_allocator_node', - parameters=[os.path.join(get_package_share_directory('asv_setup'),'config','robots','freya.yaml')], - output='screen', - ) + package='thruster_allocator', + executable='thruster_allocator_node', + name='thruster_allocator_node', + parameters=[config], + output='screen' + ) + return LaunchDescription([ thruster_allocator_node ]) diff --git a/motion/thruster_allocator/src/allocator_ros.cpp b/motion/thruster_allocator/src/allocator_ros.cpp index d29afb90..3f26b0a5 100644 --- a/motion/thruster_allocator/src/allocator_ros.cpp +++ b/motion/thruster_allocator/src/allocator_ros.cpp @@ -1,7 +1,7 @@ #include "thruster_allocator/allocator_ros.hpp" #include "thruster_allocator/allocator_utils.hpp" #include "thruster_allocator/pseudoinverse_allocator.hpp" -#include +#include #include #include @@ -25,17 +25,18 @@ ThrusterAllocator::ThrusterAllocator() max_thrust_ = get_parameter("propulsion.thrusters.max").as_int(); direction_ = get_parameter("propulsion.thrusters.direction").as_integer_array(); + thrust_configuration = doubleArrayToEigenMatrix( get_parameter("propulsion.thrusters.configuration_matrix") .as_double_array(), num_dof_, num_thrusters_); - subscription_ = this->create_subscription( + wrench_subscriber_ = this->create_subscription( "thrust/wrench_input", 1, std::bind(&ThrusterAllocator::wrench_callback, this, std::placeholders::_1)); - publisher_ = this->create_publisher( + thrust_publisher_ = this->create_publisher( "thrust/thruster_forces", 1); timer_ = this->create_wall_timer( @@ -60,12 +61,9 @@ void ThrusterAllocator::timer_callback() { RCLCPP_WARN(get_logger(), "Thruster forces vector required saturation."); } - vortex_msgs::msg::ThrusterForces msg_out; + std_msgs::msg::Float32MultiArray msg_out; arrayEigenToMsg(thruster_forces, msg_out); - std::transform(msg_out.thrust.begin(), msg_out.thrust.end(), - direction_.begin(), msg_out.thrust.begin(), - std::multiplies<>()); - publisher_->publish(msg_out); + thrust_publisher_->publish(msg_out); } void ThrusterAllocator::wrench_callback(const geometry_msgs::msg::Wrench &msg) { diff --git a/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_driver_lib.hpp b/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_driver_lib.hpp index 2043938a..a306b75d 100644 --- a/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_driver_lib.hpp +++ b/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_driver_lib.hpp @@ -12,7 +12,6 @@ #include #include // Used for the close function -// Declare functions we can use namespace thruster_interface_asv_driver_lib { void init(const std::string &pathToCSVFile, int8_t *thrusterMapping, int8_t *thrusterDirection, int16_t *offsetPWM, int16_t *minPWM, diff --git a/motion/thruster_interface_asv/launch/thruster_interface_asv.launch.py b/motion/thruster_interface_asv/launch/thruster_interface_asv.launch.py old mode 100644 new mode 100755 index 5c303363..8ddeb502 --- a/motion/thruster_interface_asv/launch/thruster_interface_asv.launch.py +++ b/motion/thruster_interface_asv/launch/thruster_interface_asv.launch.py @@ -1,23 +1,19 @@ from launch import LaunchDescription from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory -import os +from os import path def generate_launch_description(): # Path to the YAML file - yaml_file_path = os.path.join( - get_package_share_directory("asv_setup"), - "config/robots/", - "freya.yaml" + config = path.join(get_package_share_directory("asv_setup"),'config', 'robots', "freya.yaml") + + thruste_interface_asv_node = Node( + package='thruster_interface_asv', + namespace='thruster_interface_asv', + executable='thruster_interface_asv_node', + name='thruster_interface_asv_node', + output='screen', + parameters=[config], ) - return LaunchDescription([ - Node( - package='thruster_interface_asv', - namespace='thruster_interface_asv', - executable='thruster_interface_asv_node', - name='thruster_interface_asv_node', - output='screen', - parameters=[yaml_file_path], - ), - ]) \ No newline at end of file + return LaunchDescription([thruste_interface_asv_node]) \ No newline at end of file diff --git a/motion/thruster_interface_asv/src/thruster_interface_asv_driver_lib.cpp b/motion/thruster_interface_asv/src/thruster_interface_asv_driver_lib.cpp index 29e8cd84..24799953 100644 --- a/motion/thruster_interface_asv/src/thruster_interface_asv_driver_lib.cpp +++ b/motion/thruster_interface_asv/src/thruster_interface_asv_driver_lib.cpp @@ -15,10 +15,9 @@ std::vector<_ForcePWM> _loadDataFromCSV(const std::string &filepath) { std::vector<_ForcePWM> data; // Try opening the file and if suceeded fill our datastructure with all the - // values in the .CSV file + // values from the force mapping .CSV file std::ifstream file(filepath); if (file.is_open()) { - // Variable for the currect line value in the .CSV file std::string line; // Skip the header line @@ -29,6 +28,7 @@ std::vector<_ForcePWM> _loadDataFromCSV(const std::string &filepath) { // Temporarty variables to store data correctly ---------- // Define temporary placeholders for variables we are extracting std::string tempVar; + // Define data structure format we want our .CSV vlaues to be ordered in _ForcePWM ForcePWMDataStructure; @@ -36,13 +36,16 @@ std::vector<_ForcePWM> _loadDataFromCSV(const std::string &filepath) { // csvLineSplit variable converts "line" variable to a char stream of data // that can further be used to split and filter out values we want std::istringstream csvLineSplit(line); + // Extract Forces from "csvLineSplit" variable std::getline(csvLineSplit, tempVar, '\t'); ForcePWMDataStructure.force = std::stof(tempVar); + // Convert grams into Newtons as we expect to get Forces in Newtons but - // the .CSV file calculates forsces in grams + // the .CSV file contains forces in grams ForcePWMDataStructure.force = ForcePWMDataStructure.force * (9.81 / 1000.0); + // Extract PWM from "csvLineSplit" variable std::getline(csvLineSplit, tempVar, '\t'); ForcePWMDataStructure.pwm = std::stof(tempVar); @@ -70,10 +73,12 @@ int16_t *_interpolate_force_to_pwm(float *forces) { // accordingly if (forces[i] <= _ForcePWMTable.front().force) { interpolatedPWMArray[i] = - static_cast(_ForcePWMTable.front().pwm); // To small Force + static_cast(_ForcePWMTable.front().pwm); // Too small Force + } else if (forces[i] >= _ForcePWMTable.back().force) { interpolatedPWMArray[i] = - static_cast(_ForcePWMTable.back().pwm); // To big Force + static_cast(_ForcePWMTable.back().pwm); // Too big Force + } else { // Set temporary variables for interpolating // Initialize with the first element @@ -112,6 +117,7 @@ int16_t *_interpolate_force_to_pwm(float *forces) { const int8_t _I2C_BUS = 1; const int16_t _I2C_ADDRESS = 0x21; const char *_I2C_DEVICE = "/dev/i2c-1"; +int _fileI2C = -1; void _send_pwm_to_ESCs(int16_t *pwm) { // Variables ---------- // 4 PWM values of 16-bits @@ -133,27 +139,15 @@ void _send_pwm_to_ESCs(int16_t *pwm) { } // Data sending ---------- - // Open I2C conection - int fileI2C = open(_I2C_DEVICE, O_RDWR); - - // Error handling in case of edge cases with I2C - if (fileI2C < 0) { - std::cerr << "ERROR: Couldn't opening I2C device" << std::endl; - exit(2); - } - if (ioctl(fileI2C, I2C_SLAVE, _I2C_ADDRESS) < 0) { - std::cerr << "ERROR: Couldn't set I2C address" << std::endl; - close(fileI2C); // Close I2C connection before exiting - exit(3); - } - - // Send the I2C message - if (write(fileI2C, messageInBytesPWM, dataSize) != dataSize) { - std::cerr << "ERROR: Couldn't send data, ignoring message..." << std::endl; + try { + // Send the I2C message + if (write(_fileI2C, messageInBytesPWM, dataSize) != dataSize) { + throw std::runtime_error( + "ERROR: Couldn't send data, ignoring message..."); + } + } catch (const std::exception &error) { + std::cerr << error.what() << std::endl; } - - // Close I2C connection - close(fileI2C); } // Initial function to set everything up with thruster driver @@ -168,16 +162,9 @@ void init(const std::string &pathToCSVFile, int8_t *thrusterMapping, int8_t *thrusterDirection, int16_t *offsetPWM, int16_t *minPWM, int16_t *maxPWM) { // Load data from the .CSV file - // We load it here instead of interpolation step as this will save time as we - // only open and load all the data once, savind time in intrepolation isnce we - // dont need to open the .CSV file over and over again. _ForcePWMTable = _loadDataFromCSV(pathToCSVFile); // Set correct parameters - // - Thruster Mapping - // - Thruster Direction - // - Offset PWM - // - Limit PWM for (int8_t i = 0; i < 4; i++) { _thrusterMapping[i] = thrusterMapping[i]; _thrusterDirection[i] = thrusterDirection[i]; @@ -185,32 +172,48 @@ void init(const std::string &pathToCSVFile, int8_t *thrusterMapping, _minPWM[i] = minPWM[i]; _maxPWM[i] = maxPWM[i]; } + + // Connecting to the I2C + try { + // Open I2C conection + _fileI2C = open(_I2C_DEVICE, O_RDWR); + + // Error handling in case of edge cases with I2C + if (_fileI2C < 0) { + throw std::runtime_error("ERROR: Couldn't opening I2C device"); + } + if (ioctl(_fileI2C, I2C_SLAVE, _I2C_ADDRESS) < 0) { + throw std::runtime_error("ERROR: Couldn't set I2C address"); + } + } catch (const std::exception &error) { + std::cerr << error.what() << std::endl; + } } // The main core functionality of interacting and controling the thrusters int16_t *drive_thrusters(float *thrusterForces) { + + // Change direction of the thruster (Forward(+1)/Backwards(-1)) according to + // the direction parameter + float thrusterForcesChangedDirection[4] = {0.0, 0.0, 0.0, 0.0}; + for (int8_t i = 0; i < 4; i++) { + thrusterForcesChangedDirection[i] = + thrusterForces[i] * _thrusterDirection[i]; + } + // Remap Thrusters - // From: [pin1:thruster1 , pin2:thruster2 , pin3:thruster3 - // , pin4:thruster4 ] To: [pin1:, - // pin2:, pin3:, - // pin4:] + // From: [pin1:thruster1, pin2:thruster2, pin3:thruster3, pin4:thruster4] + // To: [pin1:, pin2:, + // pin3:, pin4:] float thrusterForcesChangedMapping[4] = {0.0, 0.0, 0.0, 0.0}; for (int8_t pinNr = 0; pinNr < 4; pinNr++) { int8_t remapedThrusterForcesIndex = _thrusterMapping[pinNr]; thrusterForcesChangedMapping[pinNr] = - thrusterForces[remapedThrusterForcesIndex]; - } - - // Change direction of the thruster (Forward(+1)/Backwards(-1)) according to - // the direction parameter - float thrusterForcesCahngedDirection[4] = {0.0, 0.0, 0.0, 0.0}; - for (int8_t i = 0; i < 4; i++) { - thrusterForcesCahngedDirection[i] = - thrusterForcesChangedMapping[i] * _thrusterDirection[i]; + thrusterForcesChangedDirection[remapedThrusterForcesIndex]; } // Interpolate forces to raw PWM values - int16_t *pwm = _interpolate_force_to_pwm(thrusterForcesCahngedDirection); + int16_t *pwm = _interpolate_force_to_pwm(thrusterForcesChangedMapping); // Offset PWM for (int8_t i = 0; i < 4; i++) { diff --git a/motion/thruster_interface_asv/src/thruster_interface_asv_node.cpp b/motion/thruster_interface_asv/src/thruster_interface_asv_node.cpp index 2c119835..a4ebf83c 100644 --- a/motion/thruster_interface_asv/src/thruster_interface_asv_node.cpp +++ b/motion/thruster_interface_asv/src/thruster_interface_asv_node.cpp @@ -4,12 +4,9 @@ class ThrusterInterfaceASVNode : public rclcpp::Node { private: // ROS2 Variables ---------- - // Creates ROS2 subscriber rclcpp::Subscription::SharedPtr _subscriberThrusterForces; - // Create ROS2 publisher rclcpp::Publisher::SharedPtr _publisherPWM; - // Create ROS2 timer/cycler rclcpp::TimerBase::SharedPtr _thruster_driver_timer; // Variables that are shared inside the object ---------- @@ -33,6 +30,7 @@ class ThrusterInterfaceASVNode : public rclcpp::Node { // Publish PWM values for debuging purposes std::shared_ptr messagePWM = std::make_shared(); + messagePWM->data.resize(4); for (int8_t i = 0; i < 4; i++) { messagePWM->data[i] = _pwm[i]; @@ -44,7 +42,7 @@ class ThrusterInterfaceASVNode : public rclcpp::Node { // Builder for the object ---------- ThrusterInterfaceASVNode() : Node("thruster_interface_asv_node") { // Thruster Driver Setup ---------- - // Get filepath of .CSV file with ROS2 file path finder + // Get filepath of .CSV file with force mapping std::string forcesToPWMDataFilePath = ament_index_cpp::get_package_share_directory("thruster_interface_asv"); forcesToPWMDataFilePath += "/config/ThrustMe_P1000_force_mapping.csv"; @@ -53,6 +51,7 @@ class ThrusterInterfaceASVNode : public rclcpp::Node { std::vector parameterThrusterMapping = this->declare_parameter>( "propulsion.thrusters.thruster_to_pin_mapping", {0, 1, 2, 3}); + int8_t ThrusterMapping[4] = { static_cast(parameterThrusterMapping[0]), static_cast(parameterThrusterMapping[1]), @@ -63,6 +62,7 @@ class ThrusterInterfaceASVNode : public rclcpp::Node { std::vector parameterThrusterDirection = this->declare_parameter>( "propulsion.thrusters.thruster_direction", {1, 1, 1, 1}); + int8_t thrusterDirection[4] = { static_cast(parameterThrusterDirection[0]), static_cast(parameterThrusterDirection[1]), @@ -73,6 +73,7 @@ class ThrusterInterfaceASVNode : public rclcpp::Node { std::vector parameterPWMOffset = this->declare_parameter>( "propulsion.thrusters.thruster_PWM_offset", {0, 0, 0, 0}); + int16_t offsetPWM[4] = {static_cast(parameterPWMOffset[0]), static_cast(parameterPWMOffset[1]), static_cast(parameterPWMOffset[2]), @@ -82,13 +83,16 @@ class ThrusterInterfaceASVNode : public rclcpp::Node { std::vector parameterPWMMin = this->declare_parameter>( "propulsion.thrusters.thruster_PWM_min", {1100, 1100, 1100, 1100}); + std::vector parameterPWMMax = this->declare_parameter>( "propulsion.thrusters.thruster_PWM_max", {1900, 1900, 1900, 1900}); + int16_t minPWM[4] = {static_cast(parameterPWMMin[0]), static_cast(parameterPWMMin[1]), static_cast(parameterPWMMin[2]), static_cast(parameterPWMMin[3])}; + int16_t maxPWM[4] = {static_cast(parameterPWMMax[0]), static_cast(parameterPWMMax[1]), static_cast(parameterPWMMax[2]), @@ -100,18 +104,18 @@ class ThrusterInterfaceASVNode : public rclcpp::Node { offsetPWM, minPWM, maxPWM); // ROS Setup ---------- - // Initialize ROS2 subscribers role + // Initialize ROS2 thrusterForces subscriber _subscriberThrusterForces = this->create_subscription( "/thrust/thruster_forces", 5, std::bind(&ThrusterInterfaceASVNode::_thruster_forces_callback, this, std::placeholders::_1)); - // Initialize ROS2 publisher role + // Initialize ROS2 pwm publisher _publisherPWM = this->create_publisher("/pwm", 5); - // Initialize a never ending cycle that continuisly publishes and drives + // Initialize a never ending cycle that continuously publishes and drives // thrusters depending on what the ThrusterForces value is set to from // ThrusterForces subscriber using namespace std::chrono_literals; @@ -126,13 +130,8 @@ class ThrusterInterfaceASVNode : public rclcpp::Node { }; int main(int argc, char *argv[]) { - // Start ROS2 rclcpp::init(argc, argv); - - // Start infinite loop of ROS2 Node rclcpp::spin(std::make_shared()); - - // Shut down ROS2 rclcpp::shutdown(); return 0; } diff --git a/navigation/waypoint_manager/CMakeLists.txt b/navigation/waypoint_manager/CMakeLists.txt new file mode 100644 index 00000000..0f41d72e --- /dev/null +++ b/navigation/waypoint_manager/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.8) +project(waypoint_manager) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclpy REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(vortex_msgs REQUIRED) + +ament_python_install_package(scripts) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS + scripts/waypoint_manager.py + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) +find_package(ament_cmake_pytest REQUIRED) +set(_pytest_tests + tests/test_waypoint_manager.py +) +foreach(_test_path ${_pytest_tests}) + get_filename_component(_test_name ${_test_path} NAME_WE) + ament_add_pytest_test(${_test_name} ${_test_path} + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + TIMEOUT 60 + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) +endforeach() +endif() + +ament_package() diff --git a/navigation/waypoint_manager/launch/waypoint_manager.launch.py b/navigation/waypoint_manager/launch/waypoint_manager.launch.py new file mode 100755 index 00000000..9e3a6d45 --- /dev/null +++ b/navigation/waypoint_manager/launch/waypoint_manager.launch.py @@ -0,0 +1,18 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + waypoint_manager_node = Node( + package='waypoint_manager', + executable='waypoint_manager.py', + name='waypoint_manager', + output='screen' + ) + + return LaunchDescription([ + waypoint_manager_node + ]) diff --git a/navigation/waypoint_manager/package.xml b/navigation/waypoint_manager/package.xml new file mode 100644 index 00000000..b5dde0d1 --- /dev/null +++ b/navigation/waypoint_manager/package.xml @@ -0,0 +1,25 @@ + + + + waypoint_manager + 0.0.0 + Waypoint manager package + alice + MIT + + ament_cmake + + rclpy + geometry_msgs + ros2launch + rnav_msgs + vortex_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_pytest + + + ament_cmake + + diff --git a/navigation/waypoint_manager/scripts/Waypoint2D.py b/navigation/waypoint_manager/scripts/Waypoint2D.py new file mode 100644 index 00000000..2fd0d8a6 --- /dev/null +++ b/navigation/waypoint_manager/scripts/Waypoint2D.py @@ -0,0 +1,7 @@ +import dataclasses + + +@dataclasses.dataclass(frozen=True) +class Waypoint2D: + north: float + east: float \ No newline at end of file diff --git a/navigation/waypoint_manager/scripts/__init__.py b/navigation/waypoint_manager/scripts/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/navigation/waypoint_manager/scripts/waypoint_manager.py b/navigation/waypoint_manager/scripts/waypoint_manager.py new file mode 100755 index 00000000..8714089a --- /dev/null +++ b/navigation/waypoint_manager/scripts/waypoint_manager.py @@ -0,0 +1,123 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from vortex_msgs.srv import Waypoint +from nav_msgs.msg import Path +from geometry_msgs.msg import PoseStamped, Point +from scripts.Waypoint2D import Waypoint2D + + +class WaypointManager(Node): + """ + Nodes created: WaypointManager + Subscribes to: d_star_lite + Publishes to: hybridpath_guidance + """ + + def __init__(self): + """ + Initializes the WaypointManager node, sets up services, clients, and publishers. + """ + + super().__init__("WaypointManager") + + self.waypoint_list = [] + + self.waypoint_service = self.create_service(Waypoint, 'waypoint', self.waypoint_callback) + self.add_waypoint_service = self.create_service(Waypoint, 'add_waypoint', self.add_waypoint_callback) + self.waypoint_list_client = self.create_client(Waypoint, 'waypoint_list') + + self.path = Path() + self.path.header.frame_id = 'world' + + def waypoint_callback(self, request, response): + """ + Clears the current waypoint list and adds new waypoints from the request. + + Args: + request: The request message containing waypoints. + response: The response message indicating the success of the operation. + + Returns: + The response message with the success status. + """ + + self.waypoint_list.clear() + return self.add_waypoint_to_list(request) + + + def add_waypoint_to_list(self, req): + """ + Adds waypoints to the waypoint list from the request. + + This method processes a request containing a list of waypoints, converting them into `Waypoint2D` objects, + and appending them to the waypoint list. It also updates the `path` with new poses. + + Args: + req (Waypoint.Request): Request containing the list of waypoints to be added. + + Returns: + Waypoint.Response: A response object indicating success. + """ + + new_waypoints = list(zip(req.waypoint[::2], req.waypoint[1::2])) + self.waypoint_list.extend([Waypoint2D(north=n, east=e) for n, e in new_waypoints]) + + for waypoint in self.waypoint_list: + new_pose = PoseStamped() + new_pose.pose.position.x = waypoint.north + new_pose.pose.position.y = waypoint.east + new_pose.pose.position.z = 0.0 + self.path.poses.append(new_pose) + + response = Waypoint.Response() + response.success = True + return response + + def add_waypoint_callback(self, request, response): + """ + Callback function for the add_waypoint service. + + Forwards the request to `add_waypoint_to_list` method. + + Args: + request: The request object containing new waypoints. + response: The response object to be filled by `add_waypoint_to_list`. + + Returns: + The response from `add_waypoint_to_list`. + """ + + return self.add_waypoint_to_list(request) + + def sending_waypoints(self): + """ + Sends the current list of waypoints to the waypoint list service. + + Waits for the waypoint list service to become available. Once available, + it sends the current list of waypoints and waits for a response. Logs the + outcome of the service call. + """ + + while not self.waypoint_list_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('Waypoint service not available, waiting again...') + request = Waypoint.Request() + + request.waypoints = self.waypoint_list + future = self.waypoint_list_client.call_async(request) + + rclpy.spin_until_future_complete(self, future, timeout_sec = 1.0) + try: + response = future.result() + self.get_logger().info(f'Waypoints successfully submitted: {response.success}') + except Exception as e: + self.get_logger().error('Service call failed %r' % (e,)) + +def main(args=None): + rclpy.init(args=args) + waypoint_manager = WaypointManager() + rclpy.spin(waypoint_manager) + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/navigation/waypoint_manager/tests/__init__.py b/navigation/waypoint_manager/tests/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/navigation/waypoint_manager/tests/test_waypoint_manager.py b/navigation/waypoint_manager/tests/test_waypoint_manager.py new file mode 100755 index 00000000..4a0d593e --- /dev/null +++ b/navigation/waypoint_manager/tests/test_waypoint_manager.py @@ -0,0 +1,43 @@ +import rclpy +import unittest +from scripts.waypoint_manager import WaypointManager +from vortex_msgs.srv import Waypoint +from scripts.Waypoint2D import Waypoint2D + +class TestWaypointManager(unittest.TestCase): + def setUp(self): + rclpy.init() + self.node = WaypointManager() + + def tearDown(self): + self.node.destroy_node() + rclpy.shutdown() + + def test_add_waypoint_to_list(self): + # Mocking a request object + class MockRequest1: + waypoint = [1.0, 2.0] + class MockRequest2: + waypoint = [2.0, 3.1] + + request1 = MockRequest1() + request2 = MockRequest2() + + # Call the method + response1 = self.node.add_waypoint_to_list(request1) + + # Check if the waypoint is added + self.assertTrue(response1.success) + self.assertIn(Waypoint2D(north =1.0, east=2.0), self.node.waypoint_list) + + # Check that a waypoint that has not been added is not in the list + self.assertNotIn(Waypoint2D(north =2.0, east=3.1), self.node.waypoint_list) + + # Adds and checks if the waypoint is added + response2 = self.node.add_waypoint_to_list(request2) + self.assertTrue(response2.success) + self.assertIn(Waypoint2D(north =2.0, east=3.1), self.node.waypoint_list) + + +if __name__ == '__main__': + unittest.main() diff --git a/scripts/add_service_files_to_bootup_sequence.sh b/scripts/add_service_files_to_bootup_sequence.sh new file mode 100755 index 00000000..22dbbb9d --- /dev/null +++ b/scripts/add_service_files_to_bootup_sequence.sh @@ -0,0 +1,91 @@ +#!/bin/bash + +# Variables ---------- +# Define variables for launching service files +SERVICE_FILE_NAME_BMS="bms.service" +SERVICE_FILE_PATH_BMS="../sensors/bms/startup_scripts/" +SERVICE_FILE_NAME_TEMPERATURE="temperature.service" +SERVICE_FILE_PATH_TEMPERATURE="../sensors/temperature/startup_scripts/" +SERVICE_FILE_NAME_INTERNAL_STATUS="internal_status.service" +SERVICE_FILE_PATH_INTERNAL_STATUS="../mission/internal_status/startup_scripts/" +SERVICE_FILE_NAME_BLACKBOX="blackbox.service" +SERVICE_FILE_PATH_BLACKBOX="../mission/blackbox/startup_scripts/" + +SYSTEMD_PATH="/etc/systemd/system/" + + + +# Navigating ---------- +echo "Navigated to the correct folder..." +# Get scripts directory and go there +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )/" +cd $SCRIPT_DIR + + + +# Setup ---------- +echo "Setting up .service files..." +# Copy the .service files to current directory +cp $SERVICE_FILE_PATH_BMS$SERVICE_FILE_NAME_BMS $SERVICE_FILE_NAME_BMS # BMS +cp $SERVICE_FILE_PATH_TEMPERATURE$SERVICE_FILE_NAME_TEMPERATURE $SERVICE_FILE_NAME_TEMPERATURE # Temperature +cp $SERVICE_FILE_PATH_INTERNAL_STATUS$SERVICE_FILE_NAME_INTERNAL_STATUS $SERVICE_FILE_NAME_INTERNAL_STATUS # Internal Status +cp $SERVICE_FILE_PATH_BLACKBOX$SERVICE_FILE_NAME_BLACKBOX $SERVICE_FILE_NAME_BLACKBOX # Blackbox + +# Replace placeholders in the .service files +# Note: This assumes is to be replaced with the current directory +sed -i "s||$SCRIPT_DIR$SERVICE_FILE_PATH_BMS|g" $SERVICE_FILE_NAME_BMS # BMS +sed -i "s||$SCRIPT_DIR$SERVICE_FILE_PATH_TEMPERATURE|g" $SERVICE_FILE_NAME_TEMPERATURE # Temperature +sed -i "s||$SCRIPT_DIR$SERVICE_FILE_PATH_INTERNAL_STATUS|g" $SERVICE_FILE_NAME_INTERNAL_STATUS # Internal Status +sed -i "s||$SCRIPT_DIR$SERVICE_FILE_PATH_BLACKBOX|g" $SERVICE_FILE_NAME_BLACKBOX # Blackbox + +# Kill the systems service files if it exists +sudo systemctl kill $SERVICE_FILE_NAME_BMS # BMS +sudo systemctl kill $SERVICE_FILE_NAME_TEMPERATURE # Temperature +sudo systemctl kill $SERVICE_FILE_NAME_INTERNAL_STATUS # Internal Status +sudo systemctl kill $SERVICE_FILE_NAME_BLACKBOX # Blackbox + +# Copy the modified .service files to the systemd directory +# Note: Need sudo permission to copy to /etc/systemd/system +sudo cp $SERVICE_FILE_NAME_BMS $SYSTEMD_PATH # BMS +sudo cp $SERVICE_FILE_NAME_TEMPERATURE $SYSTEMD_PATH # Temperature +sudo cp $SERVICE_FILE_NAME_INTERNAL_STATUS $SYSTEMD_PATH # Internal Status +sudo cp $SERVICE_FILE_NAME_BLACKBOX $SYSTEMD_PATH # Blackbox + +# Delete the redundant copy +rm $SERVICE_FILE_NAME_BMS # BMS +rm $SERVICE_FILE_NAME_TEMPERATURE # Temperature +rm $SERVICE_FILE_NAME_INTERNAL_STATUS # Internal Status +rm $SERVICE_FILE_NAME_BLACKBOX # Blackbox + +# Change permision of the .service files +sudo chmod 777 $SYSTEMD_PATH$SERVICE_FILE_NAME_BMS # BMS +sudo chmod 777 $SYSTEMD_PATH$SERVICE_FILE_NAME_TEMPERATURE # Temperature +sudo chmod 777 $SYSTEMD_PATH$SERVICE_FILE_NAME_INTERNAL_STATUS # Internal Status +sudo chmod 777 $SYSTEMD_PATH$SERVICE_FILE_NAME_BLACKBOX # Blackbox + + + +# Launching ---------- +echo "Launching .service files..." +# Reload systemd to recognize the new services +sudo systemctl daemon-reload + +# Enable new services to start on boot +sudo systemctl enable $SERVICE_FILE_NAME_BMS # BMS +sudo systemctl enable $SERVICE_FILE_NAME_TEMPERATURE # Temperature +sudo systemctl enable $SERVICE_FILE_NAME_INTERNAL_STATUS # Internal Status +sudo systemctl enable $SERVICE_FILE_NAME_BLACKBOX # Blackbox + +# Start the services immediately +sudo systemctl start $SERVICE_FILE_NAME_BMS # BMS +sudo systemctl start $SERVICE_FILE_NAME_TEMPERATURE # Temperature +sudo systemctl start $SERVICE_FILE_NAME_INTERNAL_STATUS # Internal Status +sudo systemctl start $SERVICE_FILE_NAME_BLACKBOX # Blackbox + + + +# Debugging ---------- +echo "'$SERVICE_FILE_NAME_BMS' - installed and started successfully :)" +echo "'$SERVICE_FILE_NAME_TEMPERATURE' - installed and started successfully :)" +echo "'$SERVICE_FILE_NAME_INTERNAL_STATUS' - installed and started successfully :)" +echo "'$SERVICE_FILE_NAME_BLACKBOX' - installed and started successfully :)" \ No newline at end of file diff --git a/sensors/bms/packages/jbdtool b/sensors/bms/packages/jbdtool deleted file mode 160000 index 505f9ea5..00000000 --- a/sensors/bms/packages/jbdtool +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 505f9ea5e7bad281daef501d9d0129efb35fb2e1 diff --git a/sensors/bms/packages/jbdtool/LICENSE b/sensors/bms/packages/jbdtool/LICENSE new file mode 100644 index 00000000..588b4927 --- /dev/null +++ b/sensors/bms/packages/jbdtool/LICENSE @@ -0,0 +1,27 @@ +Copyright (c) 2021, Stephen P. Shoecraft +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* 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. + +* Neither the name of the copyright holder 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 HOLDER 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. diff --git a/sensors/bms/packages/jbdtool/Makefile b/sensors/bms/packages/jbdtool/Makefile new file mode 100644 index 00000000..b2d643d9 --- /dev/null +++ b/sensors/bms/packages/jbdtool/Makefile @@ -0,0 +1,128 @@ + +DEBUG=yes +BLUETOOTH=yes +MQTT=yes + +ifeq ($(TARGET),win32) + CC = /usr/bin/i686-w64-mingw32-gcc + CFLAGS+=-D_WIN32 -DWINDOWS + WINDOWS=yes + EXT=.exe +else ifeq ($(TARGET),win64) + CC = /usr/bin/x86_64-w64-mingw32-gcc + CFLAGS+=-D_WIN64 -DWINDOWS + WINDOWS=yes + EXT=.exe +else ifeq ($(TARGET),pi) + CC = /usr/arm-linux-gnueabihf/bin/arm-linux-gnueabihf-gcc + LINUX=yes +else + CC = gcc + LINUX=yes +endif + +PROG=jbdtool$(EXT) +SRCS=main.c jbd_info.c jbd.c parson.c list.c utils.c cfg.c daemon.c module.c ip.c serial.c bt.c can.c + +ifeq ($(DEBUG),yes) +CFLAGS+=-Wall -g -DDEBUG=1 +else +CFLAGS+=-Wall -O2 -pipe +endif +#LIBS=-ldl + +STATIC=no +ifeq ($(MQTT),yes) + SRCS+=mqtt.c + CFLAGS+=-DMQTT + ifeq ($(WINDOWS),yes) + ifeq ($(STATIC),yes) + LIBS+=-lpaho-mqtt3c-static + else + LIBS+=-lpaho-mqtt3c + endif + LIBS+=-lws2_32 + ifeq ($(STATIC),yes) + LIBS+=-lgdi32 -lcrypt32 -lrpcrt4 -lkernel32 + endif + else + LIBS+=-lpaho-mqtt3c + endif +endif + +ifneq ($(WINDOWS),yes) +ifeq ($(BLUETOOTH),yes) + CFLAGS+=-DBLUETOOTH + LIBS+=-lgattlib -lgio-2.0 -lgobject-2.0 -lgmodule-2.0 -lglib-2.0 + # XXX in order to static link on my RPI4 64-bit debian 11 box + # download https://github.com/util-linux/util-linux + # ./configure --disable-shared --enable-static + # make + # cp ./.libs/libmount.a /usr/lib/aarch64-linux-gnu/ + ifeq ($(STATIC),yes) + LIBS+=-lffi -lpcre -lpthread -lmount -ldl -lblkid -lresolv -lz -lselinux + endif +endif +endif + +LIBS+=-lpthread +#LDFLAGS+=-rdynamic +OBJS=$(SRCS:.c=.o) + +ifeq ($(STATIC),yes) + LDFLAGS+=-static +endif + +.PHONY: all +all: $(PROG) + +$(PROG): $(OBJS) $(DEPS) + $(CC) $(CFLAGS) $(LDFLAGS) -o $(PROG) $(OBJS) $(LIBS) + +#$(OBJS): Makefile + +DEPDIR := .deps +CLEANFILES+=.deps +DEPFLAGS = -MT $@ -MMD -MP -MF $(DEPDIR)/$*.d + +COMPILE.c = $(CC) $(DEPFLAGS) $(CFLAGS) $(CPPFLAGS) $(TARGET_ARCH) -c + +%.o : %.c +%.o : %.c $(DEPDIR)/%.d | $(DEPDIR) + $(COMPILE.c) $(OUTPUT_OPTION) $< + +main.o: $(SRCS) + @mkdir -p .deps + @echo "#define BUILD $$(date '+%Y%m%d%H%M')LL" > build.h + $(COMPILE.c) main.c + +$(DEPDIR): ; @mkdir -p $@ + +DEPFILES := $(SRCS:%.c=$(DEPDIR)/%.d) +$(DEPFILES): + +include $(wildcard $(DEPFILES)) + +debug: $(PROG) + gdb ./$(PROG) + +install: $(PROG) + sudo install -m 755 -o bin -g bin $(PROG) /usr/bin/$(PROG) + +clean: + rm -rf $(PROG) $(OBJS) $(CLEANFILES) build.h + +zip: $(PROG) + rm -f jbdtool_$(TARGET)_static.zip + zip jbdtool_$(TARGET)_static.zip $(PROG) + +push: clean + git add -A . + git commit -m refresh + git push + +pull: clean + git reset --hard + git pull + + diff --git a/sensors/bms/packages/jbdtool/README.md b/sensors/bms/packages/jbdtool/README.md new file mode 100644 index 00000000..8f4c9c5c --- /dev/null +++ b/sensors/bms/packages/jbdtool/README.md @@ -0,0 +1,110 @@ + +JBD BMS linux-based utility + +Bluetooth support requires gattlib https://github.com/labapart/gattlib + +build & install the library then edit the Makefile and set BLUETOOTH=yes + +MQTT support requires the phao.mqtt.c library https://github.com/eclipse/paho.mqtt.c + +build & install the library then edit the Makefile and set MQTT=yes + + +To build MQTT - paho.mqtt.c (https://github.com/eclipse/paho.mqtt.c.git) + + mkdir -p build && cd build + cmake -DPAHO_BUILD_SHARED=TRUE -DPAHO_BUILD_STATIC=TRUE -DPAHO_ENABLE_TESTING=FALSE -DPAHO_BUILD_SAMPLES=FALSE .. + make && make install + +To build gattlib (https://github.com/labapart/gattlib.git) + + mkdir -p build && cd build + cmake -DGATTLIB_BUILD_EXAMPLES=NO -DGATTLIB_SHARED_LIB=NO -DGATTLIB_BUILD_DOCS=NO -DGATTLIB_PYTHON_INTERFACE=NO .. + make && make install + +IF USING BLUETOOTH, YOU MUST PAIR THE DEVICE FIRST + + $ bluetoothctl + # scan on + (look for your device) + [NEW] Device XX:XX:XX:XX:XX:XX name + # trust XX:XX:XX:XX:XX:XX + # pair XX:XX:XX:XX:XX:XX + (it may ask for your passkey) + + +Transports are specified as: + +jbdtool -t transport:target,opt1[,optN] + +For CAN: + +jbdtool -t can:device[,speed] + +example: + + jbdtool -t can:can0,500000 + +For Serial: + +jbdtool -t serial:device[,speed] + +example: + + jbdtool -t serial:/dev/ttyS0,9600 + +For Bluetooth: + +jbdtool -t bt:mac,descriptor + +exmples: + + jbdtool -t bt:01:02:03:04:05,06 + + jbdtool -t bt:01:02:03:04:05:06,ff01 + +For IP/esplink: + +jbdtool -t ip:address[,port] + +example: + + jbdtool -t ip:10.0.0.1,23 + +for CANServer/Can-over-ip + +jbdtool -t can_ip:address,[port],interface on server,[speed] + +example: + + jbdtool -t can_ip:10.0.0.1,3930,can0,500000 + + +>>> CAN bus cannot read/write parameters + + +to read all parameters using bluetooth: + +jbdtool -t bt:01:02:03:04:06 -r -a + +to list the params the program supports, use -l + +to specify single params, specify them after -r + +jbdtool -t bt:01:02:03:04:06 -r BalanceStartVoltage BalanceWindow + +to read a list of parameters using a file use -f: + +jbdtool -t serial:/dev/ttyS0,9600 -r -f jbd_settings.fig + +use -j and -J (pretty) to specify filename is a json file + + +to write parameters, specify a key value pair after -w + +jbdtool -t ip:10.0.0.1 -w BalanceStartVoltage 4050 BalanceWindow 20 + + +to send all output to a file, use -o. If the filename ends with .json, file will be written in JSON format: + +jbdtool -t can:can0,500000 -j -o pack_1.json diff --git a/sensors/bms/packages/jbdtool/battery.h b/sensors/bms/packages/jbdtool/battery.h new file mode 100644 index 00000000..7071668a --- /dev/null +++ b/sensors/bms/packages/jbdtool/battery.h @@ -0,0 +1,19 @@ + +#ifndef __BATTERY_H +#define __BATTERY_H + +#include "config.h" + +enum BATTERY_CHEMS { + BATTERY_CHEM_UNKNOWN, + BATTERY_CHEM_LITHIUM, + BATTERY_CHEM_LIFEPO4, + BATTERY_CHEM_TITANATE, +}; + +int battery_init(mybmm_config_t *conf); +void charge_check(mybmm_config_t *conf); +void charge_stop(mybmm_config_t *conf, int); +void charge_start(mybmm_config_t *conf, int); + +#endif diff --git a/sensors/bms/packages/jbdtool/bt.c b/sensors/bms/packages/jbdtool/bt.c new file mode 100644 index 00000000..6c6814dd --- /dev/null +++ b/sensors/bms/packages/jbdtool/bt.c @@ -0,0 +1,237 @@ + +#if defined(BLUETOOTH) && !defined(__WIN32) && !defined(__WIN64) +/* +Copyright (c) 2021, Stephen P. Shoecraft +All rights reserved. + +This source code is licensed under the BSD-style license found in the +LICENSE file in the root directory of this source tree. + +Bluetooth transport + +This was tested against the MLT-BT05 TTL-to-Bluetooth module (HM-10 compat) on +the Pi3B+ + +*/ + +#undef DEBUG_MEM + +#include "gattlib.h" +#include "mybmm.h" + +struct bt_session { + gatt_connection_t *c; + uuid_t uuid; + char target[32]; + char topts[32]; + char data[4096]; + int not ; + int len; + int cbcnt; + int have_char; +}; +typedef struct bt_session bt_session_t; + +static int bt_init(mybmm_config_t *conf) { return 0; } + +static void *bt_new(mybmm_config_t *conf, ...) { + bt_session_t *s; + va_list ap; + char *target, *topts; + + va_start(ap, conf); + target = va_arg(ap, char *); + topts = va_arg(ap, char *); + va_end(ap); + + dprintf(5, "target: %s, topts: %s\n", target, topts); + s = calloc(1, sizeof(*s)); + if (!s) { + perror("bt_new: malloc"); + return 0; + } + strncat(s->target, (char *)target, sizeof(s->target) - 1); + if (topts) { + if (strlen((char *)topts) && strncmp((char *)topts, "0x", 2) != 0 && + !strchr((char *)topts, '-')) + sprintf(s->topts, "0x%s", (char *)topts); + else + strncat(s->topts, (char *)topts, sizeof(s->topts) - 1); + } + dprintf(5, "target: %s, topts: %s\n", s->target, s->topts); + return s; +} + +static void notification_cb(const uuid_t *uuid, const uint8_t *data, + size_t data_length, void *user_data) { + bt_session_t *s = (bt_session_t *)user_data; + + /* Really should check for overrun here */ + dprintf(7, "s->len: %d, data: %p, data_length: %d\n", s->len, data, + (int)data_length); + if (s->len + data_length > sizeof(s->data)) + data_length = sizeof(s->data) - s->len; + memcpy(&s->data[s->len], data, data_length); + s->len += data_length; + dprintf(7, "s->len: %d\n", s->len); + s->cbcnt++; + dprintf(7, "s->cbcnt: %d\n", s->cbcnt); +} + +static int bt_open(void *handle) { + bt_session_t *s = handle; + gattlib_characteristic_t *cp; + int count, ret, i, found; + char uuid_str[40]; + + if (s->c) + return 0; + + dprintf(1, "connecting to: %s\n", s->target); + s->c = gattlib_connect(NULL, s->target, + GATTLIB_CONNECTION_OPTIONS_LEGACY_DEFAULT); + if (!s->c) { + log_write(LOG_ERROR, "Fail to connect to the bluetooth device.\n"); + return 1; + } + dprintf(1, "s->c: %p\n", s->c); + + ret = gattlib_discover_char(s->c, &cp, &count); + if (ret) { + log_write(LOG_ERROR, "Failed to discover characteristics.\n"); + return 1; + } + + if (!s->have_char) { + found = 0; + dprintf(1, "topts: %s\n", s->topts); + if (strlen(s->topts)) { + for (i = 0; i < count; i++) { + gattlib_uuid_to_string(&cp[i].uuid, uuid_str, sizeof(uuid_str)); + dprintf(1, "uuid: %s, value_handle: %04x\n", uuid_str, + cp[i].value_handle); + if (strcmp(uuid_str, s->topts) == 0) { + memcpy(&s->uuid, &cp[i].uuid, sizeof(s->uuid)); + found = 1; + break; + } + } + } else if (count > 0) { + gattlib_uuid_to_string(&cp[0].uuid, uuid_str, sizeof(uuid_str)); + dprintf(1, "uuid: %s, value_handle: 0x%04x\n", uuid_str, + cp[0].value_handle); + memcpy(&s->uuid, &cp[0].uuid, sizeof(s->uuid)); + dprintf(1, "using characteristic: %s\n", uuid_str); + found = 1; + } + free(cp); + if (!found) { + /* just force use of ffe1 */ + dprintf(1, "not found, using 0xffe1\n"); + strcpy(s->topts, "0xffe1"); + gattlib_string_to_uuid(s->topts, strlen(s->topts) + 1, &s->uuid); + } + s->have_char = 1; + } + + gattlib_register_notification(s->c, notification_cb, s); + if (gattlib_notification_start(s->c, &s->uuid)) { + dprintf(1, "error: failed to start bluetooth notification.\n"); + gattlib_disconnect(s->c); + return 1; + } else { + s->not = 1; + } + + dprintf(1, "s->c: %p\n", s->c); + return 0; +} + +static int bt_read(void *handle, ...) { + bt_session_t *s = handle; + uint8_t *buf; + int buflen, len, retries; + va_list ap; + + if (!s->c) + return -1; + + va_start(ap, handle); + buf = va_arg(ap, uint8_t *); + buflen = va_arg(ap, int); + va_end(ap); + + dprintf(1, "buf: %p, buflen: %d\n", buf, buflen); + + retries = 3; + while (1) { + dprintf(1, "s->len: %d\n", s->len); + if (!s->len) { + if (!--retries) + return 0; + sleep(1); + continue; + } + len = (s->len > buflen ? buflen : s->len); + dprintf(1, "len: %d\n", len); + memcpy(buf, s->data, len); + s->len = 0; + break; + } + + return len; +} + +static int bt_write(void *handle, ...) { + bt_session_t *s = handle; + uint8_t *buf; + int buflen; + va_list ap; + + if (!s->c) + return -1; + + va_start(ap, handle); + buf = va_arg(ap, uint8_t *); + buflen = va_arg(ap, int); + va_end(ap); + + dprintf(1, "buf: %p, buflen: %d\n", buf, buflen); + + s->len = 0; + s->cbcnt = 0; + dprintf(1, "s->c: %p\n", s->c); + if (gattlib_write_char_by_uuid(s->c, &s->uuid, buf, buflen)) + return -1; + bindump("bt write", buf, buflen); + + return buflen; +} + +static int bt_close(void *handle) { + bt_session_t *s = handle; + + dprintf(1, "s->c: %p\n", s->c); + if (s->c) { + if (s->not ) { + dprintf(1, "stopping notifications\n"); + gattlib_notification_stop(s->c, &s->uuid); + s->not = 0; + } + dprintf(1, "disconnecting...\n"); + gattlib_disconnect(s->c); + s->c = 0; + } + return 0; +} + +mybmm_module_t bt_module = {MYBMM_MODTYPE_TRANSPORT, + "bt", + 0, + bt_init, + bt_new, + bt_open, + bt_read, + bt_write, + bt_close}; +#endif diff --git a/sensors/bms/packages/jbdtool/buffer.h b/sensors/bms/packages/jbdtool/buffer.h new file mode 100644 index 00000000..7074e62d --- /dev/null +++ b/sensors/bms/packages/jbdtool/buffer.h @@ -0,0 +1,22 @@ + +#ifndef __BUFFER_H +#define __BUFFER_H + +#include + +typedef struct buffer_info buffer_t; + +typedef int(buffer_read_t)(void *, uint8_t *, int); +struct buffer_info { + uint8_t *buffer; /* buffer */ + int size; /* size of buffer */ + int bytes; /* number of bytes in buffer */ + int index; /* next read position in buffer */ + buffer_read_t *read; /* read function */ + void *ctx; /* read function context */ +}; + +buffer_t *buffer_init(int, buffer_read_t *, void *); +int buffer_get(buffer_t *, uint8_t *, int); + +#endif diff --git a/sensors/bms/packages/jbdtool/can.c b/sensors/bms/packages/jbdtool/can.c new file mode 100644 index 00000000..3473fa42 --- /dev/null +++ b/sensors/bms/packages/jbdtool/can.c @@ -0,0 +1,958 @@ + +#if !defined(__WIN32) && !defined(__WIN64) +/* +Copyright (c) 2021, Stephen P. Shoecraft +All rights reserved. + +This source code is licensed under the BSD-style license found in the +LICENSE file in the root directory of this source tree. + +*/ + +#include "mybmm.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define DEFAULT_BITRATE 250000 +#define CAN_INTERFACE_LEN 16 + +struct can_session { + int fd; + char interface[CAN_INTERFACE_LEN + 1]; + int bitrate; + struct can_filter *f; +}; +typedef struct can_session can_session_t; + +#define _getshort(p) ((short)((*((p)) << 8) | *((p) + 1))) + +static int can_init(mybmm_config_t *conf) { return 0; } + +static void *can_new(mybmm_config_t *conf, ...) { + can_session_t *s; + va_list ap; + char *target, *p; + + va_start(ap, conf); + target = va_arg(ap, char *); + va_end(ap); + dprintf(3, "target: %s\n", target); + + s = calloc(1, sizeof(*s)); + if (!s) { + perror("can_new: malloc"); + return 0; + } + s->fd = -1; + s->interface[0] = 0; + strncat(s->interface, strele(0, ",", target), sizeof(s->interface) - 1); + p = strele(1, ",", target); + if (strlen(p)) + s->bitrate = atoi(p); + else + s->bitrate = DEFAULT_BITRATE; + dprintf(3, "interface: %s, bitrate: %d\n", s->interface, s->bitrate); + + return s; +} + +/* The below get/set speed code was pulled from libsocketcan +https://git.pengutronix.de/cgit/tools/libsocketcan + +*/ + +/* libsocketcan.c + * + * (C) 2009 Luotao Fu + * + * This library is free software; you can redistribute it and/or modify it under + * the terms of the GNU Lesser General Public License as published by the Free + * Software Foundation; either version 2.1 of the License, or (at your option) + * any later version. + * + * This library is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS + * FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more + * details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this library; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#define IF_UP 1 +#define IF_DOWN 2 + +#define GET_STATE 1 +#define GET_RESTART_MS 2 +#define GET_BITTIMING 3 +#define GET_CTRLMODE 4 +#define GET_CLOCK 5 +#define GET_BITTIMING_CONST 6 +#define GET_BERR_COUNTER 7 +#define GET_XSTATS 8 +#define GET_LINK_STATS 9 + +/* + * CAN bit-timing parameters + * + * For further information, please read chapter "8 BIT TIMING + * REQUIREMENTS" of the "Bosch CAN Specification version 2.0" + * at http://www.semiconductors.bosch.de/pdf/can2spec.pdf. + */ +struct can_bittiming { + __u32 bitrate; /* Bit-rate in bits/second */ + __u32 sample_point; /* Sample point in one-tenth of a percent */ + __u32 tq; /* Time quanta (TQ) in nanoseconds */ + __u32 prop_seg; /* Propagation segment in TQs */ + __u32 phase_seg1; /* Phase buffer segment 1 in TQs */ + __u32 phase_seg2; /* Phase buffer segment 2 in TQs */ + __u32 sjw; /* Synchronisation jump width in TQs */ + __u32 brp; /* Bit-rate prescaler */ +}; + +/* + * CAN harware-dependent bit-timing constant + * + * Used for calculating and checking bit-timing parameters + */ +struct can_bittiming_const { + char name[16]; /* Name of the CAN controller hardware */ + __u32 tseg1_min; /* Time segement 1 = prop_seg + phase_seg1 */ + __u32 tseg1_max; + __u32 tseg2_min; /* Time segement 2 = phase_seg2 */ + __u32 tseg2_max; + __u32 sjw_max; /* Synchronisation jump width */ + __u32 brp_min; /* Bit-rate prescaler */ + __u32 brp_max; + __u32 brp_inc; +}; + +/* + * CAN clock parameters + */ +struct can_clock { + __u32 freq; /* CAN system clock frequency in Hz */ +}; + +/* + * CAN operational and error states + */ +enum can_state { + CAN_STATE_ERROR_ACTIVE = 0, /* RX/TX error count < 96 */ + CAN_STATE_ERROR_WARNING, /* RX/TX error count < 128 */ + CAN_STATE_ERROR_PASSIVE, /* RX/TX error count < 256 */ + CAN_STATE_BUS_OFF, /* RX/TX error count >= 256 */ + CAN_STATE_STOPPED, /* Device is stopped */ + CAN_STATE_SLEEPING, /* Device is sleeping */ + CAN_STATE_MAX +}; + +/* + * CAN bus error counters + */ +struct can_berr_counter { + __u16 txerr; + __u16 rxerr; +}; + +/* + * CAN controller mode + */ +struct can_ctrlmode { + __u32 mask; + __u32 flags; +}; + +#define CAN_CTRLMODE_LOOPBACK 0x01 /* Loopback mode */ +#define CAN_CTRLMODE_LISTENONLY 0x02 /* Listen-only mode */ +#define CAN_CTRLMODE_3_SAMPLES 0x04 /* Triple sampling mode */ +#define CAN_CTRLMODE_ONE_SHOT 0x08 /* One-Shot mode */ +#define CAN_CTRLMODE_BERR_REPORTING 0x10 /* Bus-error reporting */ +#define CAN_CTRLMODE_FD 0x20 /* CAN FD mode */ +#define CAN_CTRLMODE_PRESUME_ACK 0x40 /* Ignore missing CAN ACKs */ + +/* + * CAN device statistics + */ +struct can_device_stats { + __u32 bus_error; /* Bus errors */ + __u32 error_warning; /* Changes to error warning state */ + __u32 error_passive; /* Changes to error passive state */ + __u32 bus_off; /* Changes to bus off state */ + __u32 arbitration_lost; /* Arbitration lost errors */ + __u32 restarts; /* CAN controller re-starts */ +}; + +/* + * CAN netlink interface + */ +enum { + IFLA_CAN_UNSPEC, + IFLA_CAN_BITTIMING, + IFLA_CAN_BITTIMING_CONST, + IFLA_CAN_CLOCK, + IFLA_CAN_STATE, + IFLA_CAN_CTRLMODE, + IFLA_CAN_RESTART_MS, + IFLA_CAN_RESTART, + IFLA_CAN_BERR_COUNTER, + IFLA_CAN_DATA_BITTIMING, + IFLA_CAN_DATA_BITTIMING_CONST, + __IFLA_CAN_MAX +}; + +#define IFLA_CAN_MAX (__IFLA_CAN_MAX - 1) +struct get_req { + struct nlmsghdr n; + struct ifinfomsg i; +}; + +struct set_req { + struct nlmsghdr n; + struct ifinfomsg i; + char buf[1024]; +}; + +struct req_info { + __u8 restart; + __u8 disable_autorestart; + __u32 restart_ms; + struct can_ctrlmode *ctrlmode; + struct can_bittiming *bittiming; +}; + +#define parse_rtattr_nested(tb, max, rta) \ + (parse_rtattr((tb), (max), RTA_DATA(rta), RTA_PAYLOAD(rta))) + +#define NLMSG_TAIL(nmsg) \ + ((struct rtattr *)(((void *)(nmsg)) + NLMSG_ALIGN((nmsg)->nlmsg_len))) + +#define IFLA_CAN_MAX (__IFLA_CAN_MAX - 1) + +static int send_dump_request(int fd, const char *name, int family, int type) { + struct get_req req; + + memset(&req, 0, sizeof(req)); + + req.n.nlmsg_len = sizeof(req); + req.n.nlmsg_type = type; + req.n.nlmsg_flags = NLM_F_REQUEST; + req.n.nlmsg_pid = 0; + req.n.nlmsg_seq = 0; + + req.i.ifi_family = family; + /* + * If name is null, set flag to dump link information from all + * interfaces otherwise, just dump specified interface's link + * information. + */ + if (name == NULL) { + req.n.nlmsg_flags |= NLM_F_DUMP; + } else { + req.i.ifi_index = if_nametoindex(name); + if (req.i.ifi_index == 0) { + fprintf(stderr, "Cannot find device \"%s\"\n", name); + return -1; + } + } + + return send(fd, (void *)&req, sizeof(req), 0); +} +static void parse_rtattr(struct rtattr **tb, int max, struct rtattr *rta, + int len) { + memset(tb, 0, sizeof(*tb) * (max + 1)); + while (RTA_OK(rta, len)) { + if (rta->rta_type <= max) { + tb[rta->rta_type] = rta; + } + + rta = RTA_NEXT(rta, len); + } +} + +static int do_get_nl_link(int fd, __u8 acquire, const char *name, void *res) { + struct sockaddr_nl peer; + + char cbuf[64]; + char nlbuf[1024 * 8]; + + int ret = -1; + int done = 0; + + struct iovec iov = { + .iov_base = (void *)nlbuf, + .iov_len = sizeof(nlbuf), + }; + + struct msghdr msg = { + .msg_name = (void *)&peer, + .msg_namelen = sizeof(peer), + .msg_iov = &iov, + .msg_iovlen = 1, + .msg_control = &cbuf, + .msg_controllen = sizeof(cbuf), + .msg_flags = 0, + }; + struct nlmsghdr *nl_msg; + ssize_t msglen; + + struct rtattr *linkinfo[IFLA_INFO_MAX + 1]; + struct rtattr *can_attr[IFLA_CAN_MAX + 1]; + + if (send_dump_request(fd, name, AF_PACKET, RTM_GETLINK) < 0) { + perror("Cannot send dump request"); + return ret; + } + + while (!done && (msglen = recvmsg(fd, &msg, 0)) > 0) { + size_t u_msglen = (size_t)msglen; + /* Check to see if the buffers in msg get truncated */ + if (msg.msg_namelen != sizeof(peer) || + (msg.msg_flags & (MSG_TRUNC | MSG_CTRUNC))) { + fprintf(stderr, "Uhoh... truncated message.\n"); + return -1; + } + + for (nl_msg = (struct nlmsghdr *)nlbuf; NLMSG_OK(nl_msg, u_msglen); + nl_msg = NLMSG_NEXT(nl_msg, u_msglen)) { + int type = nl_msg->nlmsg_type; + int len; + + if (type == NLMSG_DONE) { + done++; + continue; + } + if (type != RTM_NEWLINK) + continue; + + struct ifinfomsg *ifi = NLMSG_DATA(nl_msg); + struct rtattr *tb[IFLA_MAX + 1]; + + len = nl_msg->nlmsg_len - NLMSG_LENGTH(sizeof(struct ifaddrmsg)); + parse_rtattr(tb, IFLA_MAX, IFLA_RTA(ifi), len); + + /* Finish process if the reply message is matched */ + if (strcmp((char *)RTA_DATA(tb[IFLA_IFNAME]), name) == 0) + done++; + else + continue; + + if (acquire == GET_LINK_STATS) { + if (!tb[IFLA_STATS64]) { + fprintf(stderr, "no link statistics (64-bit) found\n"); + } else { + memcpy(res, RTA_DATA(tb[IFLA_STATS64]), + sizeof(struct rtnl_link_stats64)); + ret = 0; + } + continue; + } + + if (tb[IFLA_LINKINFO]) + parse_rtattr_nested(linkinfo, IFLA_INFO_MAX, tb[IFLA_LINKINFO]); + else + continue; + + if (acquire == GET_XSTATS) { + if (!linkinfo[IFLA_INFO_XSTATS]) + fprintf(stderr, "no can statistics found\n"); + else { + memcpy(res, RTA_DATA(linkinfo[IFLA_INFO_XSTATS]), + sizeof(struct can_device_stats)); + ret = 0; + } + continue; + } + + if (!linkinfo[IFLA_INFO_DATA]) { + fprintf(stderr, "no link data found\n"); + return ret; + } + + parse_rtattr_nested(can_attr, IFLA_CAN_MAX, linkinfo[IFLA_INFO_DATA]); + + switch (acquire) { + case GET_STATE: + if (can_attr[IFLA_CAN_STATE]) { + *((int *)res) = *((__u32 *)RTA_DATA(can_attr[IFLA_CAN_STATE])); + ret = 0; + } else { + fprintf(stderr, "no state data found\n"); + } + + break; + case GET_RESTART_MS: + if (can_attr[IFLA_CAN_RESTART_MS]) { + *((__u32 *)res) = *((__u32 *)RTA_DATA(can_attr[IFLA_CAN_RESTART_MS])); + ret = 0; + } else + fprintf(stderr, "no restart_ms data found\n"); + + break; + case GET_BITTIMING: + if (can_attr[IFLA_CAN_BITTIMING]) { + memcpy(res, RTA_DATA(can_attr[IFLA_CAN_BITTIMING]), + sizeof(struct can_bittiming)); + ret = 0; + } else + fprintf(stderr, "no bittiming data found\n"); + + break; + case GET_CTRLMODE: + if (can_attr[IFLA_CAN_CTRLMODE]) { + memcpy(res, RTA_DATA(can_attr[IFLA_CAN_CTRLMODE]), + sizeof(struct can_ctrlmode)); + ret = 0; + } else + fprintf(stderr, "no ctrlmode data found\n"); + + break; + case GET_CLOCK: + if (can_attr[IFLA_CAN_CLOCK]) { + memcpy(res, RTA_DATA(can_attr[IFLA_CAN_CLOCK]), + sizeof(struct can_clock)); + ret = 0; + } else + fprintf(stderr, "no clock parameter data found\n"); + + break; + case GET_BITTIMING_CONST: + if (can_attr[IFLA_CAN_BITTIMING_CONST]) { + memcpy(res, RTA_DATA(can_attr[IFLA_CAN_BITTIMING_CONST]), + sizeof(struct can_bittiming_const)); + ret = 0; + } else + fprintf(stderr, "no bittiming_const data found\n"); + + break; + case GET_BERR_COUNTER: + if (can_attr[IFLA_CAN_BERR_COUNTER]) { + memcpy(res, RTA_DATA(can_attr[IFLA_CAN_BERR_COUNTER]), + sizeof(struct can_berr_counter)); + ret = 0; + } else + fprintf(stderr, "no berr_counter data found\n"); + + break; + + default: + fprintf(stderr, "unknown acquire mode\n"); + } + } + } + + return ret; +} + +static int open_nl_sock() { + int fd; + int sndbuf = 32768; + int rcvbuf = 32768; + unsigned int addr_len; + struct sockaddr_nl local; + + fd = socket(AF_NETLINK, SOCK_RAW, NETLINK_ROUTE); + if (fd < 0) { + perror("Cannot open netlink socket"); + return -1; + } + + setsockopt(fd, SOL_SOCKET, SO_SNDBUF, (void *)&sndbuf, sizeof(sndbuf)); + + setsockopt(fd, SOL_SOCKET, SO_RCVBUF, (void *)&rcvbuf, sizeof(rcvbuf)); + + memset(&local, 0, sizeof(local)); + local.nl_family = AF_NETLINK; + local.nl_groups = 0; + + if (bind(fd, (struct sockaddr *)&local, sizeof(local)) < 0) { + perror("Cannot bind netlink socket"); + return -1; + } + + addr_len = sizeof(local); + if (getsockname(fd, (struct sockaddr *)&local, &addr_len) < 0) { + perror("Cannot getsockname"); + return -1; + } + if (addr_len != sizeof(local)) { + fprintf(stderr, "Wrong address length %u\n", addr_len); + return -1; + } + if (local.nl_family != AF_NETLINK) { + fprintf(stderr, "Wrong address family %d\n", local.nl_family); + return -1; + } + return fd; +} + +static int get_link(const char *name, __u8 acquire, void *res) { + int err, fd; + + fd = open_nl_sock(); + if (fd < 0) + return -1; + + err = do_get_nl_link(fd, acquire, name, res); + close(fd); + + return err; +} + +int can_get_bittiming(const char *name, struct can_bittiming *bt) { + return get_link(name, GET_BITTIMING, bt); +} + +static int addattr32(struct nlmsghdr *n, size_t maxlen, int type, __u32 data) { + int len = RTA_LENGTH(4); + struct rtattr *rta; + + if (NLMSG_ALIGN(n->nlmsg_len) + len > maxlen) { + fprintf(stderr, "addattr32: Error! max allowed bound %zu exceeded\n", + maxlen); + return -1; + } + + rta = NLMSG_TAIL(n); + rta->rta_type = type; + rta->rta_len = len; + memcpy(RTA_DATA(rta), &data, 4); + n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + len; + + return 0; +} + +static int addattr_l(struct nlmsghdr *n, size_t maxlen, int type, + const void *data, int alen) { + int len = RTA_LENGTH(alen); + struct rtattr *rta; + + if (NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len) > maxlen) { + fprintf(stderr, "addattr_l ERROR: message exceeded bound of %zu\n", maxlen); + return -1; + } + + rta = NLMSG_TAIL(n); + rta->rta_type = type; + rta->rta_len = len; + memcpy(RTA_DATA(rta), data, alen); + n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len); + + return 0; +} + +static int send_mod_request(int fd, struct nlmsghdr *n) { + int status; + struct sockaddr_nl nladdr; + struct nlmsghdr *h; + + struct iovec iov = {.iov_base = (void *)n, .iov_len = n->nlmsg_len}; + struct msghdr msg = { + .msg_name = &nladdr, + .msg_namelen = sizeof(nladdr), + .msg_iov = &iov, + .msg_iovlen = 1, + }; + char buf[16384]; + + memset(&nladdr, 0, sizeof(nladdr)); + + nladdr.nl_family = AF_NETLINK; + nladdr.nl_pid = 0; + nladdr.nl_groups = 0; + + n->nlmsg_seq = 0; + n->nlmsg_flags |= NLM_F_ACK; + + status = sendmsg(fd, &msg, 0); + + if (status < 0) { + perror("Cannot talk to rtnetlink"); + return -1; + } + + iov.iov_base = buf; + while (1) { + iov.iov_len = sizeof(buf); + status = recvmsg(fd, &msg, 0); + for (h = (struct nlmsghdr *)buf; (size_t)status >= sizeof(*h);) { + int len = h->nlmsg_len; + int l = len - sizeof(*h); + if (l < 0 || len > status) { + if (msg.msg_flags & MSG_TRUNC) { + fprintf(stderr, "Truncated message\n"); + return -1; + } + fprintf(stderr, "!!!malformed message: len=%d\n", len); + return -1; + } + + if (h->nlmsg_type == NLMSG_ERROR) { + struct nlmsgerr *err = (struct nlmsgerr *)NLMSG_DATA(h); + if ((size_t)l < sizeof(struct nlmsgerr)) { + fprintf(stderr, "ERROR truncated\n"); + } else { + errno = -err->error; + if (errno == 0) + return 0; + + perror("RTNETLINK answers"); + } + return -1; + } + status -= NLMSG_ALIGN(len); + h = (struct nlmsghdr *)((char *)h + NLMSG_ALIGN(len)); + } + } + + return 0; +} + +static int do_set_nl_link(int fd, __u8 if_state, const char *name, + struct req_info *req_info) { + struct set_req req; + + const char *type = "can"; + + memset(&req, 0, sizeof(req)); + + req.n.nlmsg_len = NLMSG_LENGTH(sizeof(struct ifinfomsg)); + req.n.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK; + req.n.nlmsg_type = RTM_NEWLINK; + req.i.ifi_family = 0; + + req.i.ifi_index = if_nametoindex(name); + if (req.i.ifi_index == 0) { + fprintf(stderr, "Cannot find device \"%s\"\n", name); + return -1; + } + + if (if_state) { + switch (if_state) { + case IF_DOWN: + req.i.ifi_change |= IFF_UP; + req.i.ifi_flags &= ~IFF_UP; + break; + case IF_UP: + req.i.ifi_change |= IFF_UP; + req.i.ifi_flags |= IFF_UP; + break; + default: + fprintf(stderr, "unknown state\n"); + return -1; + } + } + + if (req_info != NULL) { + /* setup linkinfo section */ + struct rtattr *linkinfo = NLMSG_TAIL(&req.n); + addattr_l(&req.n, sizeof(req), IFLA_LINKINFO, NULL, 0); + addattr_l(&req.n, sizeof(req), IFLA_INFO_KIND, type, strlen(type)); + /* setup data section */ + struct rtattr *data = NLMSG_TAIL(&req.n); + addattr_l(&req.n, sizeof(req), IFLA_INFO_DATA, NULL, 0); + + if (req_info->restart_ms > 0 || req_info->disable_autorestart) + addattr32(&req.n, 1024, IFLA_CAN_RESTART_MS, req_info->restart_ms); + + if (req_info->restart) + addattr32(&req.n, 1024, IFLA_CAN_RESTART, 1); + + if (req_info->bittiming != NULL) { + addattr_l(&req.n, 1024, IFLA_CAN_BITTIMING, req_info->bittiming, + sizeof(struct can_bittiming)); + } + + if (req_info->ctrlmode != NULL) { + addattr_l(&req.n, 1024, IFLA_CAN_CTRLMODE, req_info->ctrlmode, + sizeof(struct can_ctrlmode)); + } + + /* mark end of data section */ + data->rta_len = (void *)NLMSG_TAIL(&req.n) - (void *)data; + + /* mark end of link info section */ + linkinfo->rta_len = (void *)NLMSG_TAIL(&req.n) - (void *)linkinfo; + } + + return send_mod_request(fd, &req.n); +} + +static int set_link(const char *name, __u8 if_state, + struct req_info *req_info) { + int err, fd; + + fd = open_nl_sock(); + if (fd < 0) + return -1; + + err = do_set_nl_link(fd, if_state, name, req_info); + close(fd); + + return err; +} + +int can_set_bittiming(const char *name, struct can_bittiming *bt) { + struct req_info req_info = { + .bittiming = bt, + }; + + return set_link(name, 0, &req_info); +} + +int can_set_bitrate(const char *name, __u32 bitrate) { + struct can_bittiming bt; + + memset(&bt, 0, sizeof(bt)); + bt.bitrate = bitrate; + + return can_set_bittiming(name, &bt); +} + +/* End of libsocketcan code */ + +static int up_down_up(int fd, char *interface) { + struct ifreq ifr; + + memset(&ifr, 0, sizeof(ifr)); + strcpy(ifr.ifr_name, interface); + + /* Bring up the IF */ + ifr.ifr_flags |= (IFF_UP | IFF_RUNNING | IFF_NOARP); + if (ioctl(fd, SIOCSIFFLAGS, &ifr) < 0) { + // perror("up_down_up: SIOCSIFFLAGS IFF_UP"); + return 1; + } + + /* Bring down the IF */ + ifr.ifr_flags = 0; + if (ioctl(fd, SIOCSIFFLAGS, &ifr) < 0) { + // perror("up_down_up: SIOCSIFFLAGS IFF_DOWN"); + return 1; + } + + /* Bring up the IF */ + ifr.ifr_flags |= (IFF_UP | IFF_RUNNING | IFF_NOARP); + if (ioctl(fd, SIOCSIFFLAGS, &ifr) < 0) { + // perror("up_down_up: SIOCSIFFLAGS IFF_UP"); + return 1; + } + return 0; +} + +static int can_open(void *handle) { + can_session_t *s = handle; + struct can_bittiming bt; + struct sockaddr_can addr; + struct ifreq ifr; + int if_up; + + /* If already open, dont try again */ + if (s->fd >= 0) + return 0; + + /* Open socket */ + dprintf(3, "opening socket...\n"); + s->fd = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (s->fd < 0) { + perror("can_open: socket"); + return 1; + } + dprintf(3, "fd: %d\n", s->fd); + + /* Get the state */ + memset(&ifr, 0, sizeof(ifr)); + strcpy(ifr.ifr_name, s->interface); + if (ioctl(s->fd, SIOCGIFFLAGS, &ifr) < 0) { + perror("can_open: SIOCGIFFLAGS"); + goto can_open_error; + } + if_up = ((ifr.ifr_flags & IFF_UP) != 0 ? 1 : 0); + + dprintf(3, "if_up: %d\n", if_up); + if (!if_up) { + /* Set the bitrate */ + if (can_set_bitrate(s->interface, s->bitrate) < 0) { + printf("ERROR: unable to set bitrate on %s!\n", s->interface); + goto can_open_error; + } + up_down_up(s->fd, s->interface); + } else { + /* Get the current timing */ + if (can_get_bittiming(s->interface, &bt) < 0) { + perror("can_open: can_get_bittiming"); + goto can_open_error; + } + + /* Check the bitrate */ + dprintf(3, "current bitrate: %d, wanted bitrate: %d\n", bt.bitrate, + s->bitrate); + if (bt.bitrate != s->bitrate) { + /* Bring down the IF */ + ifr.ifr_flags = 0; + dprintf(3, "can_open: SIOCSIFFLAGS clear\n"); + if (ioctl(s->fd, SIOCSIFFLAGS, &ifr) < 0) { + perror("can_open: SIOCSIFFLAGS IFF_DOWN"); + goto can_open_error; + } + + /* Set the bitrate */ + dprintf(3, "setting bitrate...\n"); + if (can_set_bitrate(s->interface, s->bitrate) < 0) { + printf("ERROR: unable to set bitrate on %s!\n", s->interface); + goto can_open_error; + } + up_down_up(s->fd, s->interface); + } + } + + /* Get IF index */ + strcpy(ifr.ifr_name, s->interface); + if (ioctl(s->fd, SIOCGIFINDEX, &ifr) < 0) { + perror("can_open: SIOCGIFINDEX"); + goto can_open_error; + } + + /* Bind the socket */ + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + if (bind(s->fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + perror("can_open: bind"); + goto can_open_error; + } + + /* If caller added a filter, apply it */ + if (s->f) { + if (setsockopt(s->fd, SOL_CAN_RAW, CAN_RAW_FILTER, s->f, sizeof(*s->f)) < + 0) { + perror("can_open: setsockopt"); + goto can_open_error; + } + } + dprintf(3, "done!\n"); + return 0; +can_open_error: + close(s->fd); + s->fd = -1; + return 1; +} + +static int can_read(void *handle, ...) { + can_session_t *s = handle; + struct can_frame frame; + uint8_t *buf; + int id, buflen, bytes, len; + va_list ap; + + /* If not open, error */ + if (s->fd < 0) + return -1; + + va_start(ap, handle); + id = va_arg(ap, int); + buf = va_arg(ap, uint8_t *); + buflen = va_arg(ap, int); + va_end(ap); + + dprintf(8, "id: %03x, buf: %p, buflen: %d\n", id, buf, buflen); + /* Keep reading until we get our ID */ + do { + bytes = read(s->fd, &frame, sizeof(struct can_frame)); + dprintf(8, "frame: id: %x\n", frame.can_id); + dprintf(8, "fd: %d, read bytes: %d\n", s->fd, bytes); + if (bytes < 0) { + if (errno != EAGAIN) + bytes = -1; + break; + } + if (bytes < sizeof(struct can_frame)) { + fprintf(stderr, "read: incomplete CAN frame\n"); + continue; + } + dprintf(8, "bytes: %d, id: %x, frame.can_id: %x\n", bytes, id, + frame.can_id); + } while (id != 0xFFFF && frame.can_id != id); + /* If the id is 0xFFFF, return the whole frame */ + if (id == 0x0FFFF) { + dprintf(8, "returning frame...\n"); + len = buflen < sizeof(frame) ? buflen : sizeof(frame); + memcpy(buf, &frame, len); + } else { + len = buflen > frame.can_dlc ? frame.can_dlc : buflen; + dprintf(8, "len: %d, can_dlc: %d\n", len, frame.can_dlc); + memcpy(buf, &frame.data, len); + } + if (bytes > 0 && debug >= 8) + bindump("FROM BMS", buf, buflen); + dprintf(6, "returning: %d\n", bytes); + return len; +} + +static int can_write(void *handle, ...) { + can_session_t *s = handle; + struct can_frame frame; + uint8_t *buf; + int bytes, id, buflen, len; + va_list ap; + + /* If not open, error */ + if (s->fd < 0) + return -1; + + va_start(ap, handle); + id = va_arg(ap, int); + buf = va_arg(ap, uint8_t *); + buflen = va_arg(ap, int); + va_end(ap); + + dprintf(5, "id: %03x, buf: %p, buflen: %d\n", id, buf, buflen); + len = buflen > 8 ? 8 : buflen; + dprintf(5, "len: %d\n", len); + memset(&frame, 0, sizeof(frame)); + frame.can_id = id; + frame.can_dlc = len; + memcpy(&frame.data, buf, len); + if (debug >= 5) + bindump("TO BMS", &frame, sizeof(struct can_frame)); + bytes = write(s->fd, &frame, sizeof(struct can_frame)); + dprintf(5, "fd: %d, returning: %d\n", s->fd, bytes); + if (bytes < 0) + perror("write"); + return (bytes < 0 ? -1 : len); +} + +static int can_close(void *handle) { + can_session_t *s = handle; + + if (s->fd >= 0) { + close(s->fd); + s->fd = -1; + } + return 0; +} + +mybmm_module_t can_module = { + MYBMM_MODTYPE_TRANSPORT, + "can", + 0, + can_init, + can_new, + can_open, + can_read, + can_write, + can_close, +}; +#endif diff --git a/sensors/bms/packages/jbdtool/cfg.c b/sensors/bms/packages/jbdtool/cfg.c new file mode 100644 index 00000000..2c377f7b --- /dev/null +++ b/sensors/bms/packages/jbdtool/cfg.c @@ -0,0 +1,777 @@ + +/* uncomment this if you have conv.c & conv.h */ +//#define HAVE_CONF + +#include "cfg.h" +#include "utils.h" +#include +#include +#include +#include +#include +#include +#ifdef HAVE_CONF +#include "conv.h" +#endif + +#ifdef DEBUG +#undef DEBUG +#endif +//#define DEBUG 1 + +#if DEBUG +#define dprintf(format, args...) \ + { \ + printf("%s(%d): " format "\n", __FUNCTION__, __LINE__, ##args); \ + fflush(stdout); \ + } +#else +#define dprintf(format, args...) /* noop */ +#endif + +char *typenames[] = {"Unknown", "char", "string", "byte", "short", + "int", "long", "quad", "float", "double", + "logical", "date", "list"}; + +#ifndef HAVE_CONV +static void conv_type(int dtype, void *dest, int dlen, int stype, void *src, + int slen) { + char temp[1024]; + int *iptr; + float *fptr; + double *dptr; + char *sptr; + int len; + + dprintf("stype: %d, src: %p, slen: %d, dtype: %d, dest: %p, dlen: %d", stype, + src, slen, dtype, dest, dlen); + + dprintf("stype: %s, dtype: %s\n", typenames[stype], typenames[dtype]); + switch (dtype) { + case DATA_TYPE_INT: + iptr = dest; + *iptr = atoi(src); + break; + case DATA_TYPE_FLOAT: + case DATA_TYPE_DOUBLE: + fptr = dest; + *fptr = atof(src); + break; + case DATA_TYPE_LOGICAL: + dprintf("src: %s\n", (char *)src); + /* yes/no/true/false */ + { + char temp[16]; + int i; + + iptr = dest; + i = 0; + for (sptr = src; *sptr; sptr++) + temp[i++] = tolower(*sptr); + temp[i] = 0; + dprintf("temp: %s\n", temp); + if (strcmp(temp, "yes") == 0 || strcmp(temp, "true") == 0 || + strcmp(temp, "1") == 0) + *iptr = 1; + else + *iptr = 0; + dprintf("iptr: %d\n", *iptr); + } + break; + case DATA_TYPE_STRING: + switch (stype) { + case DATA_TYPE_INT: + iptr = src; + sprintf(temp, "%d", *iptr); + break; + case DATA_TYPE_FLOAT: + fptr = src; + sprintf(temp, "%f", *fptr); + break; + case DATA_TYPE_DOUBLE: + dptr = src; + sprintf(temp, "%f", *dptr); + break; + case DATA_TYPE_LOGICAL: + iptr = src; + sprintf(temp, "%s", *iptr ? "yes" : "no"); + break; + case DATA_TYPE_STRING: + temp[0] = 0; + len = slen > dlen ? dlen : slen; + strncat(temp, src, len); + break; + default: + printf("conv_type: unknown src type: %d\n", stype); + temp[0] = 0; + break; + } + dprintf("temp: %s\n", temp); + strcpy((char *)dest, temp); + break; + default: + printf("conv_type: unknown dest type: %d\n", dtype); + break; + } +} +#endif + +CFG_INFO *cfg_create(char *filename) { + CFG_INFO *cfg_info; + + /* Allocate memory for the new cfg_info */ + cfg_info = (CFG_INFO *)malloc(CFG_INFO_SIZE); + if (!cfg_info) { + return 0; + } + + /* Save filename */ + strcpy(cfg_info->filename, filename); + + /* Create the sections list */ + cfg_info->sections = list_create(); + + /* Return the new cfg_info */ + return cfg_info; +} + +CFG_SECTION *cfg_create_section(CFG_INFO *info, char *name) { + struct _cfg_section newsec, *section; + register char *ptr; + register int x; + + /* Add the name and create the lists */ + newsec.name[0] = x = 0; + for (ptr = name; *ptr != 0 && x < sizeof(newsec.name); ptr++) + newsec.name[x++] = toupper(*ptr); + newsec.name[x] = 0; + newsec.items = list_create(); + + /* Add the new section to the cfg_info */ + section = list_add(info->sections, &newsec, CFG_SECTION_SIZE); + if (!section) { + list_destroy(newsec.items); + return 0; + } + + /* Return the new section */ + return section; +} + +void cfg_destroy(CFG_INFO *info) { + CFG_SECTION *section; + + /* Try and fool me! Ha! */ + if (!info) + return; + + /* Destroy each section's items */ + list_reset(info->sections); + while ((section = list_get_next(info->sections)) != 0) + list_destroy(section->items); + + /* Destroy the sections list */ + list_destroy(info->sections); + + /* Now free the cfg_info */ + free(info); + + return; +} + +CFG_ITEM *_cfg_get_item(CFG_SECTION *section, char *name) { + CFG_ITEM *item; + + dprintf("_cfg_get_item: looking for keyword: %s", name); + list_reset(section->items); + while ((item = list_get_next(section->items)) != 0) { + dprintf("item->keyword: %s", item->keyword); + if (strcmp(item->keyword, name) == 0) { + dprintf("_cfg_get_item: found\n"); + return item; + } + } + dprintf("_cfg_get_item: not found"); + return 0; +} + +char *cfg_get_item(CFG_INFO *info, char *section_name, char *keyword) { + CFG_SECTION *section; + CFG_ITEM *item; + char itemname[CFG_KEYWORD_SIZE]; + register int x; + + if (!info) + return 0; + dprintf("cfg_get_item: section name: %s, keyword: %s", section_name, keyword); + + /* Get the section */ + section = cfg_get_section(info, section_name); + if (!section) + return 0; + + /* Convert the keyword to uppercase */ + for (x = 0; keyword[x] != 0; x++) + itemname[x] = toupper(keyword[x]); + itemname[x] = 0; + + /* Get the item */ + item = _cfg_get_item(section, itemname); + if (item) + return item->value; + return 0; +} + +char *cfg_get_string(CFG_INFO *info, char *name, char *keyword, char *def) { + char *p; + + p = cfg_get_item(info, name, keyword); + return (p ? p : def); +} + +int cfg_get_bool(CFG_INFO *info, char *name, char *keyword, int def) { + char *p; + + p = cfg_get_item(info, name, keyword); + if (!p) + return def; + if (*p == '1' || *p == 't' || *p == 'T' || *p == 'y' || *p == 'Y') + return 1; + else + return 0; +} + +int cfg_get_int(CFG_INFO *info, char *name, char *keyword, int def) { + char *p; + int val; + + p = cfg_get_item(info, name, keyword); + if (!p) + return def; + val = atoi(p); + conv_type(DATA_TYPE_INT, &val, 0, DATA_TYPE_STRING, p, strlen(p)); + return val; +} + +long long cfg_get_quad(CFG_INFO *info, char *name, char *keyword, + long long def) { + char *p; + long long val; + + p = cfg_get_item(info, name, keyword); + if (!p) + return def; + val = strtoll(p, 0, 10); + conv_type(DATA_TYPE_QUAD, &val, 0, DATA_TYPE_STRING, p, strlen(p)); + return val; +} + +double cfg_get_double(CFG_INFO *info, char *name, char *keyword, double def) { + char *p; + double val; + + p = cfg_get_item(info, name, keyword); + if (!p) + return def; + conv_type(DATA_TYPE_DOUBLE, &val, 0, DATA_TYPE_STRING, p, strlen(p)); + return val; +} + +list cfg_get_list(CFG_INFO *info, char *name, char *keyword, char *def) { + char *str, *p; + list l; + int i; + + str = cfg_get_item(info, name, keyword); + if (!str) + str = def; + l = list_create(); + i = 0; + while (1) { + p = strele(i++, ",", str); + if (!strlen(p)) + break; + list_add(l, p, strlen(p) + 1); + } + + return l; +} + +int cfg_set_item(CFG_INFO *info, char *sname, char *iname, char *desc, + char *ival) { + CFG_SECTION *section; + CFG_ITEM *item, newitem; + ; + char itemname[CFG_KEYWORD_SIZE - 1]; + register int x; + + if (!info) + return 1; + + /* Get the section */ + dprintf("cfg_set_item: section: %s, name: %s, value: %s", sname, iname, ival); + section = cfg_get_section(info, sname); + if (!section) { + dprintf("cfg_set_item: creating %s section", sname); + section = cfg_create_section(info, sname); + if (!section) { + perror("cfg_create_section"); + return 1; + } + } + + /* Convert the keyword to uppercase */ + for (x = 0; iname[x] != 0; x++) + itemname[x] = toupper(iname[x]); + itemname[x] = 0; + + item = _cfg_get_item(section, itemname); + if (item) { + free(item->value); + item->value = (char *)malloc(strlen(ival) + 1); + if (!item->value) { + perror("cfg_set_item: malloc repl"); + return 1; + } + strcpy(item->value, ival); + dprintf("cfg_set_item: desc: %p", desc); + if (desc) { + dprintf("cfg_set_item: desc: %s", desc); + dprintf("cfg_set_item: item->desc: %p", item->desc); + if (item->desc) { + dprintf("cfg_set_item: item->desc: %s", item->desc); + free(item->desc); + } + item->desc = (char *)malloc(strlen(desc) + 1); + strcpy(item->desc, desc); + } + } else { + /* Add a new item */ + memset(&newitem, 0, sizeof(newitem)); + strncpy(newitem.keyword, itemname, sizeof(newitem.keyword) - 1); + newitem.value = (char *)malloc(strlen(ival) + 1); + if (!newitem.value) { + perror("cfg_set_item: malloc new"); + return 1; + } + strcpy(newitem.value, ival); + if (desc) { + newitem.desc = (char *)malloc(strlen(desc) + 1); + strcpy(newitem.desc, desc); + } + list_add(section->items, &newitem, CFG_ITEM_SIZE); + } + + return 0; +} + +int cfg_set_bool(CFG_INFO *info, char *name, char *keyword, int val) { + char *p; + + p = (val ? "1" : 0); + return cfg_set_item(info, name, keyword, 0, p); +} + +int cfg_set_int(CFG_INFO *info, char *name, char *keyword, int val) { + char ival[16]; + + sprintf(ival, "%d", val); + return cfg_set_item(info, name, keyword, 0, ival); +} + +int cfg_set_quad(CFG_INFO *info, char *name, char *keyword, long long val) { + char ival[32]; + + sprintf(ival, "%lld", val); + return cfg_set_item(info, name, keyword, 0, ival); +} + +int cfg_set_double(CFG_INFO *info, char *name, char *keyword, double val) { + char ival[32]; + + sprintf(ival, "%lf", val); + return cfg_set_item(info, name, keyword, 0, ival); +} + +extern CFG_ITEM *_cfg_get_item(CFG_SECTION *, char *); + +static char *_readline(char *line, int size, FILE *fp) { + int have_start; + char *line_end; + register char *ptr; + register int ch; + + /* If eof, don't bother */ + if (feof(fp)) + return 0; + + ptr = line; + line_end = line + (size - 1); + have_start = 0; + line[0] = 0; + while ((ch = fgetc(fp)) != EOF) { + /* We only accept ascii chars, thank you... */ + if (!isascii(ch)) + continue; + /* We need to see blank lines... */ + if (ch == '\n') + break; + if (have_start) { + if (ptr < line_end) + *ptr++ = ch; + else + break; + } else { + if (!isspace(ch)) { + have_start = 1; + *ptr++ = ch; + } + } + } + /* Read anything? */ + if (ptr > line) { + /* Trim up the end and return it */ + while (isspace((int)*(ptr - 1))) + ptr--; + *ptr = 0; + } + return line; +} + +CFG_INFO *cfg_read(char *filename) { + FILE *fp; + CFG_INFO *cfg_info; + CFG_SECTION *section; + CFG_ITEM *item, newitem; + char line[1022], name[CFG_SECTION_NAME_SIZE], *ts_ptr, *te_ptr; + char desc[1024]; + register char *ptr; + register int x; + + dprintf("cfg_read: filename: %s", filename); + fp = fopen(filename, "r"); + if (!fp) { + dprintf("cfg_read: error: fopen(%s): %s", filename, strerror(errno)); + return 0; + } + + /* Create a cfg_info with no type (version defaults to 2) */ + cfg_info = cfg_create(filename); + if (!cfg_info) { + fclose(fp); + return 0; + } + + /* Ensure we set section to null */ + section = 0; + + /* Read all lines */ + desc[0] = 0; + while (_readline(line, sizeof(line), fp)) { + dprintf("_cfg_read: line: %s", line); + + /* Blank lines end a section */ + if (!strlen(line)) { + if (section) + section = 0; + continue; + } + + /* Is there a LB? MUST be at start of line... */ + ts_ptr = (line[0] == '[' ? line : 0); + + /* If there's a LB see if there's a RB */ + te_ptr = (ts_ptr ? strchr(line, ']') : 0); + + /* If we have both open and close... */ + if (ts_ptr && te_ptr) { + /* Get the section name and create it */ + x = 0; + for (ptr = ts_ptr + 1; ptr < te_ptr; ptr++) { + name[x++] = toupper(*ptr); + if (x > sizeof(name) - 2) + break; + } + name[x] = 0; + section = cfg_get_section(cfg_info, name); + if (!section) + section = cfg_create_section(cfg_info, name); + desc[0] = 0; + } else { + /* Do we have a section? */ + if (!section) + continue; + + /* Is this line commented out? */ + if (line[0] == ';' || line[0] == '#') { + desc[0] = 0; + strncat(desc, line, sizeof(desc) - 1); + continue; + } + + /* Init newitem */ + memset(&newitem, 0, sizeof(newitem)); + + /* Get the keyword */ + ptr = line; + newitem.keyword[0] = x = 0; + while (*ptr != '=' && *ptr != 0) { + newitem.keyword[x++] = toupper(*ptr++); + if (x > sizeof(newitem.keyword) - 2) + break; + } + newitem.keyword[x] = 0; + trim(newitem.keyword); + + /* Get the value (little tricky-dicky here) */ + ptr++; + newitem.value = (char *)malloc(strlen(ptr) + 1); + strcpy(newitem.value, ptr); + + /* Set desc/comment */ + if (desc[0]) { + newitem.desc = (char *)malloc(strlen(desc) + 1); + strcpy(newitem.desc, desc); + } + + /* Was there a prev item for this keyword? */ + item = _cfg_get_item(section, newitem.keyword); + if (item) { + dprintf("cfg_read: deleting previous item!"); + list_delete(section->items, item); + } + + /* Add the new item to the section */ + dprintf("cfg_read: keyword: %s, value: %s", newitem.keyword, + newitem.value); + + list_add(section->items, &newitem, CFG_ITEM_SIZE); + desc[0] = 0; + } + } + fclose(fp); + + cfg_info->filename[0] = 0; + strncat(cfg_info->filename, filename, sizeof(cfg_info->filename) - 1); + return cfg_info; +} + +CFG_SECTION *cfg_get_section(CFG_INFO *info, char *name) { + CFG_SECTION *section; + char secname[CFG_SECTION_NAME_SIZE]; + register int x; + + if (!info) + return 0; + + /* Convert the section name to uppercase */ + for (x = 0; name[x] != 0; x++) + secname[x] = toupper(name[x]); + secname[x] = 0; + + /* Find the section */ + dprintf("cfg_get_section: looking for section: %s", secname); + list_reset(info->sections); + while ((section = list_get_next(info->sections)) != 0) { + dprintf("name: %s", section->name); + if (strcmp(section->name, secname) == 0) { + dprintf("found section."); + return section; + } + } + return 0; +} + +int cfg_get_tab(CFG_INFO *info, struct cfg_proctab *tab) { + struct cfg_proctab *ent; + char *p; + int i; + + dprintf("cfg_get_tab: filename: %s", info->filename); + for (i = 0; tab[i].section; i++) { + ent = &tab[i]; + p = cfg_get_item(info, ent->section, ent->keyword); + dprintf("p: %p, ent->def: %p", p, ent->def); + if (!p) { + if (ent->def) + p = ent->def; + else + continue; + } + dprintf("cfg_get_tab: section: %s, keyword: %s, value: %s", ent->section, + ent->keyword, p); + conv_type(ent->type, ent->dest, ent->dlen, DATA_TYPE_STRING, p, strlen(p)); + } + + return 0; +} + +#if 0 +int cfg_set_tab(CFG_INFO *info, struct cfg_proctab *tab, int empty) { + struct cfg_proctab *ent; + char temp[4096]; + int i; + + for(i=0; tab[i].section; i++) { + ent = &tab[i]; + conv_type(DATA_TYPE_STRING,temp,sizeof(temp),ent->type,ent->dest,ent->dlen); + dprintf("cfg_set_tab: section: %s, keyword: %s, value: %s\n", + ent->section, ent->keyword, temp); + if (empty || strlen(temp)) cfg_set_item(info,ent->section,ent->keyword,ent->desc,temp); + } + return 0; +} +#endif + +void cfg_disp_tab(struct cfg_proctab *tab, char *name, int logopt) { + char temp[1024], stemp[CFG_SECTION_NAME_SIZE], format[32]; + struct cfg_proctab *ent; + int smax, kmax; + + /* Get len of longest section */ + smax = 0; + for (ent = tab; ent->section; ent++) { + if (strlen(ent->section) > smax) + smax = strlen(ent->section); + } + /* Get len of longest keyword */ + kmax = 0; + for (ent = tab; ent->section; ent++) { + if (strlen(ent->keyword) > kmax) + kmax = strlen(ent->keyword); + } + dprintf("smax: %d, kmax: %d", smax, kmax); + sprintf(format, "%%-%ds %%%ds: %%s\n", smax + 4, kmax + 4); + dprintf("format: %s", format); + + /* Display the tab */ + dprintf("cfg_disp_tab: displaying tab..."); + dprintf("*************** %s Configuration ***************", name); + for (ent = tab; ent->section; ent++) { + dprintf("ent->section: %s", ent->section); + dprintf("keyword: %s", ent->keyword); + conv_type(DATA_TYPE_STRING, temp, sizeof(temp), ent->type, ent->dest, + ent->dlen); + dprintf("temp: %s", temp); + sprintf(stemp, "[%s]", ent->section); + printf(format, stemp, ent->keyword, temp); + } + dprintf("cfg_disp_tab: done!"); +} + +#if 0 +static int cmpsec(const void *i1, const void *i2) { + const struct cfg_proctab *p1 = i1; + const struct cfg_proctab *p2 = i2; + + return strcmp(p1->section,p2->section); +} +#endif + +#if 0 +struct cfg_proctab *cfg_combine_tabs(struct cfg_proctab *tab1, struct cfg_proctab *tab2) { + char *p; + struct cfg_proctab *tab,*ent; + int count,i,found; + list l; + + dprintf("cfg_combine_tabs: tab1: %p, tab2: %p", tab1, tab2); + + l = list_create(); + + /* Get a count of both tabs */ + count = 0; + if (tab1) { + for(ent = tab1; ent->section; ent++) { + found = 0; + list_reset(l); + while( (p = list_get_next(l)) != 0) { + if (strcmp(ent->section,p) == 0) { + found = 1; + break; + } + } + if (!found) list_add(l,ent->section,strlen(ent->section)+1); + count++; + } + } + if (tab2) { + for(ent = tab2; ent->section; ent++) { + found = 0; + list_reset(l); + while( (p = list_get_next(l)) != 0) { + if (strcmp(ent->section,p) == 0) { + found = 1; + break; + } + } + if (!found) list_add(l,ent->section,strlen(ent->section)+1); + count++; + } + } + dprintf("cfg_combine_tabs: count: %d", count); + + /* Alloc a new tab big enough to hold both XXX plus end marker */ + tab = (struct cfg_proctab *) malloc(sizeof(struct cfg_proctab) * (count + 1)); + if (!tab) return 0; + i = 0; + + /* list is a list of sections *IN THE ORDER WE FOUND THEM* */ + list_reset(l); + while( (p = list_get_next(l)) != 0) { + if (tab1) { + for(ent = tab1; ent->section; ent++) { + if (strcmp(ent->section,p) == 0) + memcpy(&tab[i++],ent,sizeof(*ent)); + } + } + if (tab2) { + for(ent = tab2; ent->section; ent++) { + if (strcmp(ent->section,p) == 0) + memcpy(&tab[i++],ent,sizeof(*ent)); + } + } + } + memset(&tab[i],0,sizeof(*ent)); + list_destroy(l); + + dprintf("cfg_combine_tabs: returning: %p",tab); + return tab; +} +#endif + +int cfg_write(CFG_INFO *info) { + FILE *fp; + CFG_SECTION *section; + CFG_ITEM *item; + + /* open the file */ + dprintf("cfg_write: filename: %s", info->filename); + fp = fopen(info->filename, "wb+"); + if (!fp) + return 1; + + /* For each section, write the items out */ + list_reset(info->sections); + while ((section = list_get_next(info->sections)) != 0) { + /* Write the section name */ + fprintf(fp, "[%s]\n", section->name); + + /* Write the section's data */ + list_reset(section->items); + while ((item = list_get_next(section->items)) != 0) { + if (item->desc) { + if (item->desc[0] != ';') + fprintf(fp, ";"); + fprintf(fp, "%s\n", item->desc); + } + fprintf(fp, "%s=%s\n", item->keyword, item->value); + } + + /* Write a blank line to end the section */ + fprintf(fp, "\n"); + } + fclose(fp); + + return 0; +} diff --git a/sensors/bms/packages/jbdtool/cfg.h b/sensors/bms/packages/jbdtool/cfg.h new file mode 100644 index 00000000..fdf709e9 --- /dev/null +++ b/sensors/bms/packages/jbdtool/cfg.h @@ -0,0 +1,118 @@ +#ifndef __CFG_H +#define __CFG_H + +/************************************************************************* + * + * Configuration file functions + * + *************************************************************************/ + +#include "list.h" +//#include "conv.h" + +/* Define the item structure */ +#define CFG_KEYWORD_SIZE 64 +struct _cfg_item { + char keyword[CFG_KEYWORD_SIZE]; + char *desc; + char *value; +}; +typedef struct _cfg_item CFG_ITEM; +typedef struct _cfg_item cfg_item; +#define CFG_ITEM_SIZE sizeof(struct _cfg_item) + +/* Define the section info structure */ +#define CFG_SECTION_NAME_SIZE 64 +struct _cfg_section { + char name[CFG_SECTION_NAME_SIZE]; + list items; +}; +typedef struct _cfg_section CFG_SECTION; +typedef struct _cfg_section cfg_section; +#define CFG_SECTION_SIZE sizeof(struct _cfg_section) + +/* Define the cfg info structure */ +struct _cfg_info { + char filename[256]; /* Filename */ + list sections; /* Section list */ +}; +typedef struct _cfg_info CFG_INFO; +typedef struct _cfg_info cfg_info; +#define CFG_INFO_SIZE sizeof(struct _cfg_info) +typedef struct _cfg_info cfg_info_t; + +struct cfg_proctab { + char *section; + char *keyword; + char *desc; + int type; + void *dest; + int dlen; + void *def; +}; +typedef struct cfg_proctab cfg_proctab_t; + +#define CFG_WRITE_ONLY 0 /* Write only non-null strings */ +#define CFG_WRITE_NULL 1 /* Write null string values (as empty) */ + +#define CFG_PROCTAB_END \ + { 0, 0, 0, 0, 0, 0 } + +#ifdef __cplusplus +extern "C" { +#endif +/* functions */ +CFG_INFO *cfg_create(char *); +CFG_SECTION *cfg_create_section(CFG_INFO *, char *); +void cfg_destroy(CFG_INFO *); +CFG_INFO *cfg_read(char *); +int cfg_write(CFG_INFO *); +CFG_SECTION *cfg_get_section(CFG_INFO *, char *); + +char *cfg_get_item(CFG_INFO *, char *, char *); +char *cfg_get_string(CFG_INFO *, char *, char *, char *); +int cfg_get_bool(CFG_INFO *, char *, char *, int); +int cfg_get_int(CFG_INFO *, char *, char *, int); +long long cfg_get_quad(CFG_INFO *, char *, char *, long long); +double cfg_get_double(CFG_INFO *, char *, char *, double); +list cfg_get_list(CFG_INFO *, char *, char *, char *); + +int cfg_set_item(CFG_INFO *, char *, char *, char *, char *); +#define cfg_set_string cfg_set_item +int cfg_set_bool(CFG_INFO *, char *, char *, int); +int cfg_set_int(CFG_INFO *, char *, char *, int); +int cfg_set_quad(CFG_INFO *, char *, char *, long long); +int cfg_set_double(CFG_INFO *, char *, char *, double); +int cfg_set_list(CFG_INFO *, char *, char *, list); + +int cfg_get_tab(CFG_INFO *, struct cfg_proctab *); +int cfg_set_tab(CFG_INFO *, struct cfg_proctab *, int); +void cfg_disp_tab(struct cfg_proctab *, char *, int); +struct cfg_proctab *cfg_combine_tabs(struct cfg_proctab *, + struct cfg_proctab *); + +#ifdef __cplusplus +} +#endif + +#ifndef HAVE_CONV +/* From conv.h */ +enum DATA_TYPE { + DATA_TYPE_UNKNOWN = 0, /* Unknown */ + DATA_TYPE_CHAR, /* Array of chars w/no null */ + DATA_TYPE_STRING, /* Array of chars w/null */ + DATA_TYPE_BYTE, /* char -- 8 bit */ + DATA_TYPE_SHORT, /* Short -- 16 bit */ + DATA_TYPE_INT, /* Integer -- 16 | 32 bit */ + DATA_TYPE_LONG, /* Long -- 32 bit */ + DATA_TYPE_QUAD, /* Quadword - 64 bit */ + DATA_TYPE_FLOAT, /* Float -- 32 bit */ + DATA_TYPE_DOUBLE, /* Double -- 64 bit */ + DATA_TYPE_LOGICAL, /* (int) Yes/No,True/False,on/off */ + DATA_TYPE_DATE, /* (char 23) DD-MMM-YYYY HH:MM:SS.HH */ + DATA_TYPE_LIST, /* Itemlist */ + DATA_TYPE_MAX /* Max data type number */ +}; +#endif + +#endif /* __CFG_H */ diff --git a/sensors/bms/packages/jbdtool/changelog b/sensors/bms/packages/jbdtool/changelog new file mode 100644 index 00000000..40e13e54 --- /dev/null +++ b/sensors/bms/packages/jbdtool/changelog @@ -0,0 +1,3 @@ + +version date what +1.8 2022-04-11 lock target diff --git a/sensors/bms/packages/jbdtool/config.h b/sensors/bms/packages/jbdtool/config.h new file mode 100644 index 00000000..e96e5e14 --- /dev/null +++ b/sensors/bms/packages/jbdtool/config.h @@ -0,0 +1,106 @@ + +#ifndef __MYBMM_CONFIG_H +#define __MYBMM_CONFIG_H + +#include "list.h" +#include "worker.h" +#include +#include +#include + +#ifdef MQTT +#include "mqtt.h" +#endif + +struct mybmm_inverter; +typedef struct mybmm_inverter mybmm_inverter_t; + +struct mybmm_pack; +typedef struct mybmm_pack mybmm_pack_t; + +struct mqtt_info; +typedef struct mqtt_info mqtt_info_t; + +struct mybmm_config { +#ifdef MQTT + mqtt_session_t *mqtt; /* MQTT info */ +#endif + char *filename; /* Config filename */ + void *logfp; /* Log filehandle */ + char db_name[32]; /* DB Name */ + void *dlsym_handle; /* Image handle */ + list modules; /* Modules */ + mybmm_inverter_t *inverter; /* Inverter */ + pthread_t inverter_tid; /* Inverter Thread ID */ + list packs; /* Packs */ + pthread_t pack_tid; /* Pack thread ID */ + worker_pool_t *pack_pool; /* Pack worker pool */ + int interval; /* Check interval */ + int system_voltage; /* System Voltage (defaults to 48) */ + int battery_chem; /* Battery type (0=Li-ion, 1=LifePO4, 2=Titanate) */ + float battery_voltage; /* Battery Voltage */ + float battery_amps; /* Total amount of power into/out of battery */ + float battery_temp; + float frequency; + int cells; /* Number of cells per battery pack */ + float cell_low; /* Cell discharge low cutoff */ + float cell_min; /* Min cell voltage per chem */ + float cell_crit_low; /* Cell critical low */ + float cell_high; /* Cell charge high cutoff */ + float cell_max; /* Max cell voltage per chem */ + float cell_crit_high; /* Cell critical high */ + float capacity; /* Total capacity, in AH (all packs) */ + float c_rate; /* Charge current rate */ + float e_rate; /* Discharge current rate */ + float kwh; /* Calculated kWh */ + float soc; /* State of Charge */ + float soh; /* State of Health */ + float input_current; /* Power from sources */ + float output_current; /* Power to loads/batt */ + float capacity_remain; /* Remaining capacity */ + float max_charge_amps; /* From inverter */ + float max_discharge_amps; /* From inverter */ + float discharge_voltage; /* Calculated: cell_low * cells */ + float discharge_amps; /* Calculated */ + float charge_voltage; /* Calculated: cell_high * cells */ + float charge_amps; /* Calculated */ + float min_charge_amps; + float user_capacity; /* User-specified capacity */ + float user_charge_voltage; /* User-specified charge voltage */ + float user_charge_amps; /* User-specified charge amps */ + float user_discharge_voltage; /* User-specified discharge voltage */ + float user_discharge_amps; /* User-specified discharge amps */ + float user_soc; /* Forced State of Charge */ + void *cfg; /* Config file handle */ + uint16_t state; /* States */ + uint16_t capabilities; /* Capabilities */ + int use_packv; + int charging; + int charge_mode; + float charge_max_voltage; + float charge_target_voltage; + float user_charge_max_voltage; + int charge_at_max; + float pack_temp_max, start_temp; + float pack_cell_max, pack_cell_min; + time_t cv_start_time; + float tvolt; +}; +typedef struct mybmm_config mybmm_config_t; + +/* States */ +#define MYBMM_STATE_CONFIG_DIRTY \ + 0x01 /* Config has been updated & needs written */ + +/* Capabilities */ +#define MYBMM_CHARGE_CONTROL \ + 0x01 /* All packs have ability to start/stop charging */ +#define MYBMM_DISCHARGE_CONTROL \ + 0x02 /* All packs have ability to start/stop discharging */ +#define MYBMM_BALANCE_CONTROL \ + 0x04 /* All packs have ability to start/stop balancing */ + +mybmm_config_t *get_config(char *); +int reconfig(mybmm_config_t *conf); + +#endif /* __MYBMM_CONFIG_H */ diff --git a/sensors/bms/packages/jbdtool/daemon.c b/sensors/bms/packages/jbdtool/daemon.c new file mode 100644 index 00000000..e3993cba --- /dev/null +++ b/sensors/bms/packages/jbdtool/daemon.c @@ -0,0 +1,51 @@ +#ifndef __WIN32 +#include "debug.h" +#include +#include +#include +#include +#include +#include +#include + +int become_daemon(void) { + pid_t pid; + + /* Fork the process */ + DLOG(LOG_DEBUG, "become_daemon: forking process..."); + pid = fork(); + + /* If pid < 0, error */ + if (pid < 0) { + DLOG(LOG_DEBUG | LOG_SYSERR, "become_daemon: fork"); + return 1; + } + + /* If pid > 0, parent */ + else if (pid > 0) { + DLOG(LOG_DEBUG, "become_daemon: parent exiting..."); + _exit(0); + } + + /* Set the session ID */ + setsid(); + + /* Fork again */ + pid = fork(); + if (pid < 0) + return 1; + else if (pid != 0) { + DLOG(LOG_DEBUG, "become_daemon(2): parent exiting..."); + _exit(0); + } + + /* Set the umask */ + umask(022); + + /* Close stdin,stdout,stderr */ + close(0); + close(1); + close(2); + return 0; +} +#endif diff --git a/sensors/bms/packages/jbdtool/debug.h b/sensors/bms/packages/jbdtool/debug.h new file mode 100644 index 00000000..1983bd1e --- /dev/null +++ b/sensors/bms/packages/jbdtool/debug.h @@ -0,0 +1,79 @@ + +/* +Copyright (c) 2021, Stephen P. Shoecraft +All rights reserved. + +This source code is licensed under the BSD-style license found in the +LICENSE file in the root directory of this source tree. +*/ + +#ifndef __DEBUG_H +#define __DEBUG_H + +#include "utils.h" +#include + +extern int debug; + +#if DEBUG != 0 +//#define dprintf(level, format, args...) { if (debug >= level) printf("%s(%d): +//" format,__FUNCTION__,__LINE__, ## args); } +#define dprintf(level, format, args...) \ + { \ + if (debug >= level) \ + log_write(LOG_DEBUG, "%s(%d) %s: " format, __FILE__, __LINE__, \ + __FUNCTION__, ##args); \ + } +#define DPRINTF(format, args...) \ + printf("%s(%d): " format, __FUNCTION__, __LINE__, ##args) +#define DLOG(opts, format, args...) \ + log_write(opts, "%s(%d): " format, __FUNCTION__, __LINE__, ##args) +#define DDLOG(format, args...) \ + log_write(LOG_DEBUG, "%s(%d): " format, __FUNCTION__, __LINE__, ##args) +#else +#define dprintf(level, format, args...) /* noop */ +#define DPRINTF(format, args...) /* noop */ +#define DLOG(opts, format, args...) /* noop */ +#define DDLOG(format, args...) /* noop */ +#endif + +/* dlevels +1-9 programs +2-9 libs +5 transports +7 protocol bindump +*/ + +#if defined(DEBUG_MEM) && !defined(__WIN64) +void *mem_alloc(size_t size, int clear); +void *mem_malloc(size_t size); +void *mem_calloc(size_t nmemb, size_t size); +void *mem_realloc(void *, size_t size); +void mem_free(void *mem); +unsigned long mem_used(void); +unsigned long mem_peak(void); +#ifdef malloc +#undef malloc +#endif +#define malloc(s) mem_alloc(s, 0) +#ifdef calloc +#undef calloc +#endif +#define calloc(n, s) mem_alloc((n) * (s), 1) +#ifdef realloc +#undef realloc +#endif +#define realloc(n, s) mem_realloc(n, s) +#ifdef free +#undef free +#endif +#define free(s) mem_free(s) +#else +#include +#define mem_alloc(s, c) (c ? calloc(s, 1) : malloc(s)) +#define mem_free(m) free(m) +#define mem_used() 0L +#define mem_peak() 0L +#endif + +#endif /* __DEBUG_H */ diff --git a/sensors/bms/packages/jbdtool/dsfuncs.h b/sensors/bms/packages/jbdtool/dsfuncs.h new file mode 100644 index 00000000..1a084c4c --- /dev/null +++ b/sensors/bms/packages/jbdtool/dsfuncs.h @@ -0,0 +1,37 @@ + +#ifndef __DEVSERVER_FUNCS_H +#define __DEVSERVER_FUNCS_H + +int devserver_send(int fd, uint8_t opcode, uint8_t unit, void *data, + int datasz); +int devserver_recv(int fd, uint8_t *opcode, uint8_t *unit, void *data, + int datasz, int timeout); +int devserver_request(int fd, uint8_t opcode, uint8_t unit, void *data, + int len); +int devserver_reply(int fd, uint8_t status, uint8_t unit, uint8_t *data, + int len); +int devserver_error(int fd, uint8_t status); + +#define devserver_getshort(p) (uint16_t)(*(p) | (*((p) + 1) << 8)) +#define devserver_putshort(p, v) \ + { \ + float tmp; \ + *(p) = ((uint16_t)(tmp = (v))); \ + *((p) + 1) = ((uint16_t)(tmp = (v)) >> 8); \ + } +#define devserver_get16(p) (uint16_t)(*(p) | (*((p) + 1) << 8)) +#define devserver_put16(p, v) \ + { \ + float tmp; \ + *(p) = ((uint16_t)(tmp = (v))); \ + *((p) + 1) = ((uint16_t)(tmp = (v)) >> 8); \ + } +#define devserver_get32(p) \ + (uint16_t)(*(p) | (*((p) + 1) << 8) | (*((p) + 2) << 16) | (*((p) + 3) << 24)) +#define devserver_put32(p, v) \ + *(p) = ((int)(v)&0xFF); \ + *((p) + 1) = ((int)(v) >> 8) & 0xFF; \ + *((p) + 2) = ((int)(v) >> 16) & 0xFF; \ + *((p) + 3) = ((int)(v) >> 24) & 0xFF + +#endif /* __DEVSERVER_FUNCS_H */ diff --git a/sensors/bms/packages/jbdtool/ip.c b/sensors/bms/packages/jbdtool/ip.c new file mode 100644 index 00000000..97829fee --- /dev/null +++ b/sensors/bms/packages/jbdtool/ip.c @@ -0,0 +1,234 @@ + +/* +Copyright (c) 2021, Stephen P. Shoecraft +All rights reserved. + +This source code is licensed under the BSD-style license found in the +LICENSE file in the root directory of this source tree. +*/ + +#include +#ifdef __WIN32 +#include +#include +#include +#else +#include +#include +#include +#include +#endif +#include "mybmm.h" +#include +#include + +#define DEFAULT_PORT 23 + +#ifdef __WIN32 +typedef SOCKET socket_t; +#define SOCKET_CLOSE(s) closesocket(s); +#else +typedef int socket_t; +#define SOCKET_CLOSE(s) close(s) +#define INVALID_SOCKET -1 +#endif + +struct ip_session { + socket_t sock; + char target[64]; + int port; +}; +typedef struct ip_session ip_session_t; + +static int ip_init(mybmm_config_t *conf) { +#ifdef __WIN32 + WSADATA wsaData; + int iResult; + + dprintf(1, "initializng winsock...\n"); + /* Initialize Winsock */ + iResult = WSAStartup(MAKEWORD(2, 2), &wsaData); + if (iResult != 0) { + log_write(LOG_SYSERR, "WSAStartup"); + return 1; + } +#endif + return 0; +} + +static void *ip_new(mybmm_config_t *conf, ...) { + ip_session_t *s; + va_list ap; + char *target; + char *p; + + va_start(ap, conf); + target = va_arg(ap, char *); + va_end(ap); + + dprintf(1, "target: %s\n", target); + + s = calloc(1, sizeof(*s)); + if (!s) { + perror("ip_new: malloc"); + return 0; + } + s->sock = INVALID_SOCKET; + p = strchr((char *)target, ':'); + if (p) + *p = 0; + strncat(s->target, (char *)target, sizeof(s->target) - 1); + if (p) { + p++; + s->port = atoi(p); + } + if (!s->port) + s->port = DEFAULT_PORT; + dprintf(5, "target: %s, port: %d\n", s->target, s->port); + return s; +} + +static int ip_open(void *handle) { + ip_session_t *s = handle; + struct sockaddr_in addr; + socklen_t sin_size; + struct hostent *he; + char temp[64]; + uint8_t *ptr; + + dprintf(1, "s->sock: %p\n", s->sock); + if (s->sock != INVALID_SOCKET) + return 0; + + dprintf(1, "creating socket...\n"); + s->sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); + if (s->sock == INVALID_SOCKET) { + log_write(LOG_SYSERR, "socket"); + perror("socket"); + return 1; + } + + /* Try to resolve the target */ + he = (struct hostent *)0; + if (!is_ip(s->target)) { + he = gethostbyname(s->target); + dprintf(6, "he: %p\n", he); + if (he) { + ptr = (unsigned char *)he->h_addr; + sprintf(temp, "%d.%d.%d.%d", ptr[0], ptr[1], ptr[2], ptr[3]); + } + } + if (!he) + strcpy(temp, s->target); + dprintf(3, "temp: %s\n", temp); + + memset(&addr, 0, sizeof(addr)); + sin_size = sizeof(addr); + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = inet_addr(temp); + addr.sin_port = htons(s->port); + dprintf(3, "connecting...\n"); + if (connect(s->sock, (struct sockaddr *)&addr, sin_size) < 0) { + lprintf(LOG_SYSERR, "connect to %s", s->target); + return 1; + } + return 0; +} + +static int ip_read(void *handle, ...) { + ip_session_t *s = handle; + uint8_t *buf; + int buflen, bytes, bidx, num; + va_list ap; + struct timeval tv; + fd_set rdset; + + va_start(ap, handle); + buf = va_arg(ap, uint8_t *); + buflen = va_arg(ap, int); + va_end(ap); + + dprintf(5, "buf: %p, buflen: %d\n", buf, buflen); + + tv.tv_usec = 0; + tv.tv_sec = 1; + + FD_ZERO(&rdset); + + if (s->sock == INVALID_SOCKET) + return -1; + + dprintf(5, "reading...\n"); + bidx = 0; + while (1) { + FD_SET(s->sock, &rdset); + num = select(s->sock + 1, &rdset, 0, 0, &tv); + dprintf(5, "num: %d\n", num); + if (!num) + break; + dprintf(5, "buf: %p, bufen: %d\n", buf, buflen); + bytes = recv(s->sock, buf, buflen, 0); + dprintf(5, "bytes: %d\n", bytes); + if (bytes < 0) { + if (errno == EAGAIN) + continue; + bidx = -1; + break; + } else if (bytes == 0) { + break; + } else if (bytes > 0) { + buf += bytes; + bidx += bytes; + buflen -= bytes; + if (buflen < 1) + break; + } + } + if (debug >= 5) + bindump("ip_read", buf, bidx); + dprintf(5, "returning: %d\n", bidx); + return bidx; +} + +static int ip_write(void *handle, ...) { + ip_session_t *s = handle; + uint8_t *buf; + int bytes, buflen; + va_list ap; + + va_start(ap, handle); + buf = va_arg(ap, uint8_t *); + buflen = va_arg(ap, int); + va_end(ap); + + dprintf(4, "s->sock: %p\n", s->sock); + + if (s->sock == INVALID_SOCKET) + return -1; + if (debug >= 5) + bindump("ip_write", buf, buflen); + bytes = send(s->sock, buf, buflen, 0); + dprintf(4, "bytes: %d\n", bytes); + return bytes; +} + +static int ip_close(void *handle) { + ip_session_t *s = handle; + + if (s->sock != INVALID_SOCKET) { + dprintf(5, "closing...\n"); + SOCKET_CLOSE(s->sock); + s->sock = INVALID_SOCKET; + } + return 0; +} + +mybmm_module_t ip_module = {MYBMM_MODTYPE_TRANSPORT, + "ip", + 0, + ip_init, + ip_new, + ip_open, + ip_read, + ip_write, + ip_close}; diff --git a/sensors/bms/packages/jbdtool/jbd.c b/sensors/bms/packages/jbdtool/jbd.c new file mode 100644 index 00000000..63704120 --- /dev/null +++ b/sensors/bms/packages/jbdtool/jbd.c @@ -0,0 +1,547 @@ + +/* +Copyright (c) 2021, Stephen P. Shoecraft +All rights reserved. + +This source code is licensed under the BSD-style license found in the +LICENSE file in the root directory of this source tree. +*/ + +#include "jbd.h" +#include "mybmm.h" +#ifndef __WIN32 +#include +#endif + +#define JBD_PKT_START 0xDD +#define JBD_PKT_END 0x77 +#define JBD_CMD_READ 0xA5 +#define JBD_CMD_WRITE 0x5A + +#define JBD_CMD_HWINFO 0x03 +#define JBD_CMD_CELLINFO 0x04 +#define JBD_CMD_HWVER 0x05 +#define JBD_CMD_MOS 0xE1 + +#define JBD_MOS_CHARGE 0x01 +#define JBD_MOS_DISCHARGE 0x02 + +#define _getshort(p) ((short)((*((p)) << 8) | *((p) + 1))) +#define _putshort(p, v) \ + { \ + float tmp; \ + *((p)) = ((int)(tmp = v) >> 8); \ + *((p + 1)) = (int)(tmp = v); \ + } + +static uint16_t jbd_crc(unsigned char *data, int len) { + uint16_t crc = 0; + register int i; + + // bindump("jbd_crc",data,len); + dprintf(5, "len: %d\n", len); + dprintf(5, "crc: %x\n", crc); + for (i = 0; i < len; i++) + crc -= data[i]; + dprintf(5, "crc: %x\n", crc); + return crc; +} + +static int jbd_verify(uint8_t *buf, int len, uint8_t reg) { + uint16_t my_crc, pkt_crc; + int i, data_length; + + /* Anything less than 7 bytes is an error */ + dprintf(3, "len: %d\n", len); + if (len < 7) + return 1; + if (debug >= 5) + bindump("verify", buf, len); + + i = 0; + /* 0: Start bit */ + dprintf(5, "start bit: %x\n", buf[i]); + if (buf[i++] != 0xDD) + return 1; + /* 1: Register */ + dprintf(5, "register: %x, wanted: %x\n", buf[i], reg); + if (buf[i++] != reg) + return 1; + /* 2: Status */ + dprintf(5, "status: %d\n", buf[i]); + // if (buf[i++] != 0) return 1; + i++; + /* 3: Length - must be size of packet minus protocol bytes */ + data_length = buf[i++]; + dprintf(5, "data_length: %d, len - 7: %d\n", data_length, len - 7); + if (data_length != (len - 7)) + return 1; + /* Data */ + my_crc = jbd_crc(&buf[2], data_length + 2); + i += data_length; + /* CRC */ + pkt_crc = _getshort(&buf[i]); + dprintf(5, "my_crc: %x, pkt_crc: %x\n", my_crc, pkt_crc); + if (my_crc != pkt_crc) { + dprintf(1, "CRC ERROR: my_crc: %x, pkt_crc: %x\n", my_crc, pkt_crc); + bindump("data", buf, len); + return 1; + } + i += 2; + /* Stop bit */ + dprintf(5, "stop bit: %x\n", buf[i]); + if (buf[i++] != 0x77) + return 1; + + dprintf(3, "good data!\n"); + return 0; +} + +static int jbd_cmd(uint8_t *pkt, int pkt_size, int action, uint8_t reg, + uint8_t *data, int data_len) { + unsigned short crc; + int idx; + + /* Make sure no data in command */ + if (action == JBD_CMD_READ) + data_len = 0; + + dprintf(5, "action: %x, reg: %x, data: %p, len: %d\n", action, reg, data, + data_len); + memset(pkt, 0, pkt_size); + idx = 0; + pkt[idx++] = JBD_PKT_START; + pkt[idx++] = action; + pkt[idx++] = reg; + pkt[idx++] = data_len; + if (idx + data_len > pkt_size) + return -1; + memcpy(&pkt[idx], data, data_len); + crc = jbd_crc(&pkt[2], data_len + 2); + idx += data_len; + _putshort(&pkt[idx], crc); + idx += 2; + pkt[idx++] = JBD_PKT_END; + // bindump("pkt",pkt,idx); + dprintf(5, "returning: %d\n", idx); + return idx; +} + +void jbd_get_protect(struct jbd_protect *p, unsigned short bits) { +#ifdef DEBUG + { + char bitstr[40]; + unsigned short mask = 1; + int i; + + i = 0; + while (mask) { + bitstr[i++] = ((bits & mask) != 0 ? '1' : '0'); + mask <<= 1; + } + bitstr[i] = 0; + dprintf(5, "protect: %s\n", bitstr); + } +#endif +#if 0 +Bit0 monomer overvoltage protection +Bit1 monomer under voltage protection +Bit2 whole group overvoltage protection +Bit3 whole group under voltage protection +Bit4 charging over temperature protection +Bit5 charging low temperature protection +Bit6 discharge over temperature protection +Bit7 discharge low temperature protection +Bit8 charging over-current protection +Bit9 discharge over current protection +Bit10 short circuit protection +Bit11 front end detection IC error +Bit12 software lock MOS +Bit13 ~ bit15 reserved +bit0 ...... +Single overvoltage protection +bit1 ...... +Single undervoltage protection +bit2 ...... +Whole group overvoltage protection +bit3 ...... +Whole group undervoltage protection +bit4 ...... +Charge over temperature protection +bit5 ...... +Charge low temperature protection +bit6 ...... +Discharge over temperature protection +bit7 ...... +Discharge low temperature protection +bit8 ...... +Charge overcurrent protection +bit9 ...... +Discharge overcurrent protection +bit10 .... +Short circuit protection +bit11 ....IC.. +Front detection IC error +bit12 ....MOS +Software lock MOS + bit13~bit15 .. +Reserved + +#endif +} + +#ifndef __WIN32 +#define CRC_16_POLYNOMIALS 0xa001 +static unsigned short jbd_can_crc(unsigned char *pchMsg) { + unsigned char i, chChar; + unsigned short wCRC = 0xFFFF; + int wDataLen = 6; + + while (wDataLen--) { + chChar = *pchMsg++; + wCRC ^= (unsigned short)chChar; + for (i = 0; i < 8; i++) { + if (wCRC & 0x0001) + wCRC = (wCRC >> 1) ^ CRC_16_POLYNOMIALS; + else + wCRC >>= 1; + } + } + return wCRC; +} + +int jbd_can_get(jbd_session_t *s, int id, unsigned char *data, int datalen, + int chk) { + unsigned short crc, mycrc; + int retries, bytes; + + dprintf(3, "id: %03x, data: %p, len: %d, chk: %d\n", id, data, datalen, chk); + retries = 3; + do { + if (s->tp->write(s->tp_handle, id, 0, 0) < 0) + return 1; + memset(data, 0, datalen); + bytes = s->tp->read(s->tp_handle, id, data, datalen); + dprintf(3, "bytes: %d\n", bytes); + if (bytes < 0) + return 1; + if (!chk) + break; + /* Verify CRC */ + crc = _getshort(&data[6]); + dprintf(3, "crc: %x\n", crc); + mycrc = jbd_can_crc(data); + dprintf(3, "mycrc: %x\n", mycrc); + if (crc == 0 || crc == mycrc) + break; + } while (retries--); + dprintf(3, "retries: %d\n", retries); + if (!retries) + printf("ERROR: CRC failed retries for ID %03x!\n", id); + return (retries < 0); +} + +int jbd_can_get_crc(jbd_session_t *s, int id, unsigned char *data, int len) { + return jbd_can_get(s, id, data, len, 1); +} + +/* For CAN bus only */ +int jbd_can_get_pack(struct jbd_session *s) { + mybmm_pack_t *pp = s->pp; + uint8_t data[8]; + int id, i; + uint16_t protectbits, fetbits; + struct jbd_protect prot; + + /* 0x102 Equalization state low byte, equalized state high byte, protection + * status */ + if (jbd_can_get_crc(s, 0x102, data, 8)) + return 1; + pp->balancebits = _getshort(&data[0]); + pp->balancebits |= _getshort(&data[2]) << 16; + protectbits = _getshort(&data[4]); + /* Do we have any protection actions? */ + if (protectbits) { + jbd_get_protect(&prot, protectbits); + } + + /* 0x103 FET control status, production date, software version */ + if (jbd_can_get_crc(s, 0x103, data, 8)) + return 1; + fetbits = _getshort(&data[0]); + dprintf(2, "fetbits: %02x\n", fetbits); + if (fetbits & JBD_MOS_CHARGE) + mybmm_set_state(pp, MYBMM_PACK_STATE_CHARGING); + else + mybmm_clear_state(pp, MYBMM_PACK_STATE_CHARGING); + if (fetbits & JBD_MOS_DISCHARGE) + mybmm_set_state(pp, MYBMM_PACK_STATE_DISCHARGING); + else + mybmm_clear_state(pp, MYBMM_PACK_STATE_DISCHARGING); + if (s->balancing) + mybmm_set_state(pp, MYBMM_PACK_STATE_BALANCING); + else + mybmm_clear_state(pp, MYBMM_PACK_STATE_BALANCING); + + if (jbd_can_get_crc(s, 0x104, data, 8)) + return 1; + pp->cells = data[0]; + dprintf(1, "strings: %d\n", pp->cells); + pp->ntemps = data[1]; + dprintf(1, "probes: %d\n", pp->ntemps); + + /* Get Temps */ + i = 0; +#define CTEMP(v) ((v - 2731) / 10) + for (id = 0x105; id < 0x107; id++) { + if (jbd_can_get_crc(s, id, data, 8)) + return 1; + pp->temps[i++] = CTEMP((float)_getshort(&data[0])); + if (i >= pp->ntemps) + break; + pp->temps[i++] = CTEMP((float)_getshort(&data[2])); + if (i >= pp->ntemps) + break; + pp->temps[i++] = CTEMP((float)_getshort(&data[4])); + if (i >= pp->ntemps) + break; + } + + /* Cell volts */ + i = 0; + for (id = 0x107; id < 0x111; id++) { + if (jbd_can_get_crc(s, id, data, 8)) + return 1; + pp->cellvolt[i++] = (float)_getshort(&data[0]) / 1000; + if (i >= pp->cells) + break; + pp->cellvolt[i++] = (float)_getshort(&data[2]) / 1000; + if (i >= pp->cells) + break; + pp->cellvolt[i++] = (float)_getshort(&data[4]) / 1000; + if (i >= pp->cells) + break; + } + + return 0; +} +#endif + +int jbd_rw(jbd_session_t *s, uint8_t action, uint8_t reg, uint8_t *data, + int datasz) { + uint8_t cmd[256], buf[256]; + int cmdlen, bytes, retries; + + dprintf(5, "action: %x, reg: %x, data: %p, datasz: %d\n", action, reg, data, + datasz); + cmdlen = jbd_cmd(cmd, sizeof(cmd), action, reg, data, datasz); + if (debug >= 5) + bindump("cmd", cmd, cmdlen); + + /* Read the data */ + retries = 5; + while (1) { + dprintf(5, "retries: %d\n", retries); + if (!retries--) { + dprintf(5, "returning: -1\n"); + return -1; + } + dprintf(5, "writing...\n"); + bytes = s->tp->write(s->tp_handle, cmd, cmdlen); + dprintf(5, "bytes: %d\n", bytes); + memset(data, 0, datasz); + bytes = s->tp->read(s->tp_handle, buf, sizeof(buf)); + dprintf(5, "bytes: %d\n", bytes); + if (bytes < 0) + return -1; + if (!jbd_verify(buf, bytes, reg)) + break; + sleep(1); + } + memcpy(data, &buf[4], buf[3]); + dprintf(5, "returning: %d\n", buf[3]); + return buf[3]; +} + +static int jbd_get_pack(jbd_session_t *s) { + mybmm_pack_t *pp = s->pp; + uint8_t data[128]; + int i, bytes; + ; + uint8_t fetbits; + struct jbd_protect prot; + + dprintf(3, "getting HWINFO...\n"); + if ((bytes = jbd_rw(s, JBD_CMD_READ, JBD_CMD_HWINFO, data, sizeof(data))) < + 0) { + dprintf(1, "returning 1!\n"); + return 1; + } + + pp->voltage = (float)_getshort(&data[0]) / 100.0; + pp->current = (float)_getshort(&data[2]) / 100.0; + pp->capacity = (float)_getshort(&data[6]) / 100.0; + dprintf(2, "voltage: %.2f\n", pp->voltage); + dprintf(2, "current: %.2f\n", pp->current); + dprintf(2, "capacity: %.2f\n", pp->capacity); + + /* Balance */ + pp->balancebits = _getshort(&data[12]); + pp->balancebits |= _getshort(&data[14]) << 16; +#ifdef DEBUG + { + char bits[40]; + unsigned short mask = 1; + i = 0; + while (mask) { + bits[i++] = ((pp->balancebits & mask) != 0 ? '1' : '0'); + mask <<= 1; + } + bits[i] = 0; + dprintf(5, "balancebits: %s\n", bits); + } +#endif + + /* Protectbits */ + jbd_get_protect(&prot, _getshort(&data[16])); + + fetbits = data[20]; + dprintf(2, "fetbits: %02x\n", fetbits); + if (fetbits & JBD_MOS_CHARGE) + mybmm_set_state(pp, MYBMM_PACK_STATE_CHARGING); + else + mybmm_clear_state(pp, MYBMM_PACK_STATE_CHARGING); + if (fetbits & JBD_MOS_DISCHARGE) + mybmm_set_state(pp, MYBMM_PACK_STATE_DISCHARGING); + else + mybmm_set_state(pp, MYBMM_PACK_STATE_DISCHARGING); + if (s->balancing) + mybmm_set_state(pp, MYBMM_PACK_STATE_BALANCING); + else + mybmm_clear_state(pp, MYBMM_PACK_STATE_BALANCING); + + pp->cells = data[21]; + dprintf(2, "cells: %d\n", pp->cells); + pp->ntemps = data[22]; + dprintf(2, "ntemps: %d\n", pp->ntemps); + + /* Temps */ +#define CTEMP(v) ((v - 2731) / 10) + for (i = 0; i < pp->ntemps; i++) { + pp->temps[i] = CTEMP((float)_getshort(&data[23 + (i * 2)])); + } + + /* Cell volts */ + if ((bytes = jbd_rw(s, JBD_CMD_READ, JBD_CMD_CELLINFO, data, sizeof(data))) < + 0) + return 1; + + for (i = 0; i < pp->cells; i++) { + pp->cellvolt[i] = (float)_getshort(&data[i * 2]) / 1000; + } + + return 0; +} + +static int jbd_init(mybmm_config_t *conf) { +#if 0 + struct cfg_proctab jbdconf[] = { + CFG_PROCTAB_END + }; + + cfg_get_tab(conf->cfg,jbdconf); + if (debug >= 3) cfg_disp_tab(jbdconf,"",1); +#endif + return 0; +} + +static void *jbd_new(mybmm_config_t *conf, ...) { + jbd_session_t *s; + va_list ap; + mybmm_pack_t *pp; + mybmm_module_t *tp; + + va_start(ap, conf); + pp = va_arg(ap, mybmm_pack_t *); + tp = va_arg(ap, mybmm_module_t *); + va_end(ap); + + dprintf(3, "transport: %s\n", pp->transport); + s = calloc(1, sizeof(*s)); + if (!s) { + perror("jbd_new: malloc"); + return 0; + } + /* Save a copy of the pack */ + s->pp = pp; + s->tp = tp; + dprintf(1, "pp->target: %s, pp->opts: %s\n", pp->target, pp->opts); + s->tp_handle = tp->new (conf, pp->target, pp->opts); + if (!s->tp_handle) { + free(s); + return 0; + } + return s; +} + +static int jbd_open(void *handle) { + jbd_session_t *s = handle; + dprintf(1, "opening...\n"); + return s->tp->open(s->tp_handle); +} + +static int jbd_read(void *handle, ...) { + jbd_session_t *s = handle; + int r; + va_list ap; + + va_start(ap, handle); + va_end(ap); + dprintf(5, "transport: %s\n", s->tp->name); +#ifndef __WIN32 + if (strncmp(s->tp->name, "can", 3) == 0) + r = jbd_can_get_pack(s); + else +#endif + r = jbd_get_pack(s); + return r; +} + +static int jbd_close(void *handle) { + jbd_session_t *s = handle; + return s->tp->close(s->tp_handle); +} + +static int jbd_control(void *handle, ...) { + // jbd_session_t *s = handle; + va_list ap; + int op, action; + + va_start(ap, handle); + op = va_arg(ap, int); + dprintf(1, "op: %d\n", op); + switch (op) { + case MYBMM_CHARGE_CONTROL: + action = va_arg(ap, int); + dprintf(1, "action: %d\n", action); + break; + } + va_end(ap); + return 0; +} + +mybmm_module_t jbd_module = { + MYBMM_MODTYPE_CELLMON, + "jbd", + MYBMM_CHARGE_CONTROL | MYBMM_DISCHARGE_CONTROL | MYBMM_BALANCE_CONTROL, + jbd_init, /* Init */ + jbd_new, /* New */ + jbd_open, /* Open */ + jbd_read, /* Read */ + 0, /* Write */ + jbd_close, /* Close */ + 0, /* Free */ + 0, /* Shutdown */ + jbd_control, /* Control */ + 0, /* Config */ +}; diff --git a/sensors/bms/packages/jbdtool/jbd.h b/sensors/bms/packages/jbdtool/jbd.h new file mode 100644 index 00000000..6e687e93 --- /dev/null +++ b/sensors/bms/packages/jbdtool/jbd.h @@ -0,0 +1,40 @@ + +#ifndef __JBD_H__ +#define __JBD_H__ + +#include "module.h" + +struct jbd_session { + mybmm_module_t *tp; /* Our transport */ + void *tp_handle; /* Our transport handle */ + mybmm_pack_t *pp; /* Our pack info */ + int balancing; /* Balance_en flag set in eeprom */ +}; +typedef struct jbd_session jbd_session_t; + +/* Used by both jbdtool and mybmm */ +struct jbd_protect { + unsigned sover : 1; /* Single overvoltage protection */ + unsigned sunder : 1; /* Single undervoltage protection */ + unsigned gover : 1; /* Whole group overvoltage protection */ + unsigned gunder : 1; /* Whole group undervoltage protection */ + unsigned chitemp : 1; /* Charge over temperature protection */ + unsigned clowtemp : 1; /* Charge low temperature protection */ + unsigned dhitemp : 1; /* Discharge over temperature protection */ + unsigned dlowtemp : 1; /* Discharge low temperature protection */ + unsigned cover : 1; /* Charge overcurrent protection */ + unsigned cunder : 1; /* Discharge overcurrent protection */ + unsigned shorted : 1; /* Short circuit protection */ + unsigned ic : 1; /* Front detection IC error */ + unsigned mos : 1; /* Software lock MOS */ +}; + +int jbd_rw(jbd_session_t *, uint8_t action, uint8_t reg, uint8_t *data, + int datasz); +void jbd_get_protect(struct jbd_protect *p, unsigned short bits); +int jbd_can_get_crc(jbd_session_t *s, int id, unsigned char *data, int len); +int jbd_can_get(jbd_session_t *s, int id, unsigned char *data, int datalen, + int chk); +int jbd_reset(jbd_session_t *s); + +#endif diff --git a/sensors/bms/packages/jbdtool/jbd_info.c b/sensors/bms/packages/jbdtool/jbd_info.c new file mode 100644 index 00000000..1144ad72 --- /dev/null +++ b/sensors/bms/packages/jbdtool/jbd_info.c @@ -0,0 +1,350 @@ + +/* +Copyright (c) 2021, Stephen P. Shoecraft +All rights reserved. + +This source code is licensed under the BSD-style license found in the +LICENSE file in the root directory of this source tree. +*/ + +#include "jbd_info.h" +#include "debug.h" +#include +#include + +#define _getshort(p) ((short)((*((p)) << 8) | *((p) + 1))) +#define _putshort(p, v) \ + { \ + float tmp; \ + *((p)) = ((int)(tmp = v) >> 8); \ + *((p + 1)) = (int)(tmp = v); \ + } + +int jbd_eeprom_start(jbd_session_t *s) { + uint8_t payload[2] = {0x56, 0x78}; + + dprintf(2, "opening eemprom...\n"); + return jbd_rw(s, JBD_CMD_WRITE, JBD_REG_EEPROM, payload, sizeof(payload)); +} + +int jbd_eeprom_end(jbd_session_t *s) { + uint8_t payload[2] = {0x00, 0x00}; + + dprintf(2, "closing eemprom...\n"); + return jbd_rw(s, JBD_CMD_WRITE, JBD_REG_CONFIG, payload, sizeof(payload)); +} + +int jbd_set_mosfet(jbd_session_t *s, int val) { + uint8_t payload[2]; + int r; + + dprintf(2, "val: %x\n", val); + _putshort(payload, val); + if (jbd_eeprom_start(s) < 0) + return 1; + r = jbd_rw(s, JBD_CMD_WRITE, JBD_REG_MOSFET, payload, sizeof(payload)); + if (jbd_eeprom_end(s) < 0) + return 1; + return (r < 0 ? 1 : 0); +} + +int jbd_reset(jbd_session_t *s) { + uint8_t payload[2]; + int r; + +#if 0 +2021-06-05 16:33:25,846 INFO client 192.168.1.7:45868 -> server pack_01:23 (9 bytes) +-> 0000 DD 5A E3 02 43 21 FE B7 77 .Z..C!..w +2021-06-05 16:33:26,032 INFO client 192.168.1.7:45868 <= server pack_01:23 (7 bytes) +<= 0000 DD E3 80 00 FF 80 77 ......w +2021-06-05 19:34:58,910 INFO client 192.168.1.7:39154 -> server pack_01:23 (9 bytes) +-> 0000 DD 5A 0A 02 18 81 FF 5B 77 .Z.....[w +2021-06-05 19:34:59,150 INFO client 192.168.1.7:39154 <= server pack_01:23 (7 bytes) +<= 0000 DD 0A 80 00 FF 80 77 ......w +#endif + + _putshort(payload, 0x4321); + r = jbd_rw(s, JBD_CMD_WRITE, JBD_REG_RESET, payload, sizeof(payload)); + if (!r) { + _putshort(payload, 0x1881); + r = jbd_rw(s, JBD_CMD_WRITE, JBD_REG_FRESET, payload, sizeof(payload)); + } + return (r < 0 ? 1 : 0); +} + +#ifndef __WIN32 +/* For CAN bus only */ +int jbd_can_get_info(jbd_session_t *s, jbd_info_t *info) { + unsigned char data[8]; + unsigned short mfgdate; + int day, mon, year, id, i; + + if (jbd_can_get_crc(s, 0x100, data, sizeof(data))) + return 1; + info->voltage = (float)_getshort(&data[0]) / 100.0; + info->current = (float)_getshort(&data[2]) / 100.0; + info->capacity = (float)_getshort(&data[4]) / 100.0; + dprintf(1, "voltage: %.2f\n", info->voltage); + dprintf(1, "current: %.2f\n", info->current); + dprintf(1, "capacity: %.2f\n", info->capacity); + + if (jbd_can_get_crc(s, 0x101, data, 8)) + return 1; + info->fullcap = (float)_getshort(&data[0]) / 100.0; + info->cycles = _getshort(&data[2]); + info->pctcap = _getshort(&data[4]); + dprintf(1, "fullcap: %.2f\n", info->fullcap); + dprintf(1, "cycles: %d\n", info->cycles); + dprintf(1, "pctcap: %d\n", info->pctcap); + + /* 1-16 */ + if (jbd_can_get_crc(s, 0x102, data, 8)) + return 1; + info->balancebits = _getshort(&data[0]); + /* 17 - 33 */ + info->balancebits |= _getshort(&data[2]) << 16; +#ifdef DEBUG + { + char bits[40]; + unsigned short mask = 1; + i = 0; + while (mask) { + bits[i++] = ((info->balancebits & mask) != 0 ? '1' : '0'); + mask <<= 1; + } + bits[i] = 0; + dprintf(1, "balance1: %s\n", bits); + } +#endif + + /* protection */ + info->protectbits = _getshort(&data[4]); +#ifdef DEBUG + { + char bits[40]; + unsigned short mask = 1; + i = 0; + while (mask) { + bits[i++] = ((info->protectbits & mask) != 0 ? '1' : '0'); + mask <<= 1; + } + bits[i] = 0; + dprintf(1, "protect: %s\n", bits); + } +#endif + + if (jbd_can_get_crc(s, 0x103, data, 8)) + return 1; + info->fetstate = _getshort(&data[0]); + mfgdate = _getshort(&data[2]); + day = mfgdate & 0x1f; + mon = (mfgdate >> 5) & 0x0f; + year = 2000 + (mfgdate >> 9); + dprintf(3, "year: %d, mon: %d, day: %d\n", year, mon, day); + sprintf(info->mfgdate, "%04d%02d%02d", year, mon, day); + info->version = (float)_getshort(&data[4]) / 10.0; + dprintf(1, "fetstate: %d\n", info->fetstate); + dprintf(1, "mfgdate: %s\n", info->mfgdate); + dprintf(1, "version: %.1f\n", info->version); + + if (jbd_can_get_crc(s, 0x104, data, 8)) + return 1; + info->strings = data[0]; + info->probes = data[1]; + dprintf(1, "strings: %d\n", info->strings); + dprintf(1, "probes: %d\n", info->probes); + + /* Temps */ + i = 0; +#define CTEMP(v) ((v - 2731) / 10) + for (id = 0x105; id < 0x107; id++) { + if (jbd_can_get_crc(s, id, data, 8)) + return 1; + info->temps[i++] = CTEMP((float)_getshort(&data[0])); + if (i >= info->probes) + break; + info->temps[i++] = CTEMP((float)_getshort(&data[2])); + if (i >= info->probes) + break; + info->temps[i++] = CTEMP((float)_getshort(&data[4])); + if (i >= info->probes) + break; + } + +#define FTEMP(v) ((((float)(v)*9.0) / 5.0) + 32.0) + for (i = 0; i < info->probes; i++) { + dprintf(1, "temp %d: %.3f\n", i, FTEMP(info->temps[i])); + } + + /* Cell volts */ + i = 0; + for (id = 0x107; id < 0x111; id++) { + if (jbd_can_get_crc(s, id, data, 8)) + return 1; + info->cellvolt[i++] = (float)_getshort(&data[0]) / 1000; + if (i >= info->strings) + break; + info->cellvolt[i++] = (float)_getshort(&data[2]) / 1000; + if (i >= info->strings) + break; + info->cellvolt[i++] = (float)_getshort(&data[4]) / 1000; + if (i >= info->strings) + break; + } + + if (jbd_can_get(s, 0x111, data, 8, 0)) + return 1; + memcpy(&info->model[0], data, 8); + if (jbd_can_get(s, 0x112, data, 8, 0)) + return 1; + memcpy(&info->model[8], data, 8); + if (jbd_can_get(s, 0x113, data, 8, 0)) + return 1; + memcpy(&info->model[16], data, 8); + if (jbd_can_get(s, 0x114, data, 8, 0)) + return 1; + memcpy(&info->model[24], data, 8); + dprintf(1, "model: %s\n", info->model); + return 0; +} +#endif + +static int jbd_std_get_info(jbd_session_t *s, jbd_info_t *info) { + unsigned char data[256]; + unsigned short mfgdate; + int day, mon, year, i; + + if (jbd_rw(s, JBD_CMD_READ, JBD_CMD_HWINFO, data, sizeof(data)) < 0) + return 1; + + info->voltage = (float)_getshort(&data[0]) / 100.0; + info->current = (float)_getshort(&data[2]) / 100.0; + info->capacity = (float)_getshort(&data[4]) / 100.0; + dprintf(1, "voltage: %.2f\n", info->voltage); + dprintf(1, "current: %.2f\n", info->current); + dprintf(1, "capacity: %.2f\n", info->capacity); + + info->fullcap = (float)_getshort(&data[6]) / 100.0; + info->cycles = _getshort(&data[8]); + info->pctcap = data[19]; + dprintf(1, "fullcap: %.2f\n", info->fullcap); + dprintf(1, "cycles: %d\n", info->cycles); + dprintf(1, "pctcap: %x\n", info->pctcap); + + /* 1-16 */ + info->balancebits = _getshort(&data[12]); + info->balancebits |= _getshort(&data[14]) << 16; +#ifdef DEBUG + { + char bits[40]; + unsigned short mask = 1; + i = 0; + while (mask) { + bits[i++] = ((info->balancebits & mask) != 0 ? '1' : '0'); + mask <<= 1; + } + bits[i] = 0; + dprintf(1, "balance: %s\n", bits); + } +#endif + + /* protection */ + info->protectbits = _getshort(&data[16]); +#ifdef DEBUG + { + char bits[40]; + unsigned short mask = 1; + i = 0; + while (mask) { + bits[i++] = ((info->protectbits & mask) != 0 ? '1' : '0'); + mask <<= 1; + } + bits[i] = 0; + dprintf(1, "protect: %s\n", bits); + } +#endif + + info->fetstate = data[20]; + mfgdate = _getshort(&data[10]); + day = mfgdate & 0x1f; + mon = (mfgdate >> 5) & 0x0f; + year = 2000 + (mfgdate >> 9); + dprintf(3, "year: %d, mon: %d, day: %d\n", year, mon, day); + sprintf(info->mfgdate, "%04d%02d%02d", year, mon, day); + info->version = data[18] / 10.0; + dprintf(1, "fetstate: %x\n", info->fetstate); + dprintf(1, "mfgdate: %s\n", info->mfgdate); + dprintf(1, "version: %.1f\n", info->version); + + info->strings = data[21]; + info->probes = data[22]; + dprintf(1, "strings: %d\n", info->strings); + dprintf(1, "probes: %d\n", info->probes); + + /* Temps */ +#define CTEMP(v) ((v - 2731) / 10) + for (i = 0; i < info->probes; i++) { + dprintf(1, "temp[%d]: %d\n", i, _getshort(&data[23 + (i * 2)])); + info->temps[i] = CTEMP((float)_getshort(&data[23 + (i * 2)])); + } + +#define FTEMP(v) ((((float)(v)*9.0) / 5.0) + 32.0) + for (i = 0; i < info->probes; i++) { + dprintf(1, "temp %d: %.3f\n", i, FTEMP((float)info->temps[i] / 100.0)); + } + + /* Cell volts */ + if (jbd_rw(s, JBD_CMD_READ, JBD_CMD_CELLINFO, data, sizeof(data)) < 0) + return 1; + + for (i = 0; i < info->strings; i++) { + info->cellvolt[i] = (float)_getshort(&data[i * 2]) / 1000; + } + + /* HWVER */ + i = jbd_rw(s, JBD_CMD_READ, JBD_CMD_HWVER, (uint8_t *)info->model, + sizeof(info->model)); + if (i < 0) + return 1; + info->model[i] = 0; + + return 0; +} + +int jbd_get_info(jbd_session_t *s, jbd_info_t *info) { + int r, i; + + dprintf(1, "transport: %s\n", s->tp->name); +#ifndef __WIN32 + if (strncmp(s->tp->name, "can", 3) == 0) + r = jbd_can_get_info(s, info); + else +#endif + r = jbd_std_get_info(s, info); + if (r != 0) + return r; + dprintf(1, "r: %d\n", r); + + /* Fill in the protect struct */ + jbd_get_protect(&info->protect, info->protectbits); + + info->cell_min = 4.2; + info->cell_max = 0.0; + info->cell_total = 0; + for (i = 0; i < info->strings; i++) { + dprintf(1, "cell %d: %.3f\n", i, info->cellvolt[i]); + if (info->cellvolt[i] < info->cell_min) + info->cell_min = info->cellvolt[i]; + if (info->cellvolt[i] > info->cell_max) + info->cell_max = info->cellvolt[i]; + info->cell_total += info->cellvolt[i]; + } + info->cell_diff = info->cell_max - info->cell_min; + info->cell_avg = info->cell_total / info->strings; + dprintf(1, + "cells: total: %.3f, min: %.3f, max: %.3f, diff: %.3f, avg: %.3f\n", + info->cell_total, info->cell_min, info->cell_max, info->cell_diff, + info->cell_avg); + + return 0; +} diff --git a/sensors/bms/packages/jbdtool/jbd_info.h b/sensors/bms/packages/jbdtool/jbd_info.h new file mode 100644 index 00000000..59242b44 --- /dev/null +++ b/sensors/bms/packages/jbdtool/jbd_info.h @@ -0,0 +1,187 @@ + +#ifndef __JBD_INFO_H__ +#define __JBD_INFO_H__ + +#include "jbd.h" +#include + +/* This is for jbdtool */ +struct jbd_info { + char name[32]; + float voltage; /* the total voltage */ + float current; /* the current, charging is positive, the discharge is negative + */ + float capacity; /* the remaining capacity */ + float fullcap; /* full capacity */ + unsigned short + cycles; /* the number of discharge cycle , unsigned , the unit is 1 */ + unsigned short + pctcap; /* the remaining capacity (RSOC), unsigned, in units of % */ + unsigned long balancebits; + /* the protection sign */ + unsigned short protectbits; + struct jbd_protect protect; + unsigned short fetstate; /* for the MOS tube status */ + char mfgdate[9]; /* the production date, YYYYMMDD, zero terminated */ + float version; /* the software version */ + unsigned char strings; /* the number of battery strings */ + float cellvolt[32]; /* Cell voltages */ + float cell_total; /* sum of all cells */ + float cell_min; /* lowest cell value */ + float cell_max; /* highest cell value */ + float cell_diff; /* diff from lowest to highest */ + float cell_avg; /* avergae cell value */ + unsigned char probes; /* the number of NTC (temp) probes */ + float temps[6]; /* Temps */ + char model[32]; /* Model name */ +}; +typedef struct jbd_info jbd_info_t; + +#define JBD_PKT_START 0xDD +#define JBD_PKT_END 0x77 +#define JBD_CMD_READ 0xA5 +#define JBD_CMD_WRITE 0x5A + +#define JBD_CMD_HWINFO 0x03 +#define JBD_CMD_CELLINFO 0x04 +#define JBD_CMD_HWVER 0x05 +#define JBD_CMD_MOS 0xE1 + +#define JBD_REG_EEPROM 0x00 +#define JBD_REG_CONFIG 0x01 + +#define JBD_FC 3838 +#define JBD_REG_FRESET 0x0A +#define JBD_REG_DCAP 0x10 +#define JBD_REG_CCAP 0x11 +#define JBD_REG_FULL 0x12 +#define JBD_REG_EMPTY 0x13 +#define JBD_REG_RATE 0x14 +#define JBD_REG_MFGDATE 0x15 +#define JBD_REG_SERIAL 0x16 +#define JBD_REG_CYCLE 0x17 +#define JBD_REG_CHGOT 0x18 +#define JBD_REG_RCHGOT 0x19 +#define JBD_REG_CHGUT 0x1A +#define JBD_REG_RCHGUT 0x1B +#define JBD_REG_DSGOT 0x1C +#define JBD_REG_RDSGOT 0x1D +#define JBD_REG_DSGUT 0x1E +#define JBD_REG_RDSGUT 0x1F +#define JBD_REG_POVP 0x20 +#define JBD_REG_RPOVP 0x21 +#define JBD_REG_PUVP 0x22 +#define JBD_REG_RPUVP 0x23 +#define JBD_REG_COVP 0x24 +#define JBD_REG_RCOVP 0x25 +#define JBD_REG_CUVP 0x26 +#define JBD_REG_RCUVP 0x27 +#define JBD_REG_CHGOC 0x28 +#define JBD_REG_DSGOC 0x29 +#define JBD_REG_BALVOL 0x2A +#define JBD_REG_BALPREC 0x2B +#define JBD_REG_CURRES 0x2C +#define JBD_REG_FUNCMASK 0x2D +#define JBD_REG_NTCMASK 0x2E +#define JBD_REG_STRINGS 0x2F +#define JBD_REG_FETTIME 0x30 +#define JBD_REG_LEDTIME 0x31 +#define JBD_REG_VOLCAP80 0x32 +#define JBD_REG_VOLCAP60 0x33 +#define JBD_REG_VOLCAP40 0x34 +#define JBD_REG_VOLCAP20 0x35 +#define JBD_REG_HCOVP 0x36 /* HardCellOverVoltage */ +#define JBD_REG_HCUVP 0x37 /* HardCellUnderVoltage */ +#define JBD_REG_HCOC 0x38 /* HardChg/DsgOverCurrent */ +#define JBD_REG_HTRT 0x39 /* HardTime/SCReleaseTime */ +#define JBD_REG_CHGDELAY 0x3A /* low: O, high: U */ +#define JBD_REG_DSGDELAY 0x3B /* low: O, high: U */ +#define JBD_REG_PVDELAY 0x3C /* low: O, high: U */ +#define JBD_REG_CVDELAY 0x3D /* low: O, high: U */ +#define JBD_REG_CHGOCDELAY 0x3E /* low: release, high: delay */ +#define JBD_REG_DSGOCDELAY 0x3F /* low: release, high: delay */ +#define JBD_REG_GPSOFF 0x40 +#define JBD_REG_GPSOFFTIME 0x41 +#define JBD_REG_VOLCAP90 0x42 +#define JBD_REG_VOLCAP70 0x43 +#define JBD_REG_VOLCAP50 0x44 +#define JBD_REG_VOLCAP30 0x45 +#define JBD_REG_VOLCAP10 0x46 +#define JBD_REG_VOLCAP10 0x46 +#define JBD_REG_VOLCAP100 0x47 + +#define JBD_REG_MFGNAME 0xA0 +#define JBD_REG_MODEL 0xA1 +#define JBD_REG_BARCODE 0xA2 +#define JBD_REG_ERROR 0xAA + +/* Cell Voltage Calibration */ +#define JBD_REG_VCAL01 0xB0 +#define JBD_REG_VCAL02 0xB1 +#define JBD_REG_VCAL03 0xB2 +#define JBD_REG_VCAL04 0xB3 +#define JBD_REG_VCAL05 0xB4 +#define JBD_REG_VCAL06 0xB5 +#define JBD_REG_VCAL07 0xB6 +#define JBD_REG_VCAL08 0xB7 +#define JBD_REG_VCAL09 0xB8 +#define JBD_REG_VCAL10 0xB9 +#define JBD_REG_VCAL11 0xBA +#define JBD_REG_VCAL12 0xBB +#define JBD_REG_VCAL13 0xBC +#define JBD_REG_VCAL14 0xBD +#define JBD_REG_VCAL15 0xBE +#define JBD_REG_VCAL16 0xBF +#define JBD_REG_VCAL17 0xC0 +#define JBD_REG_VCAL18 0xC1 +#define JBD_REG_VCAL19 0xC2 +#define JBD_REG_VCAL20 0xC3 +#define JBD_REG_VCAL21 0xC4 +#define JBD_REG_VCAL22 0xC5 +#define JBD_REG_VCAL23 0xC6 +#define JBD_REG_VCAL24 0xC7 +#define JBD_REG_VCAL25 0xC8 +#define JBD_REG_VCAL26 0xC9 +#define JBD_REG_VCAL27 0xCA +#define JBD_REG_VCAL28 0xCB +#define JBD_REG_VCAL29 0xCC +#define JBD_REG_VCAL30 0xCD +#define JBD_REG_VCAL31 0xCE +#define JBD_REG_VCAL32 0xCF +#define JBD_REG_VCAL33 0xCF + +/* NTC Calibration */ +#define JBD_REG_TCAL00 0xD0 +#define JBD_REG_TCAL01 0xD1 +#define JBD_REG_TCAL02 0xD2 +#define JBD_REG_TCAL03 0xD3 +#define JBD_REG_TCAL04 0xD4 +#define JBD_REG_TCAL05 0xD5 +#define JBD_REG_TCAL06 0xD6 +#define JBD_REG_TCAL07 0xD7 + +#define JBD_REG_CAPACITY 0xE0 +#define JBD_REG_MOSFET 0xE1 +#define JBD_REG_BALANCE 0xE2 +#define JBD_REG_RESET 0xE3 +#define JBD_REG_ICURCAL 0xAD +#define JBD_REG_CCURCAL 0xAE +#define JBD_REG_DCURCAL 0xAF + +#define JBD_MOS_CHARGE 0x01 +#define JBD_MOS_DISCHARGE 0x02 + +int jbd_get_info(jbd_session_t *, jbd_info_t *); +int jbd_eeprom_start(jbd_session_t *); +int jbd_eeprom_end(jbd_session_t *); +int jbd_set_mosfet(jbd_session_t *, int); + +#define _getshort(p) ((short)((*((p)) << 8) | *((p) + 1))) +#define _putshort(p, v) \ + { \ + float tmp; \ + *((p)) = ((int)(tmp = v) >> 8); \ + *((p + 1)) = (int)(tmp = v); \ + } + +#endif diff --git a/sensors/bms/packages/jbdtool/list.c b/sensors/bms/packages/jbdtool/list.c new file mode 100644 index 00000000..f9329a2d --- /dev/null +++ b/sensors/bms/packages/jbdtool/list.c @@ -0,0 +1,379 @@ + +/* +Copyright (c) 2021, Stephen P. Shoecraft +All rights reserved. + +This source code is licensed under the BSD-style license found in the +LICENSE file in the root directory of this source tree. +*/ + +#include "list.h" +#include +#include +#include +#include + +#ifdef DEBUG +#undef DEBUG +#endif +//#define DEBUG 1 + +#ifdef DEBUG +#undef DPRINTF +#define DPRINTF(format, args...) \ + printf("%s(%d): " format, __FUNCTION__, __LINE__, ##args) +#else +#define DPRINTF(format, args...) /* noop */ +#endif + +#define _dump(i) \ + DPRINTF("%s: item: %p, prev: %p, next: %p\n", #i, i, i->prev, i->next); +void list_checkitem(list_item ip1) { + _dump(ip1); + assert(ip1->prev != ip1); + assert(ip1->next != ip1); + if (ip1->prev) { + _dump(ip1->prev); + assert(ip1->prev->prev != ip1); + assert(ip1->prev->next == ip1); + } + if (ip1->next) { + _dump(ip1->next); + assert(ip1->next->prev == ip1); + assert(ip1->next->next != ip1); + } +} + +static list_item _newitem(void *item, int size) { + list_item new_item; + +#if DEBUG + printf("_newitem: item: %p, size: %d\n", item, size); +#endif + new_item = (list_item)malloc(LIST_ITEM_SIZE); + if (!new_item) + return 0; + + if (size) { + new_item->item = (void *)malloc((size_t)size); + if (!new_item->item) { + free(new_item); +#if DEBUG + printf("_newitem: returning: 0"); +#endif + return 0; + } + memcpy(new_item->item, item, size); + } else { + new_item->item = item; + } + + new_item->size = size; + new_item->prev = (list_item)0; + new_item->next = (list_item)0; + +#if DEBUG + printf("_newitem: returning: %p\n", new_item); +#endif + return new_item; +} + +void *list_add(list lp, void *item, int size) { + list_item ip, new_item; + +#if DEBUG + printf("\n\n*********************\n"); + printf("list_add: lp: %p, item: %p, size: %d\n", lp, item, size); +#endif + if (!lp) + return 0; + + /* Create a new item */ + new_item = _newitem(item, size); + if (!new_item) + return 0; + +#ifdef THREAD_SAFE + pthread_mutex_lock(&lp->mutex); +#endif + /* Add it to the list */ +#if DEBUG + printf("ip->first: %p\n", lp->first); +#endif + if (!lp->first) + lp->first = lp->last = lp->next = new_item; + else { + ip = lp->last; /* Get last item */ + ip->next = new_item; /* Add this one */ + new_item->prev = ip; /* Point to it */ + lp->last = new_item; /* Make this last */ + } +#ifdef THREAD_SAFE + pthread_mutex_unlock(&lp->mutex); +#endif + return new_item->item; +} + +int list_add_list(list lp, list lp2) { + list_item ip; + + if (!lp) + return 1; + +#ifdef THREAD_SAFE + pthread_mutex_lock(&lp->mutex); + pthread_mutex_lock(&lp2->mutex); +#endif + + /* Get each item from the old list and copy it to the new one */ + ip = lp2->first; + while (ip) { + list_add(lp, ip->item, ip->size); + ip = ip->next; + } + +#ifdef THREAD_SAFE + pthread_mutex_unlock(&lp->mutex); + pthread_mutex_unlock(&lp2->mutex); +#endif + + /* Return the new list */ + return 0; +} + +int list_count(list lp) { + list_item ip; + register int count; + + if (!lp) + return -1; + +#ifdef THREAD_SAFE + pthread_mutex_lock(&lp->mutex); +#endif + count = 0; + for (ip = lp->first; ip; ip = ip->next) + count++; + +#ifdef THREAD_SAFE + pthread_mutex_unlock(&lp->mutex); +#endif + return count; +} + +list list_create(void) { + list lp; + + lp = (list)malloc(LIST_SIZE); +#if DEBUG + printf("list_create: lp: %p\n", lp); +#endif + if (!lp) + return 0; + + lp->first = lp->last = lp->next = (list_item)0; + +#ifdef THREAD_SAFE + pthread_mutex_init(&lp->mutex, NULL); +#endif + +#if DEBUG + printf("list_create: returning: %p\n", lp); +#endif + return lp; +} + +int list_delete(list lp, void *item) { + list_item ip, prev, next; + + if (!lp) + return -1; + +#ifdef THREAD_SAFE + pthread_mutex_lock(&lp->mutex); +#endif + + ip = lp->first; + while (ip) { + if (item == ip->item) { + prev = ip->prev; /* Get the pointers */ + next = ip->next; + + /* Fixup links in other first */ + if (prev) + prev->next = next; + if (next) + next->prev = prev; + + /* Was this the 1st item? */ + if (ip == lp->first) + lp->first = next; + + /* Was this the last item? */ + if (ip == lp->last) + lp->last = prev; + + /* Was this the next item? */ + if (ip == lp->next) + lp->next = next; + + free(item); /* Free the item */ + free(ip); /* Free the ptr */ +#ifdef THREAD_SAFE + pthread_mutex_unlock(&lp->mutex); +#endif + return 0; /* Item found and deleted */ + } + ip = ip->next; /* Next item, please. */ + } +#ifdef THREAD_SAFE + pthread_mutex_unlock(&lp->mutex); +#endif + return 1; /* Item not found, error. */ +} + +int list_destroy(list lp) { + list_item ip, next; + + if (!lp) + return -1; + + ip = lp->first; /* Start at beginning */ + while (ip) { + if (ip->size) + free(ip->item); /* Free the item */ + next = ip->next; /* Get next pointer */ + free(ip); /* Free current item */ + ip = next; /* Set current item to next */ + } + free(lp); /* Free list */ + return 0; +} + +list list_dup(list lp) { + list new_list; + list_item ip; + + if (!lp) + return 0; + + /* Create a new list */ + new_list = list_create(); + if (!new_list) + return 0; + +#ifdef THREAD_SAFE + pthread_mutex_lock(&lp->mutex); +#endif + + /* Get each item from the old list and copy it to the new one */ + ip = lp->first; + while (ip) { + list_add(new_list, ip->item, ip->size); + ip = ip->next; + } + +#ifdef THREAD_SAFE + pthread_mutex_unlock(&lp->mutex); +#endif + + /* Return the new list */ + return new_list; +} + +void *list_get_next(list lp) { + list_item ip; + void *item; + + if (!lp) + return 0; + +#ifdef THREAD_SAFE + pthread_mutex_lock(&lp->mutex); +#endif + + item = 0; + if (lp->next) { + ip = lp->next; + lp->next = ip->next; + item = ip->item; + } + +#ifdef THREAD_SAFE + pthread_mutex_unlock(&lp->mutex); +#endif + return item; +} + +int list_is_next(list lp) { + if (!lp) + return -1; + return (lp->next ? 1 : 0); +} + +int list_reset(list lp) { + + if (!lp) + return -1; + + lp->next = lp->first; + return 0; +} + +static int _compare(list_item item1, list_item item2) { + register int val; + + val = strcmp(item1->item, item2->item); + if (val < 0) + val = -1; + else if (val > 0) + val = 1; + return val; +} + +int list_sort(list lp, list_compare compare, int order) { + int comp, swap, save_size; + list_item save_item; + register list_item ip1, ip2; + + if (!lp) + return -1; + +#ifdef THREAD_SAFE + pthread_mutex_lock(&lp->mutex); +#endif + + /* If compare is null, use internal compare (strcmp) */ + if (!compare) { + compare = _compare; + } + + /* Set comp negative or positive, depending upon order */ + comp = (order == 0 ? 1 : -1); + + /* Sort the list */ + for (ip1 = lp->first; ip1;) { + swap = 0; + for (ip2 = ip1->next; ip2; ip2 = ip2->next) { + if (compare(ip1, ip2) == comp) { + swap = 1; + break; + } + } + DPRINTF("swap: %d\n", swap); + if (swap) { + /* LOL dont swap the list_items ... just swap the items */ + save_item = ip1->item; + save_size = ip1->size; + ip1->item = ip2->item; + ip1->size = ip2->size; + ip2->item = save_item; + ip2->size = save_size; + } else + ip1 = ip1->next; + } +#ifdef THREAD_SAFE + pthread_mutex_unlock(&lp->mutex); +#endif + return 0; +} diff --git a/sensors/bms/packages/jbdtool/list.h b/sensors/bms/packages/jbdtool/list.h new file mode 100644 index 00000000..9d5035cb --- /dev/null +++ b/sensors/bms/packages/jbdtool/list.h @@ -0,0 +1,52 @@ +#ifndef __LIST_H +#define __LIST_H + +#ifdef THREAD_SAFE +#include +#endif + +/* Define the list item */ +struct _list_item { + void *item; + int size; + struct _list_item *prev; + struct _list_item *next; +}; +typedef struct _list_item *list_item; +#define LIST_ITEM_SIZE sizeof(struct _list_item) + +/* Define the list */ +struct _list { + int type; /* Data type in list */ + list_item first; /* First item in list */ + list_item last; /* Last item in list */ + list_item next; /* Next item in list */ +#ifdef THREAD_SAFE + pthread_mutex_t mutex; +#endif +}; +typedef struct _list *list; +#define LIST_SIZE sizeof(struct _list) + +#ifdef __cplusplus +extern "C" { +#endif +/* Define the list functions */ +list list_create(void); +int list_destroy(list); +list list_dup(list); +void *list_add(list, void *, int); +int list_add_list(list, list); +int list_delete(list, void *); +void *list_get_next(list); +#define list_next list_get_next +int list_reset(list); +int list_count(list); +int list_is_next(list); +typedef int (*list_compare)(list_item, list_item); +int list_sort(list, list_compare, int); +#ifdef __cplusplus +} +#endif + +#endif /* __LIST_H */ diff --git a/sensors/bms/packages/jbdtool/main.c b/sensors/bms/packages/jbdtool/main.c new file mode 100644 index 00000000..62292c4d --- /dev/null +++ b/sensors/bms/packages/jbdtool/main.c @@ -0,0 +1,1376 @@ + +/* +Copyright (c) 2021, Stephen P. Shoecraft +All rights reserved. + +This source code is licensed under the BSD-style license found in the +LICENSE file in the root directory of this source tree. +*/ + +struct mybmm_config; +typedef struct mybmm_config mybmm_config_t; + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifndef __WIN32 +#include +#endif +//#include "util.h" +#include "jbd.h" /* For session info */ +#include "jbd_info.h" /* For info struct */ +#include "mybmm.h" +#include "parson.h" +#ifdef MQTT +#include "mqtt.h" +#endif + +#define VERSION "1.8" +#include "build.h" + +int debug = 0; + +int outfmt = 0; +FILE *outfp; +char sepch; +char *sepstr; + +JSON_Value *root_value; +JSON_Object *root_object; +char *serialized_string = NULL; + +char *trim(char *); + +int dont_interpret = 0; +int flat = 0; + +enum JBD_PARM_DT { + JBD_PARM_DT_UNK, + JBD_PARM_DT_INT, /* Std int/number */ + JBD_PARM_DT_FLOAT, /* floating pt */ + JBD_PARM_DT_STR, /* string */ + JBD_PARM_DT_TEMP, /* temp */ + JBD_PARM_DT_DATE, /* date */ + JBD_PARM_DT_PCT, /* % */ + JBD_PARM_DT_FUNC, /* function bits */ + JBD_PARM_DT_NTC, /* ntc bits */ + JBD_PARM_DT_B0, /* byte 0 */ + JBD_PARM_DT_B1, /* byte 1 */ + JBD_PARM_DT_DOUBLE, + JBD_PARM_DT_SCVAL, + JBD_PARM_DT_SCDELAY, + JBD_PARM_DT_DSGOC2, + JBD_PARM_DT_DSGOC2DELAY, + JBD_PARM_DT_HCOVPDELAY, + JBD_PARM_DT_HCUVPDELAY, + JBD_PARM_DT_DUMP, /* short, ushort, hex */ +}; + +#define JBD_FUNC_SWITCH 0x01 +#define JBD_FUNC_SCRL 0x02 +#define JBD_FUNC_BALANCE_EN 0x04 +#define JBD_FUNC_CHG_BALANCE 0x08 +#define JBD_FUNC_LED_EN 0x10 +#define JBD_FUNC_LED_NUM 0x20 +#define JBD_FUNC_RTC 0x40 +#define JBD_FUNC_EDV 0x80 + +#define JBD_NTC1 0x01 +#define JBD_NTC2 0x02 +#define JBD_NTC3 0x04 +#define JBD_NTC4 0x08 +#define JBD_NTC5 0x10 +#define JBD_NTC6 0x20 +#define JBD_NTC7 0x40 +#define JBD_NTC8 0x80 + +struct jbd_params { + uint8_t reg; + char *label; + int dt; +} params[] = { + // { JBD_FC,"FileCode",0 }, + {JBD_REG_DCAP, "DesignCapacity", JBD_PARM_DT_INT}, + {JBD_REG_CCAP, "CycleCapacity", JBD_PARM_DT_INT}, + {JBD_REG_FULL, "FullChargeVol", JBD_PARM_DT_INT}, + {JBD_REG_EMPTY, "ChargeEndVol", JBD_PARM_DT_INT}, + {JBD_REG_RATE, "DischargingRate", JBD_PARM_DT_PCT}, + {JBD_REG_MFGDATE, "ManufactureDate", JBD_PARM_DT_DATE}, + {JBD_REG_SERIAL, "SerialNumber", JBD_PARM_DT_INT}, + {JBD_REG_CYCLE, "CycleCount", JBD_PARM_DT_INT}, + {JBD_REG_CHGOT, "ChgOverTemp", JBD_PARM_DT_TEMP}, + {JBD_REG_RCHGOT, "ChgOTRelease", JBD_PARM_DT_TEMP}, + {JBD_REG_CHGUT, "ChgLowTemp", JBD_PARM_DT_TEMP}, + {JBD_REG_RCHGUT, "ChgUTRelease", JBD_PARM_DT_TEMP}, + {JBD_REG_DSGOT, "DisOverTemp", JBD_PARM_DT_TEMP}, + {JBD_REG_RDSGOT, "DsgOTRelease", JBD_PARM_DT_TEMP}, + {JBD_REG_DSGUT, "DisLowTemp", JBD_PARM_DT_TEMP}, + {JBD_REG_RDSGUT, "DsgUTRelease", JBD_PARM_DT_TEMP}, + {JBD_REG_POVP, "PackOverVoltage", JBD_PARM_DT_INT}, + {JBD_REG_RPOVP, "PackOVRelease", JBD_PARM_DT_INT}, + {JBD_REG_PUVP, "PackUnderVoltage", JBD_PARM_DT_INT}, + {JBD_REG_RPUVP, "PackUVRelease", JBD_PARM_DT_INT}, + {JBD_REG_COVP, "CellOverVoltage", JBD_PARM_DT_INT}, + {JBD_REG_RCOVP, "CellOVRelease", JBD_PARM_DT_INT}, + {JBD_REG_CUVP, "CellUnderVoltage", JBD_PARM_DT_INT}, + {JBD_REG_RCUVP, "CellUVRelease", JBD_PARM_DT_INT}, + {JBD_REG_CHGOC, "OverChargeCurrent", JBD_PARM_DT_INT}, + {JBD_REG_DSGOC, "OverDisCurrent", JBD_PARM_DT_INT}, + {JBD_REG_BALVOL, "BalanceStartVoltage", JBD_PARM_DT_INT}, + {JBD_REG_BALPREC, "BalanceWindow", JBD_PARM_DT_INT}, + {JBD_REG_CURRES, "SenseResistor", JBD_PARM_DT_INT}, + {JBD_REG_FUNCMASK, "BatteryConfig", JBD_PARM_DT_FUNC}, + {JBD_REG_NTCMASK, "NtcConfig", JBD_PARM_DT_NTC}, + {JBD_REG_STRINGS, "PackNum", JBD_PARM_DT_INT}, + {JBD_REG_FETTIME, "fet_ctrl_time_set", JBD_PARM_DT_INT}, + {JBD_REG_LEDTIME, "led_disp_time_set", JBD_PARM_DT_INT}, + {JBD_REG_VOLCAP80, "VoltageCap80", JBD_PARM_DT_INT}, + {JBD_REG_VOLCAP60, "VoltageCap60", JBD_PARM_DT_INT}, + {JBD_REG_VOLCAP40, "VoltageCap40", JBD_PARM_DT_INT}, + {JBD_REG_VOLCAP20, "VoltageCap20", JBD_PARM_DT_INT}, + {JBD_REG_HCOVP, "HardCellOverVoltage", JBD_PARM_DT_INT}, + {JBD_REG_HCUVP, "HardCellUnderVoltage", JBD_PARM_DT_INT}, + {JBD_REG_HCOC, "DoubleOCSC", JBD_PARM_DT_DOUBLE}, + {JBD_REG_HCOC, "SCValue", JBD_PARM_DT_SCVAL}, + {JBD_REG_HCOC, "SCDelay", JBD_PARM_DT_SCDELAY}, + {JBD_REG_HCOC, "DSGOC2", JBD_PARM_DT_DSGOC2}, + {JBD_REG_HCOC, "DSGOC2Delay", JBD_PARM_DT_DSGOC2DELAY}, + {JBD_REG_HTRT, "HCOVPDelay", JBD_PARM_DT_HCOVPDELAY}, + {JBD_REG_HTRT, "HCUVPDelay", JBD_PARM_DT_HCUVPDELAY}, + {JBD_REG_HTRT, "SCRelease", JBD_PARM_DT_B1}, + {JBD_REG_CHGDELAY, "ChgUTDelay", JBD_PARM_DT_B0}, + {JBD_REG_CHGDELAY, "ChgOTDelay", JBD_PARM_DT_B1}, + {JBD_REG_DSGDELAY, "DsgUTDelay", JBD_PARM_DT_B0}, + {JBD_REG_DSGDELAY, "DsgOTDelay", JBD_PARM_DT_B1}, + {JBD_REG_PVDELAY, "PackUVDelay", JBD_PARM_DT_B0}, + {JBD_REG_PVDELAY, "PackOVDelay", JBD_PARM_DT_B1}, + {JBD_REG_CVDELAY, "CellUVDelay", JBD_PARM_DT_B0}, + {JBD_REG_CVDELAY, "CellOVDelay", JBD_PARM_DT_B1}, + {JBD_REG_CHGOCDELAY, "ChgOCDelay", JBD_PARM_DT_B0}, + {JBD_REG_CHGOCDELAY, "ChgOCRDelay", JBD_PARM_DT_B1}, + {JBD_REG_DSGOCDELAY, "DsgOCDelay", JBD_PARM_DT_B0}, + {JBD_REG_DSGOCDELAY, "DsgOCRDelay", JBD_PARM_DT_B1}, + {JBD_REG_MFGNAME, "ManufacturerName", JBD_PARM_DT_STR}, + {JBD_REG_MODEL, "DeviceName", JBD_PARM_DT_STR}, + {JBD_REG_BARCODE, "BarCode", JBD_PARM_DT_STR}, + {JBD_REG_GPSOFF, "GPS_VOL", JBD_PARM_DT_INT}, + {JBD_REG_GPSOFFTIME, "GPS_TIME", JBD_PARM_DT_INT}, + {JBD_REG_VOLCAP90, "VoltageCap90", JBD_PARM_DT_INT}, + {JBD_REG_VOLCAP70, "VoltageCap70", JBD_PARM_DT_INT}, + {JBD_REG_VOLCAP50, "VoltageCap50", JBD_PARM_DT_INT}, + {JBD_REG_VOLCAP30, "VoltageCap30", JBD_PARM_DT_INT}, + {JBD_REG_VOLCAP10, "VoltageCap10", JBD_PARM_DT_INT}, + {JBD_REG_VOLCAP100, "VoltageCap100", JBD_PARM_DT_INT}, + {JBD_REG_MOSFET, "Mosfet", JBD_PARM_DT_INT}, + {0, 0, 0}}; +typedef struct jbd_params jbd_params_t; + +struct jbd_params *_getp(char *label) { + register struct jbd_params *pp; + + dprintf(3, "label: %s\n", label); + for (pp = params; pp->label; pp++) { + dprintf(3, "pp->label: %s\n", pp->label); + if (strcmp(pp->label, label) == 0) { + return pp; + } + } + return 0; +} + +void dint(char *label, char *format, int val) { + char temp[128]; + + dprintf(3, "label: %s, val: %d\n", label, val); + switch (outfmt) { + case 2: + json_object_set_number(root_object, label, val); + break; + case 1: + sprintf(temp, "%%s,%s\n", format); + dprintf(3, "temp: %s\n", temp); + fprintf(outfp, temp, label, val); + break; + default: + sprintf(temp, "%%-25s %s\n", format); + dprintf(3, "temp: %s\n", temp); + fprintf(outfp, temp, label, val); + break; + } +} +#define _dint(l, v) dint(l, "%d", v) + +void dbool(char *label, int val) { + dprintf(3, "label: %s, val: %d\n", label, val); + switch (outfmt) { + case 2: + json_object_set_boolean(root_object, label, val); + break; + case 1: + if (dont_interpret) + fprintf(outfp, "%s,%d\n", label, val); + else + fprintf(outfp, "%s,%s\n", label, val ? "true" : "false"); + break; + default: + if (dont_interpret) + fprintf(outfp, "%-25s %d\n", label, val); + else + fprintf(outfp, "%-25s %s\n", label, val ? "true" : "false"); + break; + } +} + +void dfloat(char *label, char *format, float val) { + char temp[128]; + + dprintf(3, "dint: label: %s, val: %f\n", label, val); + switch (outfmt) { + case 2: + json_object_set_number(root_object, label, val); + break; + case 1: + sprintf(temp, "%%s,%s\n", format); + dprintf(3, "temp: %s\n", temp); + fprintf(outfp, temp, label, val); + break; + default: + sprintf(temp, "%%-25s %s\n", format); + dprintf(3, "temp: %s\n", temp); + fprintf(outfp, temp, label, val); + break; + } +} +#define _dfloat(l, v) dfloat(l, "%f", v) + +void dstr(char *label, char *format, char *val) { + char temp[128]; + + dprintf(3, "dint: label: %s, val: %s\n", label, val); + switch (outfmt) { + case 2: + json_object_set_string(root_object, label, val); + break; + case 1: + sprintf(temp, "%%s,%s\n", format); + dprintf(3, "temp: %s\n", temp); + fprintf(outfp, temp, label, val); + break; + default: + sprintf(temp, "%%-25s %s\n", format); + dprintf(3, "temp: %s\n", temp); + fprintf(outfp, temp, label, val); + break; + } +} +#define _dstr(l, v) dstr(l, "%s", v) + +static inline void _addstr(char *str, char *newstr) { + dprintf(4, "str: %s, newstr: %s\n", str, newstr); + if (strlen(str)) + strcat(str, sepstr); + dprintf(4, "str: %s\n", str); + if (outfmt == 2) + strcat(str, "\""); + strcat(str, newstr); + if (outfmt == 2) + strcat(str, "\""); + dprintf(4, "str: %s\n", str); +} + +void _dump(char *label, short val) { + char str[64], temp[72]; + + dprintf(3, "label: %s, val: %d\n", label, val); + str[0] = 0; + sprintf(temp, "%d", val); + _addstr(str, temp); + sprintf(temp, "%d", (unsigned short)val); + _addstr(str, temp); + sprintf(temp, "%04x", (unsigned short)val); + _addstr(str, temp); + dprintf(3, "str: %s\n", str); + switch (outfmt) { + case 2: + sprintf(temp, "[ %s ]", str); + dprintf(2, "temp: %s\n", temp); + json_object_dotset_value(root_object, label, json_parse_string(temp)); + break; + case 1: + printf("%s,%s\n", label, str); + break; + default: + printf("%-25s %s\n", label, str); + break; + } +} + +void pdisp(char *label, int dt, uint8_t *data, int len) { + char str[64], temp[72]; + uint16_t val; + + dprintf(3, "label: %s, dt: %d\n", label, dt); + switch (dt) { + case JBD_PARM_DT_INT: + case JBD_PARM_DT_TEMP: + case JBD_PARM_DT_DATE: + case JBD_PARM_DT_PCT: + _dint(label, (int)_getshort(data)); + break; + case JBD_PARM_DT_B0: + _dint(label, data[0]); + break; + case JBD_PARM_DT_B1: + _dint(label, data[1]); + break; + case JBD_PARM_DT_FUNC: + if (dont_interpret) { + _dint(label, (int)_getshort(data)); + } else { + val = _getshort(data); + str[0] = 0; + if (val & JBD_FUNC_SWITCH) + _addstr(str, "Switch"); + if (val & JBD_FUNC_SCRL) + _addstr(str, "SCRL"); + if (val & JBD_FUNC_BALANCE_EN) + _addstr(str, "BALANCE_EN"); + if (val & JBD_FUNC_CHG_BALANCE) + _addstr(str, "CHG_BALANCE"); + if (val & JBD_FUNC_LED_EN) + _addstr(str, "LED_EN"); + if (val & JBD_FUNC_LED_NUM) + _addstr(str, "LED_NUM"); + if (val & JBD_FUNC_RTC) + _addstr(str, "RTC"); + if (val & JBD_FUNC_EDV) + _addstr(str, "EDV"); + switch (outfmt) { + case 2: + sprintf(temp, "[ %s ]", str); + dprintf(1, "temp: %s\n", temp); + json_object_dotset_value(root_object, label, json_parse_string(temp)); + break; + case 1: + dstr(label, "%s", str); + break; + default: + dstr(label, "%s", str); + break; + } + } + break; + case JBD_PARM_DT_NTC: + if (dont_interpret) { + _dint(label, (int)_getshort(data)); + } else { + val = _getshort(data); + str[0] = 0; + if (val & JBD_NTC1) + _addstr(str, "NTC1"); + if (val & JBD_NTC2) + _addstr(str, "NTC2"); + if (val & JBD_NTC3) + _addstr(str, "NTC3"); + if (val & JBD_NTC4) + _addstr(str, "NTC4"); + if (val & JBD_NTC5) + _addstr(str, "NTC5"); + if (val & JBD_NTC6) + _addstr(str, "NTC6"); + if (val & JBD_NTC7) + _addstr(str, "NTC7"); + if (val & JBD_NTC8) + _addstr(str, "NTC8"); + switch (outfmt) { + case 2: + sprintf(temp, "[ %s ]", str); + dprintf(2, "temp: %s\n", temp); + json_object_dotset_value(root_object, label, json_parse_string(temp)); + break; + case 1: + dstr(label, "%s", str); + break; + default: + dstr(label, "%s", str); + break; + } + } + break; + case JBD_PARM_DT_FLOAT: + _dfloat(label, (float)_getshort(data)); + break; + case JBD_PARM_DT_STR: + data[len] = 0; + trim((char *)data); + _dstr(label, (char *)data); + break; + case JBD_PARM_DT_DUMP: + _dump(label, _getshort(data)); + break; + case JBD_PARM_DT_DOUBLE: + dbool(label, ((data[0] & 0x80) != 0)); + break; + case JBD_PARM_DT_DSGOC2: + if (dont_interpret) { + _dint(label, data[1] & 0x0f); + } else { + int vals[] = {8, 11, 14, 17, 19, 22, 25, 28, + 31, 33, 36, 39, 42, 44, 47, 50}; + int i = data[1] & 0x0f; + dprintf(1, "data[1]: %02x\n", data[1]); + + dprintf(1, "i: %d\n", i); + _dint(label, vals[i]); + } + break; + case JBD_PARM_DT_DSGOC2DELAY: + if (dont_interpret) { + _dint(label, (data[1] >> 4) & 0x07); + } else { + int vals[] = {8, 20, 40, 80, 160, 320, 640, 1280}; + int i = (data[1] >> 4) & 0x07; + dprintf(1, "data[1]: %02x\n", data[1]); + + dprintf(1, "i: %d\n", i); + _dint(label, vals[i]); + } + break; + case JBD_PARM_DT_SCVAL: + if (dont_interpret) { + _dint(label, data[0] & 0x07); + } else { + int vals[] = {22, 33, 44, 56, 67, 78, 89, 100}; + int i = data[0] & 0x07; + dprintf(1, "data[0]: %02x\n", data[0]); + dprintf(1, "i: %d\n", i); + _dint(label, vals[i]); + } + break; + case JBD_PARM_DT_SCDELAY: + if (dont_interpret) { + _dint(label, (data[0] >> 3) & 0x03); + } else { + int vals[] = {70, 100, 200, 400}; + int i = (data[0] >> 3) & 0x03; + dprintf(1, "data[0]: %02x\n", data[0]); + + dprintf(1, "i: %d\n", i); + _dint(label, vals[i]); + } + break; + case JBD_PARM_DT_HCOVPDELAY: + if (dont_interpret) { + _dint(label, (data[0] >> 4) & 0x03); + } else { + int vals[] = {1, 2, 4, 8}; + int i = (data[0] >> 4) & 0x03; + dprintf(1, "data[0]: %02x\n", data[0]); + + dprintf(1, "i: %d\n", i); + _dint(label, vals[i]); + } + + break; + case JBD_PARM_DT_HCUVPDELAY: + if (dont_interpret) { + _dint(label, (data[0] >> 6) & 0x03); + } else { + int vals[] = {1, 4, 8, 16}; + int i = (data[0] >> 6) & 0x03; + dprintf(1, "data[0]: %02x\n", data[0]); + + dprintf(1, "i: %d\n", i); + _dint(label, vals[i]); + } + break; + } +} + +void display_info(jbd_info_t *info) { + char label[16], temp[256], *p; + int i; + + if (strlen(info->name)) + dstr("Name", "%s", info->name); + dfloat("Voltage", "%.3f", info->voltage); + dfloat("Current", "%.3f", info->current); + dfloat("DesignCapacity", "%.3f", info->fullcap); + dfloat("RemainingCapacity", "%.3f", info->capacity); + _dint("PercentCapacity", info->pctcap); + _dint("CycleCount", info->cycles); + _dint("Probes", info->probes); + _dint("Strings", info->strings); + if (flat) { + for (i = 0; i < info->probes; i++) { + sprintf(label, "temp_%02d", i); + dfloat(label, "%.1f", info->temps[i]); + } + for (i = 0; i < info->strings; i++) { + sprintf(label, "cell_%02d", i); + dfloat(label, "%.3f", info->cellvolt[i]); + } + } else { + switch (outfmt) { + case 2: + p = temp; + p += sprintf(p, "[ "); + for (i = 0; i < info->probes; i++) { + if (i) + p += sprintf(p, ","); + p += sprintf(p, "%.1f", info->temps[i]); + } + strcat(temp, " ]"); + dprintf(1, "temp: %s\n", temp); + json_object_dotset_value(root_object, "Temps", json_parse_string(temp)); + break; + default: + p = temp; + for (i = 0; i < info->probes; i++) { + if (i) + p += sprintf(p, "%c", sepch); + p += sprintf(p, "%.1f", info->temps[i]); + } + dstr("Temps", "%s", temp); + break; + } + switch (outfmt) { + case 2: + p = temp; + p += sprintf(p, "[ "); + for (i = 0; i < info->strings; i++) { + if (i) + p += sprintf(p, ","); + p += sprintf(p, "%.3f", info->cellvolt[i]); + } + strcat(temp, " ]"); + dprintf(1, "temp: %s\n", temp); + json_object_dotset_value(root_object, "Cells", json_parse_string(temp)); + break; + default: + p = temp; + for (i = 0; i < info->strings; i++) { + if (i) + p += sprintf(p, "%c", sepch); + p += sprintf(p, "%.3f", info->cellvolt[i]); + } + dstr("Cells", "%s", temp); + break; + } + } + { + char bits[40]; + unsigned long mask = 1; + i = 0; + while (mask) { + bits[i++] = ((info->balancebits & mask) != 0 ? '1' : '0'); + if (i >= info->strings) + break; + mask <<= 1; + } + bits[i] = 0; + dstr("Balance", "%s", bits); + } + dfloat("CellTotal", "%.3f", info->cell_total); + dfloat("CellMin", "%.3f", info->cell_min); + dfloat("CellMax", "%.3f", info->cell_max); + dfloat("CellDiff", "%.3f", info->cell_diff); + dfloat("CellAvg", "%.3f", info->cell_avg); + _dstr("DeviceName", info->model); + _dstr("ManufactureDate", info->mfgdate); + dfloat("Version", "%.1f", info->version); + if (dont_interpret) { + _dint("FET", info->fetstate); + } else { + temp[0] = 0; + p = temp; + if (info->fetstate & JBD_MOS_CHARGE) + p += sprintf(p, "Charge"); + if (info->fetstate & JBD_MOS_DISCHARGE) { + if (info->fetstate & JBD_MOS_CHARGE) + p += sprintf(p, sepstr); + p += sprintf(p, "Discharge"); + } + dstr("FET", "%s", temp); + } +#if 0 + unsigned long balancebits; + /* the protection sign */ + unsigned short protectbits; + struct { + unsigned sover: 1; /* Single overvoltage protection */ + unsigned sunder: 1; /* Single undervoltage protection */ + unsigned gover: 1; /* Whole group overvoltage protection */ + unsigned gunder: 1; /* Whole group undervoltage protection */ + unsigned chitemp: 1; /* Charge over temperature protection */ + unsigned clowtemp: 1; /* Charge low temperature protection */ + unsigned dhitemp: 1; /* Discharge over temperature protection */ + unsigned dlowtemp: 1; /* Discharge low temperature protection */ + unsigned cover: 1; /* Charge overcurrent protection */ + unsigned cunder: 1; /* Discharge overcurrent protection */ + unsigned shorted: 1; /* Short circuit protection */ + unsigned ic: 1; /* Front detection IC error */ + unsigned mos: 1; /* Software lock MOS */ + } protect; +#endif +} + +int init_pack(mybmm_pack_t *pp, mybmm_config_t *c, char *type, char *transport, + char *target, char *opts, mybmm_module_t *cp, + mybmm_module_t *tp) { + memset(pp, 0, sizeof(*pp)); + strcpy(pp->type, type); + if (transport) + strcpy(pp->transport, transport); + if (target) + strcpy(pp->target, target); + if (opts) + strcpy(pp->opts, opts); + pp->open = cp->open; + pp->read = cp->read; + pp->close = cp->close; + pp->handle = cp->new (c, pp, tp); + return 0; +} + +enum JBDTOOL_ACTION { + JBDTOOL_ACTION_INFO = 0, + JBDTOOL_ACTION_READ, + JBDTOOL_ACTION_WRITE, + JBDTOOL_ACTION_LIST +}; + +int write_parm(void *h, struct jbd_params *pp, char *value) { + uint8_t data[128]; + int len; + + dprintf(3, "h: %p, pp->label: %s, value: %s\n", h, pp->label, value); + len = 2; + dprintf(3, "dt: %d\n", pp->dt); + switch (pp->dt) { + case JBD_PARM_DT_INT: + case JBD_PARM_DT_TEMP: + case JBD_PARM_DT_DATE: + case JBD_PARM_DT_PCT: + case JBD_PARM_DT_FUNC: + case JBD_PARM_DT_NTC: + _putshort(data, atoi(value)); + break; + case JBD_PARM_DT_B0: + if (jbd_rw(h, JBD_CMD_READ, pp->reg, data, sizeof(data)) < 1) + return -1; + data[0] = atoi(value); + break; + case JBD_PARM_DT_B1: + if (jbd_rw(h, JBD_CMD_READ, pp->reg, data, sizeof(data)) < 1) + return -1; + data[1] = atoi(value); + break; + case JBD_PARM_DT_FLOAT: + _putshort(data, atof(value)); + break; + case JBD_PARM_DT_STR: + len = strlen(value); + memcpy(data, value, len); + break; + } + // bindump("write data",data,len); + return jbd_rw(h, JBD_CMD_WRITE, pp->reg, data, len); +} + +#ifdef MQTT +#define MQTT_OPTS " [-m ] [-i ]" +#define MQTT_GETOPT "m:i:" +#else +#define MQTT_OPTS "" +#define MQTT_GETOPT "" +#endif + +void usage() { + printf("jbdtool version %s build %lld\n", VERSION, BUILD); + printf("usage: jbdtool [-abcjJFrwlhXN] [-f filename] [-t ] " + "[-o output file]" MQTT_OPTS "\n"); + printf("arguments:\n"); +#ifdef DEBUG + printf(" -d <#> debug output\n"); +#endif + printf(" -c comma-delimited output\n"); + printf(" -g charging on/off\n"); + printf(" -G discharging on/off\n"); + printf(" -j JSON output\n"); + printf(" -J JSON output pretty print\n"); + printf(" -F Flatten JSON arrays\n"); + printf(" -r read parameters\n"); + printf(" -a read all parameters\n"); + printf(" -w write parameters\n"); + printf(" -l list supported parameters\n"); + printf(" -h this output\n"); + printf(" -f input filename for read/write.\n"); + printf(" -o output filename\n"); + printf(" -t transport & target\n"); + printf(" -e transport-specific opts\n"); + printf(" -n numbers only; dont interpret\n"); +#ifdef MQTT + printf( + " -m Send results to MQTT broker\n"); + printf(" -i Update interval\n"); +#endif + printf(" -X reset BMS\n"); + printf(" -N dont wait if locked\n"); +} + +int main(int argc, char **argv) { + int opt, bytes, action, pretty, all, i, reg, dump; + int charge, discharge, reset; + char *transport, *target, *label, *filename, *outfile, *p, *opts; + mybmm_config_t *conf; + mybmm_module_t *cp, *tp; + mybmm_pack_t pack; + jbd_info_t info; + jbd_params_t *pp; + uint8_t data[128]; +#ifdef MQTT + int interval; + char *mqtt; +#endif + char lockfile[256]; + int lockfd, dont_wait; + + charge = discharge = -1; + action = pretty = outfmt = all = reg = dump = flat = reset = dont_wait = 0; + sepch = ','; + sepstr = ","; + transport = target = label = filename = outfile = opts = 0; +#ifdef MQTT + interval = 0; + mqtt = 0; + char mqtt_topic[128]; +#endif + while ((opt = getopt(argc, argv, + "+acDg:G:d:nNt:e:f:R:jJo:rwlhFX" MQTT_GETOPT)) != -1) { + switch (opt) { + case 'D': + dump = 1; + break; + case 'a': + all = 1; + break; + case 'c': + outfmt = 1; + sepch = ' '; + sepstr = " "; + break; + case 'd': + debug = atoi(optarg); + break; + case 'g': + if (strcasecmp(optarg, "on") == 0) + charge = 1; + else if (strcasecmp(optarg, "off") == 0) + charge = 0; + else { + printf("error: invalid charge state: %s\n", optarg); + usage(); + return 1; + } + break; + case 'G': + if (strcasecmp(optarg, "on") == 0) + discharge = 1; + else if (strcasecmp(optarg, "off") == 0) + discharge = 0; + else { + printf("error: invalid discharge state: %s\n", optarg); + usage(); + return 1; + } + break; +#ifdef MQTT + case 'm': + mqtt = optarg; + break; + case 'i': + interval = atoi(optarg); + break; +#endif + case 'f': + filename = optarg; + break; + case 'j': + outfmt = 2; + pretty = 0; + break; + case 'J': + outfmt = 2; + pretty = 1; + break; + case 'o': + outfile = optarg; + break; + case 'n': + dont_interpret = 1; + break; + case 'N': + dont_wait = 1; + break; + case 't': + transport = optarg; + target = strchr(transport, ':'); + if (!target) { + printf("error: format is transport:target\n"); + usage(); + return 1; + } + *target = 0; + target++; + break; + case 'e': + opts = optarg; + break; + case 'R': + action = JBDTOOL_ACTION_READ; + if (strstr(optarg, "0x") || strncmp(optarg, "x", 1) == 0) + reg = strtol(optarg, 0, 16); + else + reg = strtol(optarg, 0, 10); + break; + case 'r': + action = JBDTOOL_ACTION_READ; + break; + case 'w': + action = JBDTOOL_ACTION_WRITE; + break; + case 'l': + for (pp = params; pp->label; pp++) + printf("%s\n", pp->label); + return 0; + break; + case 'F': + flat = 1; + break; + case 'X': + reset = 1; + break; + case 'h': + default: + usage(); + exit(0); + } + } + dprintf(1, "transport: %p, target: %p\n", transport, target); + if (!transport && action != JBDTOOL_ACTION_LIST) { + usage(); + return 1; + } + + log_open("mybmm", 0, + LOG_DEBUG | LOG_INFO | LOG_WARNING | LOG_ERROR | LOG_SYSERR); + + argc -= optind; + argv += optind; + optind = 0; + + if ((action == JBDTOOL_ACTION_READ || action == JBDTOOL_ACTION_WRITE) && + !filename && !argc && !all && !reg && !dump) { + printf( + "error: a filename or parameter name or all (a) must be specified.\n"); + usage(); + return 1; + } + + conf = calloc(sizeof(*conf), 1); + if (!conf) { + perror("calloc conf"); + return 1; + } + conf->modules = list_create(); + memset(&info, 0, sizeof(info)); + + dprintf(2, "transport: %s\n", transport); + + tp = mybmm_load_module(conf, transport, MYBMM_MODTYPE_TRANSPORT); + dprintf(1, "tp: %p\n", tp); + if (!tp) + return 1; + cp = mybmm_load_module(conf, "jbd", MYBMM_MODTYPE_CELLMON); + dprintf(1, "cp: %p\n", cp); + if (!cp) + return 1; + + /* Init the pack */ + p = strchr(target, ','); + if (p) { + *p++ = 0; + if (!opts) + opts = p; + } + if (init_pack(&pack, conf, "jbd", transport, target, opts, cp, tp)) + return 1; + +#ifndef WINDOWS + /* Lock the target */ + for (p = target + strlen(target); p >= target; p--) { + if (*p != 0 && !isalnum(*p) && *p != '_' && *p != '-') { + break; + } + } + dprintf(2, "p: %p, target: %p\n", p, target); + sprintf(lockfile, "/tmp/%s.lock", p + 1); + dprintf(2, "lockfile: %s\n", lockfile); + lockfd = lock_file(lockfile, (dont_wait ? 0 : 1)); + dprintf(2, "lockfd: %d\n", lockfd); + if (lockfd < 0) { + log_error("unable to lock target"); + return 1; + } +#endif + + /* If setting charge or discharge do that here */ + dprintf(2, "charge: %d, discharge: %d\n", charge, discharge); + if (charge >= 0 || discharge >= 0) { + opt = 0; + if (pack.open(pack.handle)) + return 1; + if (jbd_get_info(pack.handle, &info) == 0) { + dprintf(2, "fetstate: %x\n", info.fetstate); + if (charge == 0 || (info.fetstate & JBD_MOS_CHARGE) == 0) + opt |= JBD_MOS_CHARGE; + if (charge == 1) + opt = (opt & ~JBD_MOS_CHARGE); + dprintf(2, "opt: %x\n", opt); + if (discharge == 0 || (info.fetstate & JBD_MOS_DISCHARGE) == 0) + opt |= JBD_MOS_DISCHARGE; + if (discharge == 1) + opt = (opt & ~JBD_MOS_DISCHARGE); + dprintf(2, "opt: %x\n", opt); + i = jbd_set_mosfet(pack.handle, opt); + if (!i) { + if (charge >= 0) + printf("charging %s\n", charge ? "enabled" : "disabled"); + if (discharge >= 0) + printf("discharging %s\n", discharge ? "enabled" : "disabled"); + } + } + if (pack.close(pack.handle)) + return 1; + return i; + } + + /* Reset? */ + dprintf(2, "reset: %d\n", reset); + if (reset) { + printf("*** RESETTING ***\n"); + if (pack.open(pack.handle)) + return 1; + if (jbd_eeprom_start(pack.handle)) { + pack.close(pack.handle); + return 1; + } + jbd_reset(pack.handle); + jbd_eeprom_end(pack.handle); + pack.close(pack.handle); + return 1; + } + +#ifdef MQTT + /* If MQTT, output is compact JSON */ + dprintf(1, "mqtt: %p\n", mqtt); + if (mqtt) { + struct mqtt_config mc; + + memset(&mc, 0, sizeof(mc)); + + action = JBDTOOL_ACTION_INFO; + outfmt = 2; + strcpy(mc.host, strele(0, ":", mqtt)); + strcpy(mc.clientid, strele(1, ":", mqtt)); + strcpy(mqtt_topic, strele(2, ":", mqtt)); + strcpy(mc.user, strele(3, ":", mqtt)); + strcpy(mc.pass, strele(4, ":", mqtt)); + dprintf(1, "host: %s, clientid: %s, topic: %s, user: %s, pass: %s\n", + mc.host, mc.clientid, mqtt_topic, mc.user, mc.pass); + if (strlen(mc.host) == 0 || strlen(mc.clientid) == 0 || + strlen(mqtt_topic) == 0) { + printf("error: mqtt format is: host:clientid:topic[:user:pass]\n"); + return 1; + } + + conf->mqtt = mqtt_new(&mc, 0, 0); + if (!conf->mqtt) + return 1; + + /* Test the connection */ + if (mqtt_connect(conf->mqtt, 20)) + return 1; + mqtt_disconnect(conf->mqtt, 10); + + strncat(info.name, mc.clientid, sizeof(info.name) - 1); + dprintf(1, "info.name: %s\n", info.name); + pretty = 0; + } +#endif + + if (outfile) { + p = strrchr(outfile, '.'); + if (p) { + dprintf(1, "p: %s\n", p); + if (strcmp(p, ".json") == 0 || strcmp(p, ".JSON") == 0) { + outfmt = 2; + pretty = 1; + } + } + dprintf(1, "outfile: %s\n", outfile); + outfp = fopen(outfile, "w+"); + if (!outfp) { + perror("fopen outfile"); + return 1; + } + } else { + outfp = fdopen(1, "w"); + } + dprintf(1, "outfp: %p\n", outfp); + + if (outfmt == 2) { + root_value = json_value_init_object(); + root_object = json_value_get_object(root_value); + } + if (dump) { + // char temp[16]; + + if (pack.open(pack.handle)) + return 1; + if (jbd_eeprom_start(pack.handle)) + return 1; + for (i = 0x10; i < 0xFF; i++) { + bytes = jbd_rw(pack.handle, JBD_CMD_READ, i, data, sizeof(data)); + dprintf(3, "bytes: %d\n", bytes); + if (bytes > 0) { + // sprintf(temp,"Register %02x\n", reg); + // pdisp(temp,JBD_PARM_DT_INT,data,bytes); + printf("%02x: %d\n", i, _getshort(data)); + } + } + jbd_eeprom_end(pack.handle); + pack.close(pack.handle); + return 0; + } +#ifdef MQTT + do { +#endif + switch (action) { + case JBDTOOL_ACTION_INFO: + if (pack.open(pack.handle)) + return 1; + if (jbd_get_info(pack.handle, &info) == 0) { + display_info(&info); + } + pack.close(pack.handle); + break; + case JBDTOOL_ACTION_READ: + if (strcmp(transport, "can") == 0) { + printf("error: reading parameters not possible using CAN bus\n"); + return 1; + } + if (reg) { + char temp[16]; + + if (pack.open(pack.handle)) + return 1; + if (jbd_eeprom_start(pack.handle)) + return 1; + bytes = jbd_rw(pack.handle, JBD_CMD_READ, reg, data, sizeof(data)); + dprintf(3, "bytes: %d\n", bytes); + if (bytes > 0) { + sprintf(temp, "Register %02x", reg); + dprintf(2, "temp: %s\n", temp); + pdisp(temp, JBD_PARM_DT_DUMP, data, bytes); + } + jbd_eeprom_end(pack.handle); + pack.close(pack.handle); + } + if (filename) { + char line[128]; + FILE *fp; + + dprintf(2, "filename: %s\n", filename); + + /* Get param names from .json file? */ + p = strrchr(filename, '.'); + if (p && strcmp(p, ".json") == 0) { + JSON_Object *object; + int count; + + root_value = json_parse_file(filename); + if (json_value_get_type(root_value) != JSONObject) { + printf("error: not a valid json file\n"); + return 1; + } + if (pack.open(pack.handle)) + return 1; + if (jbd_eeprom_start(pack.handle)) + return 1; + object = json_value_get_object(root_value); + count = json_object_get_count(object); + for (i = 0; i < count; i++) { + p = (char *)json_object_get_name(object, i); + if (!p) { + printf("error reading json file\n"); + return 1; + } + dprintf(3, "p: %s\n", p); + pp = _getp(p); + if (!pp) { + printf("error: parm in json file not found: %s\n", p); + return 1; + } + memset(data, 0, sizeof(data)); + bytes = + jbd_rw(pack.handle, JBD_CMD_READ, pp->reg, data, sizeof(data)); + if (bytes < 0) + continue; + dprintf(3, "bytes: %d\n", bytes); + pdisp(pp->label, pp->dt, data, bytes); + } + jbd_eeprom_end(pack.handle); + pack.close(pack.handle); + } else { + fp = fopen(filename, "r"); + if (!fp) { + printf("fopen(r) %s: %s\n", filename, strerror(errno)); + return 1; + } + if (pack.open(pack.handle)) + return 1; + if (jbd_eeprom_start(pack.handle)) + return 1; + while (fgets(line, sizeof(line), fp)) { + p = line; + while (*p && isspace(*p)) + p++; + label = p; + while (*p && !isspace(*p)) + p++; + *p = 0; + pp = _getp(label); + dprintf(3, "pp: %p\n", pp); + if (!pp) + continue; + memset(data, 0, sizeof(data)); + bytes = + jbd_rw(pack.handle, JBD_CMD_READ, pp->reg, data, sizeof(data)); + if (bytes < 0) + continue; + dprintf(3, "bytes: %d\n", bytes); + pdisp(pp->label, pp->dt, data, bytes); + } + jbd_eeprom_end(pack.handle); + pack.close(pack.handle); + fclose(fp); + } + } else { + dprintf(1, "all: %d\n", all); + if (pack.open(pack.handle)) + return 1; + if (jbd_eeprom_start(pack.handle)) + return 1; + if (all) { + for (pp = params; pp->label; pp++) { + dprintf(3, "pp->label: %s\n", pp->label); + memset(data, 0, sizeof(data)); + bytes = + jbd_rw(pack.handle, JBD_CMD_READ, pp->reg, data, sizeof(data)); + if (bytes < 0) + break; + if (bytes) + pdisp(pp->label, pp->dt, data, bytes); + } + } else { + /* Every arg is a parm name */ + for (i = 0; i < argc; i++) { + pp = _getp(argv[i]); + dprintf(2, "pp: %p\n", pp); + if (!pp) { + printf("error: parameter %s not found.\n", argv[i]); + return 1; + } + memset(data, 0, sizeof(data)); + bytes = + jbd_rw(pack.handle, JBD_CMD_READ, pp->reg, data, sizeof(data)); + if (bytes > 0) + pdisp(pp->label, pp->dt, data, bytes); + } + } + jbd_eeprom_end(pack.handle); + pack.close(pack.handle); + } + break; + case JBDTOOL_ACTION_WRITE: + if (strcmp(transport, "can") == 0) { + printf("error: writing parameters not possible using CAN bus\n"); + return 1; + } + if (filename) { + char line[128], *valp; + FILE *fp; + + dprintf(3, "filename: %s\n", filename); + + p = strrchr(filename, '.'); + if (p && strcmp(p, ".json") == 0) { + char temp[128]; + JSON_Object *object; + JSON_Value *value; + int count, type, num; + + root_value = json_parse_file(filename); + if (json_value_get_type(root_value) != JSONObject) { + printf("error: not a valid json file\n"); + return 1; + } + if (pack.open(pack.handle)) + return 1; + if (jbd_eeprom_start(pack.handle)) + return 1; + object = json_value_get_object(root_value); + count = json_object_get_count(object); + for (i = 0; i < count; i++) { + p = (char *)json_object_get_name(object, i); + if (!p) { + printf("error reading json file\n"); + return 1; + } + dprintf(3, "p: %s\n", p); + pp = _getp(p); + if (!pp) { + printf("error: parm in json file not found: %s\n", p); + return 1; + } + value = json_object_get_value(object, pp->label); + type = json_value_get_type(value); + switch (type) { + case JSONString: + p = (char *)json_value_get_string(value); + break; + case JSONNumber: + num = (int)json_value_get_number(value); + dprintf(3, "value: %d\n", num); + sprintf(temp, "%d", num); + p = temp; + break; + case JSONBoolean: + num = (int)json_value_get_boolean(value); + dprintf(3, "value: %d\n", num); + sprintf(temp, "%d", num); + p = temp; + break; + default: + printf("error: bad type in json file: %d\n", type); + break; + } + if (write_parm(pack.handle, pp, p)) + break; + } + jbd_eeprom_end(pack.handle); + pack.close(pack.handle); + } else { + fp = fopen(filename, "r"); + if (!fp) { + printf("fopen(r) %s: %s\n", filename, strerror(errno)); + return 1; + } + if (pack.open(pack.handle)) + return 1; + if (jbd_eeprom_start(pack.handle)) + return 1; + while (fgets(line, sizeof(line), fp)) { + /* get parm */ + p = line; + while (*p && isspace(*p)) + p++; + label = p; + while (*p && !isspace(*p)) + p++; + *p = 0; + dprintf(3, "label: %s\n", label); + pp = _getp(label); + dprintf(4, "pp: %p\n", pp); + if (!pp) + continue; + /* get value */ + p++; + while (*p && isspace(*p)) + p++; + valp = p; + while (*p && !isspace(*p)) + p++; + *p = 0; + dprintf(3, "valp: %s\n", valp); + if (write_parm(pack.handle, pp, valp)) + break; + } + } + jbd_eeprom_end(pack.handle); + pack.close(pack.handle); + } else { + /* Every arg par is a parm name & value */ + if (pack.open(pack.handle)) + return 1; + if (jbd_eeprom_start(pack.handle)) + return 1; + for (i = 0; i < argc; i++) { + /* Ge the parm */ + pp = _getp(argv[i]); + dprintf(3, "pp: %p\n", pp); + if (!pp) { + printf("error: parameter %s not found.\n", argv[i]); + break; + } + /* Get the value */ + if (i + 1 == argc) { + printf("error: no value for parameter %s\n", argv[i]); + break; + } + i++; + if (write_parm(pack.handle, pp, argv[i])) + break; + } + jbd_eeprom_end(pack.handle); + pack.close(pack.handle); + } + break; + } + if (outfmt == 2) { + if (pretty) + serialized_string = json_serialize_to_string_pretty(root_value); + else + serialized_string = json_serialize_to_string(root_value); +#ifdef MQTT + if (conf->mqtt) { + if (mqtt_connect(conf->mqtt, 10)) + return 1; + mqtt_pub(conf->mqtt, mqtt_topic, serialized_string, 0); + mqtt_disconnect(conf->mqtt, 5); + } else +#endif + fprintf(outfp, "%s", serialized_string); + json_free_serialized_string(serialized_string); + } +#ifdef MQTT + sleep(interval); + } while (interval); +#endif +#ifndef WINDOWS + dprintf(2, "unlocking target\n"); + unlock_file(lockfd); +#endif + json_value_free(root_value); + fclose(outfp); + + return 0; +} diff --git a/sensors/bms/packages/jbdtool/module.c b/sensors/bms/packages/jbdtool/module.c new file mode 100644 index 00000000..49385417 --- /dev/null +++ b/sensors/bms/packages/jbdtool/module.c @@ -0,0 +1,83 @@ + +/* +Copyright (c) 2021, Stephen P. Shoecraft +All rights reserved. + +This source code is licensed under the BSD-style license found in the +LICENSE file in the root directory of this source tree. +*/ + +#define _GNU_SOURCE +#ifndef __WIN32 +#include +#endif +#include "mybmm.h" + +extern mybmm_module_t jbd_module; +#if !defined(__WIN32) && !defined(__WIN64) +extern mybmm_module_t bt_module; +extern mybmm_module_t can_module; +#endif +extern mybmm_module_t ip_module; +extern mybmm_module_t serial_module; + +mybmm_module_t *mybmm_get_module(mybmm_config_t *conf, char *name, int type) { + mybmm_module_t *mp; + + dprintf(3, "name: %s, type: %d\n", name, type); + list_reset(conf->modules); + while ((mp = list_get_next(conf->modules)) != 0) { + dprintf(3, "mp->name: %s, mp->type: %d\n", mp->name, mp->type); + if (strcmp(mp->name, name) == 0 && mp->type == type) { + dprintf(3, "found.\n"); + return mp; + } + } + dprintf(3, "NOT found.\n"); + return 0; +} + +mybmm_module_t *mybmm_load_module(mybmm_config_t *conf, char *name, int type) { + mybmm_module_t *mp; + + list_reset(conf->modules); + while ((mp = list_get_next(conf->modules)) != 0) { + dprintf(3, "mp->name: %s, mp->type: %d\n", mp->name, mp->type); + if (strcmp(mp->name, name) == 0 && mp->type == type) { + dprintf(3, "found.\n"); + return mp; + } + } + dprintf(3, "NOT found.\n"); + + if (strcmp(name, "jbd") == 0) { + mp = &jbd_module; +#if !defined(__WIN32) && !defined(__WIN64) +#if defined(BLUETOOTH) + } else if (strcmp(name, "bt") == 0) { + mp = &bt_module; +#endif + } else if (strcmp(name, "can") == 0) { + mp = &can_module; +#endif + } else if (strcmp(name, "ip") == 0) { + mp = &ip_module; + } else if (strcmp(name, "serial") == 0) { + mp = &serial_module; + } else { + return 0; + } + + /* Call the init function */ + dprintf(3, "init: %p\n", mp->init); + if (mp->init && mp->init(conf)) { + printf("%s init function returned error, aborting.\n", mp->name); + return 0; + } + + /* Add this module to our list */ + dprintf(3, "adding module: %s\n", mp->name); + list_add(conf->modules, mp, 0); + + return mp; +} diff --git a/sensors/bms/packages/jbdtool/module.h b/sensors/bms/packages/jbdtool/module.h new file mode 100644 index 00000000..2926eb35 --- /dev/null +++ b/sensors/bms/packages/jbdtool/module.h @@ -0,0 +1,55 @@ + +#ifndef __MYBMM_MODULE_H +#define __MYBMM_MODULE_H + +#include "config.h" + +#if 0 +/* Forward dec */ +struct mybmm_module; +#endif + +typedef int (*mybmm_module_open_t)(void *); +typedef int (*mybmm_module_read_t)(void *, ...); +typedef int (*mybmm_module_write_t)(void *, ...); +typedef int (*mybmm_module_close_t)(void *); +typedef int (*mybmm_module_control_t)(void *, ...); + +struct mybmm_module { + int type; + char *name; + unsigned short capabilities; + int (*init)(mybmm_config_t *); + // void *(*new)(mybmm_config_t *,mybmm_pack_t *,mybmm_module_t *); + void *(*new)(mybmm_config_t *, ...); + mybmm_module_open_t open; + mybmm_module_read_t read; + mybmm_module_write_t write; + mybmm_module_close_t close; + int (*free)(void *); + int (*shutdown)(void *); + mybmm_module_control_t control; + mybmm_module_control_t config; +}; +typedef struct mybmm_module mybmm_module_t; + +#define MYBMM_MODULE_CAPABILITY_CONTROL 0x01 +#define MYBMM_MODULE_CAPABILITY_CONFIG 0x02 + +// mybmm_module_t *mybmm_get_module(mybmm_config_t *conf, char *name, int type); +// mybmm_module_t *mybmm_load_module(mybmm_config_t *conf, char *name, int +// type); +mybmm_module_t *mybmm_load_module(mybmm_config_t *conf, char *name, int type); + +enum MYBMM_MODTYPE { + MYBMM_MODTYPE_INVERTER, + MYBMM_MODTYPE_CELLMON, + MYBMM_MODTYPE_TRANSPORT, + MYBMM_MODTYPE_DB, +}; + +#ifndef EXPORT_API +#define EXPORT_API __attribute__((visibility("default"))) +#endif + +#endif /* __MYBMM_MODULE_H */ diff --git a/sensors/bms/packages/jbdtool/mqtt.c b/sensors/bms/packages/jbdtool/mqtt.c new file mode 100644 index 00000000..ae539c41 --- /dev/null +++ b/sensors/bms/packages/jbdtool/mqtt.c @@ -0,0 +1,468 @@ + +/* +Copyright (c) 2021, Stephen P. Shoecraft +All rights reserved. + +This source code is licensed under the BSD-style license found in the +LICENSE file in the root directory of this source tree. +*/ + +#include "mqtt.h" +#include "debug.h" +#include "utils.h" +#include +#include +#include +#include + +#define TIMEOUT 10000L + +#define MQTT_5 0 + +struct mqtt_session { + mqtt_config_t config; + MQTTClient c; + int interval; + mqtt_callback_t *cb; + void *ctx; + MQTTClient_SSLOptions *ssl_opts; +}; +typedef struct mqtt_session mqtt_session_t; + +int mqtt_parse_config(mqtt_config_t *conf, char *str) { + strncat(conf->host, strele(0, ",", str), sizeof(conf->host) - 1); + strncat(conf->clientid, strele(1, ",", str), sizeof(conf->clientid) - 1); + strncat(conf->user, strele(2, ",", str), sizeof(conf->user) - 1); + strncat(conf->pass, strele(3, ",", str), sizeof(conf->pass) - 1); + return 0; +} + +int mqtt_get_config(void *cfg, mqtt_config_t *conf) { + struct cfg_proctab tab[] = { + {"mqtt", "broker", "Broker URL", DATA_TYPE_STRING, &conf->host, + sizeof(conf->host), 0}, + {"mqtt", "clientid", "Client ID", DATA_TYPE_STRING, &conf->clientid, + sizeof(conf->clientid), 0}, + {"mqtt", "username", "Broker username", DATA_TYPE_STRING, &conf->user, + sizeof(conf->user), 0}, + {"mqtt", "password", "Broker password", DATA_TYPE_STRING, &conf->pass, + sizeof(conf->pass), 0}, + CFG_PROCTAB_END}; + + cfg_get_tab(cfg, tab); + if (debug) + cfg_disp_tab(tab, "MQTT", 0); + return 0; +} + +#if MQTT +static void mqtt_dc(void *ctx, char *cause) { + dprintf(1, "MQTT disconnected! cause: %s\n", cause); +} + +void logProperties(MQTTProperties *props) { + int i = 0; + + dprintf(1, "props->count: %d\n", props->count); + for (i = 0; i < props->count; ++i) { + int id = props->array[i].identifier; + const char *name = MQTTPropertyName(id); + // char* intformat = "Property name %s value %d"; + + dprintf(1, "prop: id: %d, name: %s\n", id, name); + switch (MQTTProperty_getType(id)) { + case MQTTPROPERTY_TYPE_BYTE: + // MyLog(LOGA_INFO, intformat, name, + // props->array[i].value.byte); + break; + case MQTTPROPERTY_TYPE_TWO_BYTE_INTEGER: + // MyLog(LOGA_INFO, intformat, name, + // props->array[i].value.integer2); + break; + case MQTTPROPERTY_TYPE_FOUR_BYTE_INTEGER: + // MyLog(LOGA_INFO, intformat, name, + // props->array[i].value.integer4); + break; + case MQTTPROPERTY_TYPE_VARIABLE_BYTE_INTEGER: + // MyLog(LOGA_INFO, intformat, name, + // props->array[i].value.integer4); + break; + case MQTTPROPERTY_TYPE_BINARY_DATA: + case MQTTPROPERTY_TYPE_UTF_8_ENCODED_STRING: + // MyLog(LOGA_INFO, "Property name value %s %.*s", name, + // props->array[i].value.data.len, props->array[i].value.data.data); + break; + case MQTTPROPERTY_TYPE_UTF_8_STRING_PAIR: + // MyLog(LOGA_INFO, "Property name %s key %.*s value + //%.*s", name, props->array[i].value.data.len, + // props->array[i].value.data.data, props->array[i].value.value.len, + // props->array[i].value.value.data); + break; + } + } +} + +static int mqtt_getmsg(void *ctx, char *topicName, int topicLen, + MQTTClient_message *message) { + char topic[256], *replyto; + mqtt_session_t *s = ctx; + int len; + + /* Ignore zero length messages */ + if (!message->payloadlen) + goto mqtt_getmsg_skip; + + dprintf(4, "topicLen: %d\n", topicLen); + if (topicLen) { + len = topicLen > sizeof(topic) - 1 ? sizeof(topic) - 1 : topicLen; + dprintf(1, "len: %d\n", len); + memcpy(topic, topicName, len); + topic[len] = 0; + } else { + topic[0] = 0; + strncat(topic, topicName, sizeof(topic) - 1); + } + dprintf(4, "topic: %s\n", topic); + + logProperties(&message->properties); + + /* Do we have a replyto user property? */ + replyto = 0; + dprintf(1, "hasProperty UP: %d\n", + MQTTProperties_hasProperty(&message->properties, + MQTTPROPERTY_CODE_USER_PROPERTY)); + if (MQTTProperties_hasProperty(&message->properties, + MQTTPROPERTY_CODE_USER_PROPERTY)) { + MQTTProperty *prop; + + prop = MQTTProperties_getProperty(&message->properties, + MQTTPROPERTY_CODE_USER_PROPERTY); + if (strncmp(prop->value.data.data, "replyto", prop->value.data.len) == 0) { + replyto = prop->value.value.data; + } + } + + if (s->cb) + s->cb(s->ctx, topic, message->payload, message->payloadlen, replyto); + +mqtt_getmsg_skip: + MQTTClient_freeMessage(&message); + MQTTClient_free(topicName); + dprintf(4, "returning...\n"); + return 1; +} + +int mqtt_newclient(struct mqtt_session *s) { + MQTTClient_createOptions opts = MQTTClient_createOptions_initializer; + int rc; + +#if MQTT_5 + opts.MQTTVersion = MQTTVERSION_5; +#else + opts.MQTTVersion = MQTTVERSION_3_1; +#endif + // rc = MQTTClient_create(&s->c, s->config.host, s->config.clientid, + // MQTTCLIENT_PERSISTENCE_NONE, NULL); + rc = MQTTClient_createWithOptions(&s->c, s->config.host, s->config.clientid, + MQTTCLIENT_PERSISTENCE_NONE, NULL, &opts); + dprintf(2, "create rc: %d\n", rc); + if (rc != MQTTCLIENT_SUCCESS) { + lprintf(LOG_SYSERR, "MQTTClient_create"); + return 1; + } + return 0; +} + +struct mqtt_session *mqtt_new(mqtt_config_t *conf, mqtt_callback_t *cb, + void *ctx) { + mqtt_session_t *s; + int rc; + + dprintf(1, + "mqtt_config: host: %s, clientid: %s, user: %s, pass: %s, lwt: %s\n", + conf->host, conf->clientid, conf->user, conf->pass, conf->lwt_topic); + s = calloc(1, sizeof(*s)); + if (!s) { + log_write(LOG_SYSERR, "mqtt_new: calloc"); + return 0; + } + memcpy(&s->config, conf, sizeof(*conf)); + if (mqtt_newclient(s)) { + free(s); + return 0; + } + s->cb = cb; + s->ctx = ctx; +#if 0 + if (strlen(conf->ssl_key)) { + s->ssl_opts = malloc(sizeof(*s->ssl_opts)); + if (s->ssl_opts) { + s->ssl_opts = MQTTClient_SSLOptions_initializer; + ssl_opts.keyStore = s->client_pem; + ssl_opts.trustStore = s->ca_crt; + } + } +#endif + + rc = MQTTClient_setCallbacks(s->c, s, mqtt_dc, mqtt_getmsg, 0); + dprintf(2, "setcb rc: %d\n", rc); + if (rc) { + free(s); + return 0; + } + return s; +} + +int mqtt_connect(mqtt_session_t *s, int interval) { +#if MQTT_5 + MQTTClient_connectOptions conn_opts = MQTTClient_connectOptions_initializer5; + MQTTResponse response = MQTTResponse_initializer; +#else + MQTTClient_connectOptions conn_opts = MQTTClient_connectOptions_initializer; +#endif + MQTTClient_willOptions will_opts = MQTTClient_willOptions_initializer; + int rc; + + if (!s) + return 1; + + dprintf(2, "interval: %d\n", interval); + +#if MQTT_5 + conn_opts.MQTTVersion = MQTTVERSION_5; + conn_opts.cleanstart = 1; +#else + conn_opts.MQTTVersion = MQTTVERSION_3_1; + conn_opts.cleansession = 1; +#endif + conn_opts.keepAliveInterval = interval; + conn_opts.ssl = s->ssl_opts; + if (strlen(s->config.lwt_topic)) { + will_opts.topicName = s->config.lwt_topic; + will_opts.message = "Offline"; + will_opts.retained = 1; + will_opts.qos = 1; + conn_opts.will = &will_opts; + } + + s->interval = interval; + if (strlen(s->config.user)) { + conn_opts.username = s->config.user; + if (strlen(s->config.pass)) + conn_opts.password = s->config.pass; + } +#if MQTT_5 + response = MQTTClient_connect5(s->c, &conn_opts, 0, 0); + rc = response.reasonCode; +#else + rc = MQTTClient_connect(s->c, &conn_opts); +#endif + dprintf(2, "rc: %d\n", rc); + if (rc != MQTTCLIENT_SUCCESS) { + if (rc == 5) { + log_error("MQTT: bad username or password\n"); + return 1; + } else { + char *p = (char *)MQTTReasonCode_toString(rc); + log_error("MQTTClient_connect: %s\n", p ? p : "cant connect"); + } + return 1; + } else if (strlen(s->config.lwt_topic)) { + mqtt_pub(s, s->config.lwt_topic, "Online", 1); + } + return 0; +} + +int mqtt_disconnect(mqtt_session_t *s, int timeout) { + int rc; + + dprintf(2, "timeout: %d\n", timeout); + + if (!s) + return 1; +#if MQTT_5 + rc = MQTTClient_disconnect5(s->c, timeout, MQTTREASONCODE_SUCCESS, 0); +#else + rc = MQTTClient_disconnect(s->c, timeout); +#endif + dprintf(2, "rc: %d\n", rc); + return rc; +} + +int mqtt_destroy(mqtt_session_t *s) { + if (!s) + return 1; + MQTTClient_destroy(&s->c); + free(s); + return 0; +} + +int mqtt_pub(mqtt_session_t *s, char *topic, char *message, int retain) { + MQTTClient_message pubmsg = MQTTClient_message_initializer; + MQTTClient_deliveryToken token; + MQTTProperty property; +#if MQTT_5 + MQTTResponse response = MQTTResponse_initializer; +#endif + int rc; + + dprintf(2, "topic: %s, message: %s\n", topic, message); + + if (message) { + pubmsg.payload = message; + pubmsg.payloadlen = strlen(message); + } + + /* Add a replyto user prooperty */ + property.identifier = MQTTPROPERTY_CODE_USER_PROPERTY; + property.value.data.data = "replyto"; + property.value.data.len = strlen(property.value.data.data); + property.value.value.data = s->config.clientid; + property.value.value.len = strlen(property.value.value.data); + MQTTProperties_add(&pubmsg.properties, &property); + + logProperties(&pubmsg.properties); + + pubmsg.qos = 2; + pubmsg.retained = retain; + token = 0; +#if MQTT_5 + response = MQTTClient_publishMessage5(s->c, topic, &pubmsg, &token); + rc = response.reasonCode; + MQTTResponse_free(response); +#else + rc = MQTTClient_publishMessage(s->c, topic, &pubmsg, &token); +#endif + rc = MQTTClient_waitForCompletion(s->c, token, TIMEOUT); + dprintf(2, "rc: %d\n", rc); + if (rc != MQTTCLIENT_SUCCESS) + return 1; + dprintf(2, "delivered message... token: %d\n", token); + MQTTProperties_free(&pubmsg.properties); + return 0; +} + +int mqtt_setcb(mqtt_session_t *s, void *ctx, MQTTClient_connectionLost *cl, + MQTTClient_messageArrived *ma, MQTTClient_deliveryComplete *dc) { + int rc; + + dprintf(2, "s: %p, ctx: %p, ma: %p\n", s, ctx, ma); + rc = MQTTClient_setCallbacks(s->c, ctx, cl, ma, dc); + dprintf(2, "rc: %d\n", rc); + if (rc) + log_write(LOG_ERROR, "MQTTClient_setCallbacks: rc: %d\n", rc); + return rc; +} + +int mqtt_sub(mqtt_session_t *s, char *topic) { +#if MQTT_5 + MQTTSubscribe_options opts = MQTTSubscribe_options_initializer; + MQTTResponse response = MQTTResponse_initializer; +#endif + int rc; + + dprintf(2, "s: %p, topic: %s\n", s, topic); +#if MQTT_5 + opts.noLocal = 1; + response = MQTTClient_subscribe5(s->c, topic, 1, &opts, 0); + rc = response.reasonCode; + MQTTResponse_free(response); +#else + rc = MQTTClient_subscribe(s->c, topic, 1); +#endif + if (rc == MQTTREASONCODE_GRANTED_QOS_1) + rc = 0; + dprintf(2, "rc: %d\n", rc); + return rc; +} + +int mqtt_submany(mqtt_session_t *s, int count, char **topic) { + int i, rc, *qos; + + dprintf(2, "s: %p, count: %d\n", s, count); + qos = calloc(1, sizeof(int) * count); + if (!qos) { + log_write(LOG_SYSERR, "mqtt_submany: calloc"); + return 1; + } + for (i = 0; i < count; i++) + qos[i] = 1; + rc = MQTTClient_subscribeMany(s->c, count, topic, qos); + free(qos); + dprintf(2, "rc: %d\n", rc); + return rc; +} + +int mqtt_unsub(mqtt_session_t *s, char *topic) { +#if MQTT_5 + MQTTResponse response = MQTTResponse_initializer; +#endif + int rc; + + dprintf(2, "s: %p, topic: %s\n", s, topic); +#if MQTT_5 + response = MQTTClient_unsubscribe5(s->c, topic, 0); + rc = response.reasonCode; + MQTTResponse_free(response); +#else + rc = MQTTClient_unsubscribe(s->c, topic); +#endif + dprintf(2, "rc: %d\n", rc); + return rc; +} + +int mqtt_unsubmany(mqtt_session_t *s, int count, char **topic) { + int rc; + + dprintf(2, "s: %p, topic: %p\n", s, topic); + rc = MQTTClient_unsubscribeMany(s->c, count, topic); + dprintf(2, "rc: %d\n", rc); + return rc; +} + +#if 0 +int mqtt_fullsend(char *address, char *clientid, char *message, char *topic, char *user, char *pass) { + mqtt_config_t config; + mqtt_session_t *s; + int rc; + + memset(&config,0,sizeof(config)); + strncat(config.host,address,sizeof(config.host)-1); + strncat(config.clientid,clientid,sizeof(config.clientid)-1); + strncat(config.user,user,sizeof(config.user)-1); + strncat(config.pass,pass,sizeof(config.pass)-1); + s = mqtt_new(&config,0,0); + if (!s) return 1; + + rc = 1; + if (mqtt_connect(s,20)) goto mqtt_send_error; + if (mqtt_send(s,topic,message,10)) goto mqtt_send_error; + rc = 0; +mqtt_send_error: + mqtt_disconnect(s,10); + mqtt_destroy(s); + return rc; +} +#endif +#else +struct mqtt_session *mqtt_new(mqtt_config_t *conf, mqtt_callback_t *cb, + void *ctx) { + mqtt_session_t *s; + s = calloc(1, sizeof(*s)); + if (!s) { + log_write(LOG_SYSERR, "mqtt_new: calloc"); + return 0; + } + memcpy(&s->config, conf, sizeof(*conf)); + s->cb = cb; + s->ctx = ctx; + return 0; +} +int mqtt_connect(mqtt_session_t *s, int interval) { return 0; } +int mqtt_disconnect(mqtt_session_t *s, int timeout) { return 0; } +int mqtt_destroy(mqtt_session_t *s) { return 0; } +int mqtt_pub(mqtt_session_t *s, char *topic, char *message, int retain) { + return 0; +} +int mqtt_sub(mqtt_session_t *s, char *topic) { return 0; } +#endif diff --git a/sensors/bms/packages/jbdtool/mqtt.h b/sensors/bms/packages/jbdtool/mqtt.h new file mode 100644 index 00000000..dd49c437 --- /dev/null +++ b/sensors/bms/packages/jbdtool/mqtt.h @@ -0,0 +1,54 @@ + +/* +Copyright (c) 2021, Stephen P. Shoecraft +All rights reserved. + +This source code is licensed under the BSD-style license found in the +LICENSE file in the root directory of this source tree. +*/ + +#ifndef __SOLARD_MQTT_H +#define __SOLARD_MQTT_H + +/* We use the paho mqtt.c library */ +#include + +struct mqtt_session; +typedef struct mqtt_session mqtt_session_t; + +#define MQTT_HOST_LEN 64 +#define MQTT_USER_LEN 32 +#define MQTT_PASS_LEN 32 +#define MQTT_CLIENTID_LEN 64 +#define MQTT_TOPIC_LEN 128 + +struct mqtt_config { + char host[MQTT_HOST_LEN]; + char user[MQTT_USER_LEN]; + char pass[MQTT_PASS_LEN]; + char clientid[MQTT_CLIENTID_LEN]; + char lwt_topic[MQTT_TOPIC_LEN]; +}; +typedef struct mqtt_config mqtt_config_t; + +typedef void(mqtt_callback_t)(void *, char *, char *, int, char *); + +int mqtt_parse_config(mqtt_config_t *conf, char *str); +int mqtt_get_config(void *, mqtt_config_t *); +mqtt_session_t *mqtt_new(mqtt_config_t *, mqtt_callback_t *, void *); +int mqtt_newclient(mqtt_session_t *); +int mqtt_connect(mqtt_session_t *s, int interval); +int mqtt_disconnect(mqtt_session_t *s, int timeout); +int mqtt_destroy(mqtt_session_t *s); +int mqtt_send(mqtt_session_t *s, char *topic, char *message, int timeout); +int mqtt_sub(mqtt_session_t *s, char *topic); +int mqtt_unsub(mqtt_session_t *s, char *topic); +int mqtt_setcb(mqtt_session_t *s, void *ctx, MQTTClient_connectionLost *cl, + MQTTClient_messageArrived *ma, MQTTClient_deliveryComplete *dc); +int mqtt_pub(mqtt_session_t *s, char *topic, char *message, int retain); + +int mqtt_dosend(mqtt_session_t *m, char *topic, char *message); +int mqtt_fullsend(char *address, char *clientid, char *message, char *topic, + char *user, char *pass); + +#endif diff --git a/sensors/bms/packages/jbdtool/mybmm.h b/sensors/bms/packages/jbdtool/mybmm.h new file mode 100644 index 00000000..ddb0ef98 --- /dev/null +++ b/sensors/bms/packages/jbdtool/mybmm.h @@ -0,0 +1,63 @@ + +#ifndef __MYBMS_H__ +#define __MYBMS_H__ + +#include +#include +#include +#include +#include +#include +#include + +#include "debug.h" +#include "list.h" +#include "utils.h" +#include "worker.h" + +// typedef unsigned char uint8_t; + +#define MYBMM_MAX_INVERTERS 32 +#define MYBMM_MAX_PACKS 32 +#define MYBMM_PACK_NAME_LEN 32 +#define MYBMM_PACK_TYPE_LEN 8 +#define MYBMM_MODULE_NAME_LEN 32 +//#define MYBMM_INSTANCE_NAME_LEN 32 +//#define MYBMM_TRANSPORT_NAME_LEN 8 + +#include "config.h" +#include "module.h" +#include "pack.h" + +#define MYBMM_TARGET_LEN 32 + +/* States */ +#define MYBMM_SHUTDOWN 0x0001 /* Emergency Shutdown */ +#define MYBMM_GEN_ACTIVE 0x0002 /* Generator Active */ +#define MYBMM_CHARGING 0x0004 /* Charging */ +#define MYBMM_CRIT_CELLVOLT 0x0008 /* A cell voltage is critical */ +#define MYBMM_FORCE_SOC 0x0010 /* Force State of Charge */ +#define MYBMM_CONFIG_DIRTY 0x0020 /* Config has been updated */ +#define MYBMM_GRID_ENABLE 0x0040 /* Grid is enabled */ +#define MYBMM_GEN_ENABLE 0x0080 /* Generator is enabled */ +#define MYBMM_GRID_ACTIVE 0x0100 /* Grid is active */ + +/* State Macros */ +#define mybmm_set_state(c, v) (c->state |= (v)) +#define mybmm_clear_state(c, v) (c->state &= (~v)) +#define mybmm_check_state(c, v) ((c->state & v) != 0) +#define mybmm_set_cap(c, v) (c->capabilities |= (v)) +#define mybmm_clear_cap(c, v) (c->capabilities &= (~v)) +#define mybmm_check_cap(c, v) ((c->capabilities & v) != 0) +#define mybmm_is_shutdown(c) mybmm_check_state(c, MYBMM_SHUTDOWN) +#define mybmm_is_gen(c) mybmm_check_state(c, MYBMM_GEN_ACTIVE) +#define mybmm_is_charging(c) mybmm_check_state(c, MYBMM_CHARGING) +#define mybmm_is_critvolt(c) mybmm_check_state(c, MYBMM_CRIT_CELLVOLT) + +#define mybmm_force_soc(c, v) \ + { \ + c->soc = v; \ + mybmm_set_state(c, MYBMM_FORCE_SOC); \ + } + +#endif diff --git a/sensors/bms/packages/jbdtool/pack.h b/sensors/bms/packages/jbdtool/pack.h new file mode 100644 index 00000000..41c61fac --- /dev/null +++ b/sensors/bms/packages/jbdtool/pack.h @@ -0,0 +1,80 @@ + +#ifndef __PACK_H +#define __PACK_H + +#include "config.h" +#include + +#define MYBMM_PACK_NAME_LEN 32 +#define MYBMM_PACK_MAX_TEMPS 8 +#define MYBMM_PACK_MAX_CELLS 32 + +struct battery_cell { + char *label; + unsigned char status; + float voltage; + float resistance; +}; + +#define CELL_STATUS_OK 0x00 +#define CELL_STATUS_WARNING 0x01 +#define CELL_STATUS_ERROR 0x02 +#define CELL_STATUS_UNDERVOLT 0x40 +#define CELL_STATUS_OVERVOLT 0x80 + +struct mybmm_pack { + mybmm_config_t *conf; /* back ptr */ + void *mqtt_handle; /* MQTT connection handle */ + char name[MYBMM_PACK_NAME_LEN]; /* Pack name */ + char uuid[37]; /* UUID */ + char type[32]; /* BMS name */ + char transport[32]; /* Transport name */ + char target[32]; /* Transport target */ + char opts[64]; /* Pack-specific options */ + uint16_t state; /* Pack state */ + int failed; /* Update fail count */ + int error; /* Error code, from BMS */ + char errmsg[256]; /* Error message, updated by BMS */ + float capacity; /* Battery pack capacity, in AH */ + float voltage; /* Pack voltage */ + float current; /* Pack current */ + int status; /* Pack status, updated by BMS */ + int ntemps; /* Number of temps */ + float temps[MYBMM_PACK_MAX_TEMPS]; /* Temp values */ + int cells; /* Number of cells, updated by BMS */ + // battery_cell_t *cells; /* Cell info */ + float cellvolt[MYBMM_PACK_MAX_CELLS]; /* Per-cell voltages, updated by BMS */ + float + cellres[MYBMM_PACK_MAX_CELLS]; /* Per-cell resistances, updated by BMS */ + float cell_min; + float cell_max; + float cell_diff; + float cell_avg; + uint32_t balancebits; /* Balance bitmask */ + uint16_t capabilities; /* BMS Capability Mask */ + void *handle; /* BMS Handle */ + mybmm_module_open_t open; /* BMS Open */ + mybmm_module_read_t read; /* BMS Read */ + mybmm_module_close_t close; /* BMS Close */ + mybmm_module_control_t control; /* BMS Control */ + time_t last_update; +}; +typedef struct mybmm_pack mybmm_pack_t; + +/* Pack states */ +#define MYBMM_PACK_STATE_UPDATED 0x01 +#define MYBMM_PACK_STATE_CHARGING 0x02 +#define MYBMM_PACK_STATE_DISCHARGING 0x04 +#define MYBMM_PACK_STATE_BALANCING 0x08 + +int pack_update(mybmm_pack_t *pp); +int pack_update_all(mybmm_config_t *, int); +int pack_add(mybmm_config_t *conf, char *packname, mybmm_pack_t *pp); +int pack_init(mybmm_config_t *conf); +int pack_start_update(mybmm_config_t *conf); +int pack_check(mybmm_config_t *conf, mybmm_pack_t *pp); +#ifdef MQTT +int pack_send_mqtt(mybmm_config_t *conf, mybmm_pack_t *pp); +#endif + +#endif diff --git a/sensors/bms/packages/jbdtool/parson.c b/sensors/bms/packages/jbdtool/parson.c new file mode 100644 index 00000000..78ffdfa4 --- /dev/null +++ b/sensors/bms/packages/jbdtool/parson.c @@ -0,0 +1,2364 @@ +/* + SPDX-License-Identifier: MIT + + Parson 1.1.0 ( http://kgabis.github.com/parson/ ) + Copyright (c) 2012 - 2020 Krzysztof Gabis + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ +#ifdef _MSC_VER +#ifndef _CRT_SECURE_NO_WARNINGS +#define _CRT_SECURE_NO_WARNINGS +#endif /* _CRT_SECURE_NO_WARNINGS */ +#endif /* _MSC_VER */ + +#include "parson.h" + +#include +#include +#include +#include +#include +#include + +/* Apparently sscanf is not implemented in some "standard" libraries, so don't + * use it, if you don't have to. */ +#define sscanf THINK_TWICE_ABOUT_USING_SSCANF + +#define STARTING_CAPACITY 16 +#define MAX_NESTING 2048 + +#define FLOAT_FORMAT \ + "%1.17g" /* do not increase precision without incresing NUM_BUF_SIZE */ +#define NUM_BUF_SIZE \ + 64 /* double printed with "%1.17g" shouldn't be longer than 25 bytes so \ + let's be paranoid and use 64 */ + +#define SIZEOF_TOKEN(a) (sizeof(a) - 1) +#define SKIP_CHAR(str) ((*str)++) +#define SKIP_WHITESPACES(str) \ + while (isspace((unsigned char)(**str))) { \ + SKIP_CHAR(str); \ + } +#define MAX(a, b) ((a) > (b) ? (a) : (b)) + +#undef malloc +#undef free + +#if defined(isnan) && defined(isinf) +#define IS_NUMBER_INVALID(x) (isnan((x)) || isinf((x))) +#else +#define IS_NUMBER_INVALID(x) (((x)*0.0) != 0.0) +#endif + +static JSON_Malloc_Function parson_malloc = malloc; +static JSON_Free_Function parson_free = free; + +static int parson_escape_slashes = 1; + +#define IS_CONT(b) \ + (((unsigned char)(b)&0xC0) == 0x80) /* is utf-8 continuation byte */ + +typedef struct json_string { + char *chars; + size_t length; +} JSON_String; + +/* Type definitions */ +typedef union json_value_value { + JSON_String string; + double number; + JSON_Object *object; + JSON_Array *array; + int boolean; + int null; +} JSON_Value_Value; + +struct json_value_t { + JSON_Value *parent; + JSON_Value_Type type; + JSON_Value_Value value; +}; + +struct json_object_t { + JSON_Value *wrapping_value; + char **names; + JSON_Value **values; + size_t count; + size_t capacity; +}; + +struct json_array_t { + JSON_Value *wrapping_value; + JSON_Value **items; + size_t count; + size_t capacity; +}; + +/* Various */ +static char *read_file(const char *filename); +static void remove_comments(char *string, const char *start_token, + const char *end_token); +static char *parson_strndup(const char *string, size_t n); +static char *parson_strdup(const char *string); +static int hex_char_to_int(char c); +static int parse_utf16_hex(const char *string, unsigned int *result); +static int num_bytes_in_utf8_sequence(unsigned char c); +static int verify_utf8_sequence(const unsigned char *string, int *len); +static int is_valid_utf8(const char *string, size_t string_len); +static int is_decimal(const char *string, size_t length); + +/* JSON Object */ +static JSON_Object *json_object_init(JSON_Value *wrapping_value); +static JSON_Status json_object_add(JSON_Object *object, const char *name, + JSON_Value *value); +static JSON_Status json_object_addn(JSON_Object *object, const char *name, + size_t name_len, JSON_Value *value); +static JSON_Status json_object_resize(JSON_Object *object, size_t new_capacity); +static JSON_Value *json_object_getn_value(const JSON_Object *object, + const char *name, size_t name_len); +static JSON_Status json_object_remove_internal(JSON_Object *object, + const char *name, + int free_value); +static JSON_Status json_object_dotremove_internal(JSON_Object *object, + const char *name, + int free_value); +static void json_object_free(JSON_Object *object); + +/* JSON Array */ +static JSON_Array *json_array_init(JSON_Value *wrapping_value); +static JSON_Status json_array_add(JSON_Array *array, JSON_Value *value); +static JSON_Status json_array_resize(JSON_Array *array, size_t new_capacity); +static void json_array_free(JSON_Array *array); + +/* JSON Value */ +static JSON_Value *json_value_init_string_no_copy(char *string, size_t length); +static const JSON_String *json_value_get_string_desc(const JSON_Value *value); + +/* Parser */ +static JSON_Status skip_quotes(const char **string); +static int parse_utf16(const char **unprocessed, char **processed); +static char *process_string(const char *input, size_t input_len, + size_t *output_len); +static char *get_quoted_string(const char **string, size_t *output_string_len); +static JSON_Value *parse_object_value(const char **string, size_t nesting); +static JSON_Value *parse_array_value(const char **string, size_t nesting); +static JSON_Value *parse_string_value(const char **string); +static JSON_Value *parse_boolean_value(const char **string); +static JSON_Value *parse_number_value(const char **string); +static JSON_Value *parse_null_value(const char **string); +static JSON_Value *parse_value(const char **string, size_t nesting); + +/* Serialization */ +static int json_serialize_to_buffer_r(const JSON_Value *value, char *buf, + int level, int is_pretty, char *num_buf); +static int json_serialize_string(const char *string, size_t len, char *buf); +static int append_indent(char *buf, int level); +static int append_string(char *buf, const char *string); + +/* Various */ +static char *parson_strndup(const char *string, size_t n) { + /* We expect the caller has validated that 'n' fits within the input buffer. + */ + char *output_string = (char *)parson_malloc(n + 1); + if (!output_string) { + return NULL; + } + output_string[n] = '\0'; + memcpy(output_string, string, n); + return output_string; +} + +static char *parson_strdup(const char *string) { + return parson_strndup(string, strlen(string)); +} + +static int hex_char_to_int(char c) { + if (c >= '0' && c <= '9') { + return c - '0'; + } else if (c >= 'a' && c <= 'f') { + return c - 'a' + 10; + } else if (c >= 'A' && c <= 'F') { + return c - 'A' + 10; + } + return -1; +} + +static int parse_utf16_hex(const char *s, unsigned int *result) { + int x1, x2, x3, x4; + if (s[0] == '\0' || s[1] == '\0' || s[2] == '\0' || s[3] == '\0') { + return 0; + } + x1 = hex_char_to_int(s[0]); + x2 = hex_char_to_int(s[1]); + x3 = hex_char_to_int(s[2]); + x4 = hex_char_to_int(s[3]); + if (x1 == -1 || x2 == -1 || x3 == -1 || x4 == -1) { + return 0; + } + *result = (unsigned int)((x1 << 12) | (x2 << 8) | (x3 << 4) | x4); + return 1; +} + +static int num_bytes_in_utf8_sequence(unsigned char c) { + if (c == 0xC0 || c == 0xC1 || c > 0xF4 || IS_CONT(c)) { + return 0; + } else if ((c & 0x80) == 0) { /* 0xxxxxxx */ + return 1; + } else if ((c & 0xE0) == 0xC0) { /* 110xxxxx */ + return 2; + } else if ((c & 0xF0) == 0xE0) { /* 1110xxxx */ + return 3; + } else if ((c & 0xF8) == 0xF0) { /* 11110xxx */ + return 4; + } + return 0; /* won't happen */ +} + +static int verify_utf8_sequence(const unsigned char *string, int *len) { + unsigned int cp = 0; + *len = num_bytes_in_utf8_sequence(string[0]); + + if (*len == 1) { + cp = string[0]; + } else if (*len == 2 && IS_CONT(string[1])) { + cp = string[0] & 0x1F; + cp = (cp << 6) | (string[1] & 0x3F); + } else if (*len == 3 && IS_CONT(string[1]) && IS_CONT(string[2])) { + cp = ((unsigned char)string[0]) & 0xF; + cp = (cp << 6) | (string[1] & 0x3F); + cp = (cp << 6) | (string[2] & 0x3F); + } else if (*len == 4 && IS_CONT(string[1]) && IS_CONT(string[2]) && + IS_CONT(string[3])) { + cp = string[0] & 0x7; + cp = (cp << 6) | (string[1] & 0x3F); + cp = (cp << 6) | (string[2] & 0x3F); + cp = (cp << 6) | (string[3] & 0x3F); + } else { + return 0; + } + + /* overlong encodings */ + if ((cp < 0x80 && *len > 1) || (cp < 0x800 && *len > 2) || + (cp < 0x10000 && *len > 3)) { + return 0; + } + + /* invalid unicode */ + if (cp > 0x10FFFF) { + return 0; + } + + /* surrogate halves */ + if (cp >= 0xD800 && cp <= 0xDFFF) { + return 0; + } + + return 1; +} + +static int is_valid_utf8(const char *string, size_t string_len) { + int len = 0; + const char *string_end = string + string_len; + while (string < string_end) { + if (!verify_utf8_sequence((const unsigned char *)string, &len)) { + return 0; + } + string += len; + } + return 1; +} + +static int is_decimal(const char *string, size_t length) { + if (length > 1 && string[0] == '0' && string[1] != '.') { + return 0; + } + if (length > 2 && !strncmp(string, "-0", 2) && string[2] != '.') { + return 0; + } + while (length--) { + if (strchr("xX", string[length])) { + return 0; + } + } + return 1; +} + +static char *read_file(const char *filename) { + FILE *fp = fopen(filename, "r"); + size_t size_to_read = 0; + size_t size_read = 0; + long pos; + char *file_contents; + if (!fp) { + return NULL; + } + fseek(fp, 0L, SEEK_END); + pos = ftell(fp); + if (pos < 0) { + fclose(fp); + return NULL; + } + size_to_read = pos; + rewind(fp); + file_contents = (char *)parson_malloc(sizeof(char) * (size_to_read + 1)); + if (!file_contents) { + fclose(fp); + return NULL; + } + size_read = fread(file_contents, 1, size_to_read, fp); + if (size_read == 0 || ferror(fp)) { + fclose(fp); + parson_free(file_contents); + return NULL; + } + fclose(fp); + file_contents[size_read] = '\0'; + return file_contents; +} + +static void remove_comments(char *string, const char *start_token, + const char *end_token) { + int in_string = 0, escaped = 0; + size_t i; + char *ptr = NULL, current_char; + size_t start_token_len = strlen(start_token); + size_t end_token_len = strlen(end_token); + if (start_token_len == 0 || end_token_len == 0) { + return; + } + while ((current_char = *string) != '\0') { + if (current_char == '\\' && !escaped) { + escaped = 1; + string++; + continue; + } else if (current_char == '\"' && !escaped) { + in_string = !in_string; + } else if (!in_string && + strncmp(string, start_token, start_token_len) == 0) { + for (i = 0; i < start_token_len; i++) { + string[i] = ' '; + } + string = string + start_token_len; + ptr = strstr(string, end_token); + if (!ptr) { + return; + } + for (i = 0; i < (ptr - string) + end_token_len; i++) { + string[i] = ' '; + } + string = ptr + end_token_len - 1; + } + escaped = 0; + string++; + } +} + +/* JSON Object */ +static JSON_Object *json_object_init(JSON_Value *wrapping_value) { + JSON_Object *new_obj = (JSON_Object *)parson_malloc(sizeof(JSON_Object)); + if (new_obj == NULL) { + return NULL; + } + new_obj->wrapping_value = wrapping_value; + new_obj->names = (char **)NULL; + new_obj->values = (JSON_Value **)NULL; + new_obj->capacity = 0; + new_obj->count = 0; + return new_obj; +} + +static JSON_Status json_object_add(JSON_Object *object, const char *name, + JSON_Value *value) { + if (name == NULL) { + return JSONFailure; + } + return json_object_addn(object, name, strlen(name), value); +} + +static JSON_Status json_object_addn(JSON_Object *object, const char *name, + size_t name_len, JSON_Value *value) { + size_t index = 0; + if (object == NULL || name == NULL || value == NULL) { + return JSONFailure; + } + if (json_object_getn_value(object, name, name_len) != NULL) { + return JSONFailure; + } + if (object->count >= object->capacity) { + size_t new_capacity = MAX(object->capacity * 2, STARTING_CAPACITY); + if (json_object_resize(object, new_capacity) == JSONFailure) { + return JSONFailure; + } + } + index = object->count; + object->names[index] = parson_strndup(name, name_len); + if (object->names[index] == NULL) { + return JSONFailure; + } + value->parent = json_object_get_wrapping_value(object); + object->values[index] = value; + object->count++; + return JSONSuccess; +} + +static JSON_Status json_object_resize(JSON_Object *object, + size_t new_capacity) { + char **temp_names = NULL; + JSON_Value **temp_values = NULL; + + if ((object->names == NULL && object->values != NULL) || + (object->names != NULL && object->values == NULL) || new_capacity == 0) { + return JSONFailure; /* Shouldn't happen */ + } + temp_names = (char **)parson_malloc(new_capacity * sizeof(char *)); + if (temp_names == NULL) { + return JSONFailure; + } + temp_values = + (JSON_Value **)parson_malloc(new_capacity * sizeof(JSON_Value *)); + if (temp_values == NULL) { + parson_free(temp_names); + return JSONFailure; + } + if (object->names != NULL && object->values != NULL && object->count > 0) { + memcpy(temp_names, object->names, object->count * sizeof(char *)); + memcpy(temp_values, object->values, object->count * sizeof(JSON_Value *)); + } + parson_free(object->names); + parson_free(object->values); + object->names = temp_names; + object->values = temp_values; + object->capacity = new_capacity; + return JSONSuccess; +} + +static JSON_Value *json_object_getn_value(const JSON_Object *object, + const char *name, size_t name_len) { + size_t i, name_length; + for (i = 0; i < json_object_get_count(object); i++) { + name_length = strlen(object->names[i]); + if (name_length != name_len) { + continue; + } + if (strncmp(object->names[i], name, name_len) == 0) { + return object->values[i]; + } + } + return NULL; +} + +static JSON_Status json_object_remove_internal(JSON_Object *object, + const char *name, + int free_value) { + size_t i = 0, last_item_index = 0; + if (object == NULL || json_object_get_value(object, name) == NULL) { + return JSONFailure; + } + last_item_index = json_object_get_count(object) - 1; + for (i = 0; i < json_object_get_count(object); i++) { + if (strcmp(object->names[i], name) == 0) { + parson_free(object->names[i]); + if (free_value) { + json_value_free(object->values[i]); + } + if (i != + last_item_index) { /* Replace key value pair with one from the end */ + object->names[i] = object->names[last_item_index]; + object->values[i] = object->values[last_item_index]; + } + object->count -= 1; + return JSONSuccess; + } + } + return JSONFailure; /* No execution path should end here */ +} + +static JSON_Status json_object_dotremove_internal(JSON_Object *object, + const char *name, + int free_value) { + JSON_Value *temp_value = NULL; + JSON_Object *temp_object = NULL; + const char *dot_pos = strchr(name, '.'); + if (dot_pos == NULL) { + return json_object_remove_internal(object, name, free_value); + } + temp_value = json_object_getn_value(object, name, dot_pos - name); + if (json_value_get_type(temp_value) != JSONObject) { + return JSONFailure; + } + temp_object = json_value_get_object(temp_value); + return json_object_dotremove_internal(temp_object, dot_pos + 1, free_value); +} + +static void json_object_free(JSON_Object *object) { + size_t i; + for (i = 0; i < object->count; i++) { + parson_free(object->names[i]); + json_value_free(object->values[i]); + } + parson_free(object->names); + parson_free(object->values); + parson_free(object); +} + +/* JSON Array */ +static JSON_Array *json_array_init(JSON_Value *wrapping_value) { + JSON_Array *new_array = (JSON_Array *)parson_malloc(sizeof(JSON_Array)); + if (new_array == NULL) { + return NULL; + } + new_array->wrapping_value = wrapping_value; + new_array->items = (JSON_Value **)NULL; + new_array->capacity = 0; + new_array->count = 0; + return new_array; +} + +static JSON_Status json_array_add(JSON_Array *array, JSON_Value *value) { + if (array->count >= array->capacity) { + size_t new_capacity = MAX(array->capacity * 2, STARTING_CAPACITY); + if (json_array_resize(array, new_capacity) == JSONFailure) { + return JSONFailure; + } + } + value->parent = json_array_get_wrapping_value(array); + array->items[array->count] = value; + array->count++; + return JSONSuccess; +} + +static JSON_Status json_array_resize(JSON_Array *array, size_t new_capacity) { + JSON_Value **new_items = NULL; + if (new_capacity == 0) { + return JSONFailure; + } + new_items = (JSON_Value **)parson_malloc(new_capacity * sizeof(JSON_Value *)); + if (new_items == NULL) { + return JSONFailure; + } + if (array->items != NULL && array->count > 0) { + memcpy(new_items, array->items, array->count * sizeof(JSON_Value *)); + } + parson_free(array->items); + array->items = new_items; + array->capacity = new_capacity; + return JSONSuccess; +} + +static void json_array_free(JSON_Array *array) { + size_t i; + for (i = 0; i < array->count; i++) { + json_value_free(array->items[i]); + } + parson_free(array->items); + parson_free(array); +} + +/* JSON Value */ +static JSON_Value *json_value_init_string_no_copy(char *string, size_t length) { + JSON_Value *new_value = (JSON_Value *)parson_malloc(sizeof(JSON_Value)); + if (!new_value) { + return NULL; + } + new_value->parent = NULL; + new_value->type = JSONString; + new_value->value.string.chars = string; + new_value->value.string.length = length; + return new_value; +} + +/* Parser */ +static JSON_Status skip_quotes(const char **string) { + if (**string != '\"') { + return JSONFailure; + } + SKIP_CHAR(string); + while (**string != '\"') { + if (**string == '\0') { + return JSONFailure; + } else if (**string == '\\') { + SKIP_CHAR(string); + if (**string == '\0') { + return JSONFailure; + } + } + SKIP_CHAR(string); + } + SKIP_CHAR(string); + return JSONSuccess; +} + +static int parse_utf16(const char **unprocessed, char **processed) { + unsigned int cp, lead, trail; + int parse_succeeded = 0; + char *processed_ptr = *processed; + const char *unprocessed_ptr = *unprocessed; + unprocessed_ptr++; /* skips u */ + parse_succeeded = parse_utf16_hex(unprocessed_ptr, &cp); + if (!parse_succeeded) { + return JSONFailure; + } + if (cp < 0x80) { + processed_ptr[0] = (char)cp; /* 0xxxxxxx */ + } else if (cp < 0x800) { + processed_ptr[0] = ((cp >> 6) & 0x1F) | 0xC0; /* 110xxxxx */ + processed_ptr[1] = ((cp)&0x3F) | 0x80; /* 10xxxxxx */ + processed_ptr += 1; + } else if (cp < 0xD800 || cp > 0xDFFF) { + processed_ptr[0] = ((cp >> 12) & 0x0F) | 0xE0; /* 1110xxxx */ + processed_ptr[1] = ((cp >> 6) & 0x3F) | 0x80; /* 10xxxxxx */ + processed_ptr[2] = ((cp)&0x3F) | 0x80; /* 10xxxxxx */ + processed_ptr += 2; + } else if (cp >= 0xD800 && + cp <= 0xDBFF) { /* lead surrogate (0xD800..0xDBFF) */ + lead = cp; + unprocessed_ptr += 4; /* should always be within the buffer, otherwise + previous sscanf would fail */ + if (*unprocessed_ptr++ != '\\' || *unprocessed_ptr++ != 'u') { + return JSONFailure; + } + parse_succeeded = parse_utf16_hex(unprocessed_ptr, &trail); + if (!parse_succeeded || trail < 0xDC00 || + trail > 0xDFFF) { /* valid trail surrogate? (0xDC00..0xDFFF) */ + return JSONFailure; + } + cp = ((((lead - 0xD800) & 0x3FF) << 10) | ((trail - 0xDC00) & 0x3FF)) + + 0x010000; + processed_ptr[0] = (((cp >> 18) & 0x07) | 0xF0); /* 11110xxx */ + processed_ptr[1] = (((cp >> 12) & 0x3F) | 0x80); /* 10xxxxxx */ + processed_ptr[2] = (((cp >> 6) & 0x3F) | 0x80); /* 10xxxxxx */ + processed_ptr[3] = (((cp)&0x3F) | 0x80); /* 10xxxxxx */ + processed_ptr += 3; + } else { /* trail surrogate before lead surrogate */ + return JSONFailure; + } + unprocessed_ptr += 3; + *processed = processed_ptr; + *unprocessed = unprocessed_ptr; + return JSONSuccess; +} + +/* Copies and processes passed string up to supplied length. +Example: "\u006Corem ipsum" -> lorem ipsum */ +static char *process_string(const char *input, size_t input_len, + size_t *output_len) { + const char *input_ptr = input; + size_t initial_size = (input_len + 1) * sizeof(char); + size_t final_size = 0; + char *output = NULL, *output_ptr = NULL, *resized_output = NULL; + output = (char *)parson_malloc(initial_size); + if (output == NULL) { + goto error; + } + output_ptr = output; + while ((*input_ptr != '\0') && (size_t)(input_ptr - input) < input_len) { + if (*input_ptr == '\\') { + input_ptr++; + switch (*input_ptr) { + case '\"': + *output_ptr = '\"'; + break; + case '\\': + *output_ptr = '\\'; + break; + case '/': + *output_ptr = '/'; + break; + case 'b': + *output_ptr = '\b'; + break; + case 'f': + *output_ptr = '\f'; + break; + case 'n': + *output_ptr = '\n'; + break; + case 'r': + *output_ptr = '\r'; + break; + case 't': + *output_ptr = '\t'; + break; + case 'u': + if (parse_utf16(&input_ptr, &output_ptr) == JSONFailure) { + goto error; + } + break; + default: + goto error; + } + } else if ((unsigned char)*input_ptr < 0x20) { + goto error; /* 0x00-0x19 are invalid characters for json string + (http://www.ietf.org/rfc/rfc4627.txt) */ + } else { + *output_ptr = *input_ptr; + } + output_ptr++; + input_ptr++; + } + *output_ptr = '\0'; + /* resize to new length */ + final_size = (size_t)(output_ptr - output) + 1; + /* todo: don't resize if final_size == initial_size */ + resized_output = (char *)parson_malloc(final_size); + if (resized_output == NULL) { + goto error; + } + memcpy(resized_output, output, final_size); + *output_len = final_size - 1; + parson_free(output); + return resized_output; +error: + parson_free(output); + return NULL; +} + +/* Return processed contents of a string between quotes and + skips passed argument to a matching quote. */ +static char *get_quoted_string(const char **string, size_t *output_string_len) { + const char *string_start = *string; + size_t input_string_len = 0; + JSON_Status status = skip_quotes(string); + if (status != JSONSuccess) { + return NULL; + } + input_string_len = *string - string_start - 2; /* length without quotes */ + return process_string(string_start + 1, input_string_len, output_string_len); +} + +static JSON_Value *parse_value(const char **string, size_t nesting) { + if (nesting > MAX_NESTING) { + return NULL; + } + SKIP_WHITESPACES(string); + switch (**string) { + case '{': + return parse_object_value(string, nesting + 1); + case '[': + return parse_array_value(string, nesting + 1); + case '\"': + return parse_string_value(string); + case 'f': + case 't': + return parse_boolean_value(string); + case '-': + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + return parse_number_value(string); + case 'n': + return parse_null_value(string); + default: + return NULL; + } +} + +static JSON_Value *parse_object_value(const char **string, size_t nesting) { + JSON_Value *output_value = NULL, *new_value = NULL; + JSON_Object *output_object = NULL; + char *new_key = NULL; + output_value = json_value_init_object(); + if (output_value == NULL) { + return NULL; + } + if (**string != '{') { + json_value_free(output_value); + return NULL; + } + output_object = json_value_get_object(output_value); + SKIP_CHAR(string); + SKIP_WHITESPACES(string); + if (**string == '}') { /* empty object */ + SKIP_CHAR(string); + return output_value; + } + while (**string != '\0') { + size_t key_len = 0; + new_key = get_quoted_string(string, &key_len); + /* We do not support key names with embedded \0 chars */ + if (new_key == NULL || key_len != strlen(new_key)) { + json_value_free(output_value); + return NULL; + } + SKIP_WHITESPACES(string); + if (**string != ':') { + parson_free(new_key); + json_value_free(output_value); + return NULL; + } + SKIP_CHAR(string); + new_value = parse_value(string, nesting); + if (new_value == NULL) { + parson_free(new_key); + json_value_free(output_value); + return NULL; + } + if (json_object_add(output_object, new_key, new_value) == JSONFailure) { + parson_free(new_key); + json_value_free(new_value); + json_value_free(output_value); + return NULL; + } + parson_free(new_key); + SKIP_WHITESPACES(string); + if (**string != ',') { + break; + } + SKIP_CHAR(string); + SKIP_WHITESPACES(string); + } + SKIP_WHITESPACES(string); + if (**string != '}' || /* Trim object after parsing is over */ + json_object_resize(output_object, json_object_get_count(output_object)) == + JSONFailure) { + json_value_free(output_value); + return NULL; + } + SKIP_CHAR(string); + return output_value; +} + +static JSON_Value *parse_array_value(const char **string, size_t nesting) { + JSON_Value *output_value = NULL, *new_array_value = NULL; + JSON_Array *output_array = NULL; + output_value = json_value_init_array(); + if (output_value == NULL) { + return NULL; + } + if (**string != '[') { + json_value_free(output_value); + return NULL; + } + output_array = json_value_get_array(output_value); + SKIP_CHAR(string); + SKIP_WHITESPACES(string); + if (**string == ']') { /* empty array */ + SKIP_CHAR(string); + return output_value; + } + while (**string != '\0') { + new_array_value = parse_value(string, nesting); + if (new_array_value == NULL) { + json_value_free(output_value); + return NULL; + } + if (json_array_add(output_array, new_array_value) == JSONFailure) { + json_value_free(new_array_value); + json_value_free(output_value); + return NULL; + } + SKIP_WHITESPACES(string); + if (**string != ',') { + break; + } + SKIP_CHAR(string); + SKIP_WHITESPACES(string); + } + SKIP_WHITESPACES(string); + if (**string != ']' || /* Trim array after parsing is over */ + json_array_resize(output_array, json_array_get_count(output_array)) == + JSONFailure) { + json_value_free(output_value); + return NULL; + } + SKIP_CHAR(string); + return output_value; +} + +static JSON_Value *parse_string_value(const char **string) { + JSON_Value *value = NULL; + size_t new_string_len = 0; + char *new_string = get_quoted_string(string, &new_string_len); + if (new_string == NULL) { + return NULL; + } + value = json_value_init_string_no_copy(new_string, new_string_len); + if (value == NULL) { + parson_free(new_string); + return NULL; + } + return value; +} + +static JSON_Value *parse_boolean_value(const char **string) { + size_t true_token_size = SIZEOF_TOKEN("true"); + size_t false_token_size = SIZEOF_TOKEN("false"); + if (strncmp("true", *string, true_token_size) == 0) { + *string += true_token_size; + return json_value_init_boolean(1); + } else if (strncmp("false", *string, false_token_size) == 0) { + *string += false_token_size; + return json_value_init_boolean(0); + } + return NULL; +} + +static JSON_Value *parse_number_value(const char **string) { + char *end; + double number = 0; + errno = 0; + number = strtod(*string, &end); + if (errno || !is_decimal(*string, end - *string)) { + return NULL; + } + *string = end; + return json_value_init_number(number); +} + +static JSON_Value *parse_null_value(const char **string) { + size_t token_size = SIZEOF_TOKEN("null"); + if (strncmp("null", *string, token_size) == 0) { + *string += token_size; + return json_value_init_null(); + } + return NULL; +} + +/* Serialization */ +#define APPEND_STRING(str) \ + do { \ + written = append_string(buf, (str)); \ + if (written < 0) { \ + return -1; \ + } \ + if (buf != NULL) { \ + buf += written; \ + } \ + written_total += written; \ + } while (0) + +#define APPEND_INDENT(level) \ + do { \ + written = append_indent(buf, (level)); \ + if (written < 0) { \ + return -1; \ + } \ + if (buf != NULL) { \ + buf += written; \ + } \ + written_total += written; \ + } while (0) + +static int json_serialize_to_buffer_r(const JSON_Value *value, char *buf, + int level, int is_pretty, char *num_buf) { + const char *key = NULL, *string = NULL; + JSON_Value *temp_value = NULL; + JSON_Array *array = NULL; + JSON_Object *object = NULL; + size_t i = 0, count = 0; + double num = 0.0; + int written = -1, written_total = 0; + size_t len = 0; + + switch (json_value_get_type(value)) { + case JSONArray: + array = json_value_get_array(value); + count = json_array_get_count(array); + APPEND_STRING("["); + if (count > 0 && is_pretty) { + APPEND_STRING("\n"); + } + for (i = 0; i < count; i++) { + if (is_pretty) { + APPEND_INDENT(level + 1); + } + temp_value = json_array_get_value(array, i); + written = json_serialize_to_buffer_r(temp_value, buf, level + 1, + is_pretty, num_buf); + if (written < 0) { + return -1; + } + if (buf != NULL) { + buf += written; + } + written_total += written; + if (i < (count - 1)) { + APPEND_STRING(","); + } + if (is_pretty) { + APPEND_STRING("\n"); + } + } + if (count > 0 && is_pretty) { + APPEND_INDENT(level); + } + APPEND_STRING("]"); + return written_total; + case JSONObject: + object = json_value_get_object(value); + count = json_object_get_count(object); + APPEND_STRING("{"); + if (count > 0 && is_pretty) { + APPEND_STRING("\n"); + } + for (i = 0; i < count; i++) { + key = json_object_get_name(object, i); + if (key == NULL) { + return -1; + } + if (is_pretty) { + APPEND_INDENT(level + 1); + } + /* We do not support key names with embedded \0 chars */ + written = json_serialize_string(key, strlen(key), buf); + if (written < 0) { + return -1; + } + if (buf != NULL) { + buf += written; + } + written_total += written; + APPEND_STRING(":"); + if (is_pretty) { + APPEND_STRING(" "); + } + temp_value = json_object_get_value(object, key); + written = json_serialize_to_buffer_r(temp_value, buf, level + 1, + is_pretty, num_buf); + if (written < 0) { + return -1; + } + if (buf != NULL) { + buf += written; + } + written_total += written; + if (i < (count - 1)) { + APPEND_STRING(","); + } + if (is_pretty) { + APPEND_STRING("\n"); + } + } + if (count > 0 && is_pretty) { + APPEND_INDENT(level); + } + APPEND_STRING("}"); + return written_total; + case JSONString: + string = json_value_get_string(value); + if (string == NULL) { + return -1; + } + len = json_value_get_string_len(value); + written = json_serialize_string(string, len, buf); + if (written < 0) { + return -1; + } + if (buf != NULL) { + buf += written; + } + written_total += written; + return written_total; + case JSONBoolean: + if (json_value_get_boolean(value)) { + APPEND_STRING("true"); + } else { + APPEND_STRING("false"); + } + return written_total; + case JSONNumber: + num = json_value_get_number(value); + if (buf != NULL) { + num_buf = buf; + } + written = sprintf(num_buf, FLOAT_FORMAT, num); + if (written < 0) { + return -1; + } + if (buf != NULL) { + buf += written; + } + written_total += written; + return written_total; + case JSONNull: + APPEND_STRING("null"); + return written_total; + case JSONError: + return -1; + default: + return -1; + } +} + +static int json_serialize_string(const char *string, size_t len, char *buf) { + size_t i = 0; + char c = '\0'; + int written = -1, written_total = 0; + APPEND_STRING("\""); + for (i = 0; i < len; i++) { + c = string[i]; + switch (c) { + case '\"': + APPEND_STRING("\\\""); + break; + case '\\': + APPEND_STRING("\\\\"); + break; + case '\b': + APPEND_STRING("\\b"); + break; + case '\f': + APPEND_STRING("\\f"); + break; + case '\n': + APPEND_STRING("\\n"); + break; + case '\r': + APPEND_STRING("\\r"); + break; + case '\t': + APPEND_STRING("\\t"); + break; + case '\x00': + APPEND_STRING("\\u0000"); + break; + case '\x01': + APPEND_STRING("\\u0001"); + break; + case '\x02': + APPEND_STRING("\\u0002"); + break; + case '\x03': + APPEND_STRING("\\u0003"); + break; + case '\x04': + APPEND_STRING("\\u0004"); + break; + case '\x05': + APPEND_STRING("\\u0005"); + break; + case '\x06': + APPEND_STRING("\\u0006"); + break; + case '\x07': + APPEND_STRING("\\u0007"); + break; + /* '\x08' duplicate: '\b' */ + /* '\x09' duplicate: '\t' */ + /* '\x0a' duplicate: '\n' */ + case '\x0b': + APPEND_STRING("\\u000b"); + break; + /* '\x0c' duplicate: '\f' */ + /* '\x0d' duplicate: '\r' */ + case '\x0e': + APPEND_STRING("\\u000e"); + break; + case '\x0f': + APPEND_STRING("\\u000f"); + break; + case '\x10': + APPEND_STRING("\\u0010"); + break; + case '\x11': + APPEND_STRING("\\u0011"); + break; + case '\x12': + APPEND_STRING("\\u0012"); + break; + case '\x13': + APPEND_STRING("\\u0013"); + break; + case '\x14': + APPEND_STRING("\\u0014"); + break; + case '\x15': + APPEND_STRING("\\u0015"); + break; + case '\x16': + APPEND_STRING("\\u0016"); + break; + case '\x17': + APPEND_STRING("\\u0017"); + break; + case '\x18': + APPEND_STRING("\\u0018"); + break; + case '\x19': + APPEND_STRING("\\u0019"); + break; + case '\x1a': + APPEND_STRING("\\u001a"); + break; + case '\x1b': + APPEND_STRING("\\u001b"); + break; + case '\x1c': + APPEND_STRING("\\u001c"); + break; + case '\x1d': + APPEND_STRING("\\u001d"); + break; + case '\x1e': + APPEND_STRING("\\u001e"); + break; + case '\x1f': + APPEND_STRING("\\u001f"); + break; + case '/': + if (parson_escape_slashes) { + APPEND_STRING("\\/"); /* to make json embeddable in xml\/html */ + } else { + APPEND_STRING("/"); + } + break; + default: + if (buf != NULL) { + buf[0] = c; + buf += 1; + } + written_total += 1; + break; + } + } + APPEND_STRING("\""); + return written_total; +} + +static int append_indent(char *buf, int level) { + int i; + int written = -1, written_total = 0; + for (i = 0; i < level; i++) { + APPEND_STRING(" "); + } + return written_total; +} + +static int append_string(char *buf, const char *string) { + if (buf == NULL) { + return (int)strlen(string); + } + return sprintf(buf, "%s", string); +} + +#undef APPEND_STRING +#undef APPEND_INDENT + +/* Parser API */ +JSON_Value *json_parse_file(const char *filename) { + char *file_contents = read_file(filename); + JSON_Value *output_value = NULL; + if (file_contents == NULL) { + return NULL; + } + output_value = json_parse_string(file_contents); + parson_free(file_contents); + return output_value; +} + +JSON_Value *json_parse_file_with_comments(const char *filename) { + char *file_contents = read_file(filename); + JSON_Value *output_value = NULL; + if (file_contents == NULL) { + return NULL; + } + output_value = json_parse_string_with_comments(file_contents); + parson_free(file_contents); + return output_value; +} + +JSON_Value *json_parse_string(const char *string) { + if (string == NULL) { + return NULL; + } + if (string[0] == '\xEF' && string[1] == '\xBB' && string[2] == '\xBF') { + string = string + 3; /* Support for UTF-8 BOM */ + } + return parse_value((const char **)&string, 0); +} + +JSON_Value *json_parse_string_with_comments(const char *string) { + JSON_Value *result = NULL; + char *string_mutable_copy = NULL, *string_mutable_copy_ptr = NULL; + string_mutable_copy = parson_strdup(string); + if (string_mutable_copy == NULL) { + return NULL; + } + remove_comments(string_mutable_copy, "/*", "*/"); + remove_comments(string_mutable_copy, "//", "\n"); + string_mutable_copy_ptr = string_mutable_copy; + result = parse_value((const char **)&string_mutable_copy_ptr, 0); + parson_free(string_mutable_copy); + return result; +} + +/* JSON Object API */ + +JSON_Value *json_object_get_value(const JSON_Object *object, const char *name) { + if (object == NULL || name == NULL) { + return NULL; + } + return json_object_getn_value(object, name, strlen(name)); +} + +const char *json_object_get_string(const JSON_Object *object, + const char *name) { + return json_value_get_string(json_object_get_value(object, name)); +} + +size_t json_object_get_string_len(const JSON_Object *object, const char *name) { + return json_value_get_string_len(json_object_get_value(object, name)); +} + +double json_object_get_number(const JSON_Object *object, const char *name) { + return json_value_get_number(json_object_get_value(object, name)); +} + +JSON_Object *json_object_get_object(const JSON_Object *object, + const char *name) { + return json_value_get_object(json_object_get_value(object, name)); +} + +JSON_Array *json_object_get_array(const JSON_Object *object, const char *name) { + return json_value_get_array(json_object_get_value(object, name)); +} + +int json_object_get_boolean(const JSON_Object *object, const char *name) { + return json_value_get_boolean(json_object_get_value(object, name)); +} + +JSON_Value *json_object_dotget_value(const JSON_Object *object, + const char *name) { + const char *dot_position = strchr(name, '.'); + if (!dot_position) { + return json_object_get_value(object, name); + } + object = json_value_get_object( + json_object_getn_value(object, name, dot_position - name)); + return json_object_dotget_value(object, dot_position + 1); +} + +const char *json_object_dotget_string(const JSON_Object *object, + const char *name) { + return json_value_get_string(json_object_dotget_value(object, name)); +} + +size_t json_object_dotget_string_len(const JSON_Object *object, + const char *name) { + return json_value_get_string_len(json_object_dotget_value(object, name)); +} + +double json_object_dotget_number(const JSON_Object *object, const char *name) { + return json_value_get_number(json_object_dotget_value(object, name)); +} + +JSON_Object *json_object_dotget_object(const JSON_Object *object, + const char *name) { + return json_value_get_object(json_object_dotget_value(object, name)); +} + +JSON_Array *json_object_dotget_array(const JSON_Object *object, + const char *name) { + return json_value_get_array(json_object_dotget_value(object, name)); +} + +int json_object_dotget_boolean(const JSON_Object *object, const char *name) { + return json_value_get_boolean(json_object_dotget_value(object, name)); +} + +size_t json_object_get_count(const JSON_Object *object) { + return object ? object->count : 0; +} + +const char *json_object_get_name(const JSON_Object *object, size_t index) { + if (object == NULL || index >= json_object_get_count(object)) { + return NULL; + } + return object->names[index]; +} + +JSON_Value *json_object_get_value_at(const JSON_Object *object, size_t index) { + if (object == NULL || index >= json_object_get_count(object)) { + return NULL; + } + return object->values[index]; +} + +JSON_Value *json_object_get_wrapping_value(const JSON_Object *object) { + return object->wrapping_value; +} + +int json_object_has_value(const JSON_Object *object, const char *name) { + return json_object_get_value(object, name) != NULL; +} + +int json_object_has_value_of_type(const JSON_Object *object, const char *name, + JSON_Value_Type type) { + JSON_Value *val = json_object_get_value(object, name); + return val != NULL && json_value_get_type(val) == type; +} + +int json_object_dothas_value(const JSON_Object *object, const char *name) { + return json_object_dotget_value(object, name) != NULL; +} + +int json_object_dothas_value_of_type(const JSON_Object *object, + const char *name, JSON_Value_Type type) { + JSON_Value *val = json_object_dotget_value(object, name); + return val != NULL && json_value_get_type(val) == type; +} + +/* JSON Array API */ +JSON_Value *json_array_get_value(const JSON_Array *array, size_t index) { + if (array == NULL || index >= json_array_get_count(array)) { + return NULL; + } + return array->items[index]; +} + +const char *json_array_get_string(const JSON_Array *array, size_t index) { + return json_value_get_string(json_array_get_value(array, index)); +} + +size_t json_array_get_string_len(const JSON_Array *array, size_t index) { + return json_value_get_string_len(json_array_get_value(array, index)); +} + +double json_array_get_number(const JSON_Array *array, size_t index) { + return json_value_get_number(json_array_get_value(array, index)); +} + +JSON_Object *json_array_get_object(const JSON_Array *array, size_t index) { + return json_value_get_object(json_array_get_value(array, index)); +} + +JSON_Array *json_array_get_array(const JSON_Array *array, size_t index) { + return json_value_get_array(json_array_get_value(array, index)); +} + +int json_array_get_boolean(const JSON_Array *array, size_t index) { + return json_value_get_boolean(json_array_get_value(array, index)); +} + +size_t json_array_get_count(const JSON_Array *array) { + return array ? array->count : 0; +} + +JSON_Value *json_array_get_wrapping_value(const JSON_Array *array) { + return array->wrapping_value; +} + +/* JSON Value API */ +JSON_Value_Type json_value_get_type(const JSON_Value *value) { + return value ? value->type : JSONError; +} + +JSON_Object *json_value_get_object(const JSON_Value *value) { + return json_value_get_type(value) == JSONObject ? value->value.object : NULL; +} + +JSON_Array *json_value_get_array(const JSON_Value *value) { + return json_value_get_type(value) == JSONArray ? value->value.array : NULL; +} + +static const JSON_String *json_value_get_string_desc(const JSON_Value *value) { + return json_value_get_type(value) == JSONString ? &value->value.string : NULL; +} + +const char *json_value_get_string(const JSON_Value *value) { + const JSON_String *str = json_value_get_string_desc(value); + return str ? str->chars : NULL; +} + +size_t json_value_get_string_len(const JSON_Value *value) { + const JSON_String *str = json_value_get_string_desc(value); + return str ? str->length : 0; +} + +double json_value_get_number(const JSON_Value *value) { + return json_value_get_type(value) == JSONNumber ? value->value.number : 0; +} + +int json_value_get_boolean(const JSON_Value *value) { + return json_value_get_type(value) == JSONBoolean ? value->value.boolean : -1; +} + +JSON_Value *json_value_get_parent(const JSON_Value *value) { + return value ? value->parent : NULL; +} + +void json_value_free(JSON_Value *value) { + switch (json_value_get_type(value)) { + case JSONObject: + json_object_free(value->value.object); + break; + case JSONString: + parson_free(value->value.string.chars); + break; + case JSONArray: + json_array_free(value->value.array); + break; + default: + break; + } + parson_free(value); +} + +JSON_Value *json_value_init_object(void) { + JSON_Value *new_value = (JSON_Value *)parson_malloc(sizeof(JSON_Value)); + if (!new_value) { + return NULL; + } + new_value->parent = NULL; + new_value->type = JSONObject; + new_value->value.object = json_object_init(new_value); + if (!new_value->value.object) { + parson_free(new_value); + return NULL; + } + return new_value; +} + +JSON_Value *json_value_init_array(void) { + JSON_Value *new_value = (JSON_Value *)parson_malloc(sizeof(JSON_Value)); + if (!new_value) { + return NULL; + } + new_value->parent = NULL; + new_value->type = JSONArray; + new_value->value.array = json_array_init(new_value); + if (!new_value->value.array) { + parson_free(new_value); + return NULL; + } + return new_value; +} + +JSON_Value *json_value_init_string(const char *string) { + if (string == NULL) { + return NULL; + } + return json_value_init_string_with_len(string, strlen(string)); +} + +JSON_Value *json_value_init_string_with_len(const char *string, size_t length) { + char *copy = NULL; + JSON_Value *value; + if (string == NULL) { + return NULL; + } + if (!is_valid_utf8(string, length)) { + return NULL; + } + copy = parson_strndup(string, length); + if (copy == NULL) { + return NULL; + } + value = json_value_init_string_no_copy(copy, length); + if (value == NULL) { + parson_free(copy); + } + return value; +} + +JSON_Value *json_value_init_number(double number) { + JSON_Value *new_value = NULL; + if (IS_NUMBER_INVALID(number)) { + return NULL; + } + new_value = (JSON_Value *)parson_malloc(sizeof(JSON_Value)); + if (new_value == NULL) { + return NULL; + } + new_value->parent = NULL; + new_value->type = JSONNumber; + new_value->value.number = number; + return new_value; +} + +JSON_Value *json_value_init_boolean(int boolean) { + JSON_Value *new_value = (JSON_Value *)parson_malloc(sizeof(JSON_Value)); + if (!new_value) { + return NULL; + } + new_value->parent = NULL; + new_value->type = JSONBoolean; + new_value->value.boolean = boolean ? 1 : 0; + return new_value; +} + +JSON_Value *json_value_init_null(void) { + JSON_Value *new_value = (JSON_Value *)parson_malloc(sizeof(JSON_Value)); + if (!new_value) { + return NULL; + } + new_value->parent = NULL; + new_value->type = JSONNull; + return new_value; +} + +JSON_Value *json_value_deep_copy(const JSON_Value *value) { + size_t i = 0; + JSON_Value *return_value = NULL, *temp_value_copy = NULL, *temp_value = NULL; + const JSON_String *temp_string = NULL; + const char *temp_key = NULL; + char *temp_string_copy = NULL; + JSON_Array *temp_array = NULL, *temp_array_copy = NULL; + JSON_Object *temp_object = NULL, *temp_object_copy = NULL; + + switch (json_value_get_type(value)) { + case JSONArray: + temp_array = json_value_get_array(value); + return_value = json_value_init_array(); + if (return_value == NULL) { + return NULL; + } + temp_array_copy = json_value_get_array(return_value); + for (i = 0; i < json_array_get_count(temp_array); i++) { + temp_value = json_array_get_value(temp_array, i); + temp_value_copy = json_value_deep_copy(temp_value); + if (temp_value_copy == NULL) { + json_value_free(return_value); + return NULL; + } + if (json_array_add(temp_array_copy, temp_value_copy) == JSONFailure) { + json_value_free(return_value); + json_value_free(temp_value_copy); + return NULL; + } + } + return return_value; + case JSONObject: + temp_object = json_value_get_object(value); + return_value = json_value_init_object(); + if (return_value == NULL) { + return NULL; + } + temp_object_copy = json_value_get_object(return_value); + for (i = 0; i < json_object_get_count(temp_object); i++) { + temp_key = json_object_get_name(temp_object, i); + temp_value = json_object_get_value(temp_object, temp_key); + temp_value_copy = json_value_deep_copy(temp_value); + if (temp_value_copy == NULL) { + json_value_free(return_value); + return NULL; + } + if (json_object_add(temp_object_copy, temp_key, temp_value_copy) == + JSONFailure) { + json_value_free(return_value); + json_value_free(temp_value_copy); + return NULL; + } + } + return return_value; + case JSONBoolean: + return json_value_init_boolean(json_value_get_boolean(value)); + case JSONNumber: + return json_value_init_number(json_value_get_number(value)); + case JSONString: + temp_string = json_value_get_string_desc(value); + if (temp_string == NULL) { + return NULL; + } + temp_string_copy = parson_strndup(temp_string->chars, temp_string->length); + if (temp_string_copy == NULL) { + return NULL; + } + return_value = + json_value_init_string_no_copy(temp_string_copy, temp_string->length); + if (return_value == NULL) { + parson_free(temp_string_copy); + } + return return_value; + case JSONNull: + return json_value_init_null(); + case JSONError: + return NULL; + default: + return NULL; + } +} + +size_t json_serialization_size(const JSON_Value *value) { + char num_buf[NUM_BUF_SIZE]; /* recursively allocating buffer on stack is a bad + idea, so let's do it only once */ + int res = json_serialize_to_buffer_r(value, NULL, 0, 0, num_buf); + return res < 0 ? 0 : (size_t)(res) + 1; +} + +JSON_Status json_serialize_to_buffer(const JSON_Value *value, char *buf, + size_t buf_size_in_bytes) { + int written = -1; + size_t needed_size_in_bytes = json_serialization_size(value); + if (needed_size_in_bytes == 0 || buf_size_in_bytes < needed_size_in_bytes) { + return JSONFailure; + } + written = json_serialize_to_buffer_r(value, buf, 0, 0, NULL); + if (written < 0) { + return JSONFailure; + } + return JSONSuccess; +} + +JSON_Status json_serialize_to_file(const JSON_Value *value, + const char *filename) { + JSON_Status return_code = JSONSuccess; + FILE *fp = NULL; + char *serialized_string = json_serialize_to_string(value); + if (serialized_string == NULL) { + return JSONFailure; + } + fp = fopen(filename, "w"); + if (fp == NULL) { + json_free_serialized_string(serialized_string); + return JSONFailure; + } + if (fputs(serialized_string, fp) == EOF) { + return_code = JSONFailure; + } + if (fclose(fp) == EOF) { + return_code = JSONFailure; + } + json_free_serialized_string(serialized_string); + return return_code; +} + +char *json_serialize_to_string(const JSON_Value *value) { + JSON_Status serialization_result = JSONFailure; + size_t buf_size_bytes = json_serialization_size(value); + char *buf = NULL; + if (buf_size_bytes == 0) { + return NULL; + } + buf = (char *)parson_malloc(buf_size_bytes); + if (buf == NULL) { + return NULL; + } + serialization_result = json_serialize_to_buffer(value, buf, buf_size_bytes); + if (serialization_result == JSONFailure) { + json_free_serialized_string(buf); + return NULL; + } + return buf; +} + +size_t json_serialization_size_pretty(const JSON_Value *value) { + char num_buf[NUM_BUF_SIZE]; /* recursively allocating buffer on stack is a bad + idea, so let's do it only once */ + int res = json_serialize_to_buffer_r(value, NULL, 0, 1, num_buf); + return res < 0 ? 0 : (size_t)(res) + 1; +} + +JSON_Status json_serialize_to_buffer_pretty(const JSON_Value *value, char *buf, + size_t buf_size_in_bytes) { + int written = -1; + size_t needed_size_in_bytes = json_serialization_size_pretty(value); + if (needed_size_in_bytes == 0 || buf_size_in_bytes < needed_size_in_bytes) { + return JSONFailure; + } + written = json_serialize_to_buffer_r(value, buf, 0, 1, NULL); + if (written < 0) { + return JSONFailure; + } + return JSONSuccess; +} + +JSON_Status json_serialize_to_file_pretty(const JSON_Value *value, + const char *filename) { + JSON_Status return_code = JSONSuccess; + FILE *fp = NULL; + char *serialized_string = json_serialize_to_string_pretty(value); + if (serialized_string == NULL) { + return JSONFailure; + } + fp = fopen(filename, "w"); + if (fp == NULL) { + json_free_serialized_string(serialized_string); + return JSONFailure; + } + if (fputs(serialized_string, fp) == EOF) { + return_code = JSONFailure; + } + if (fclose(fp) == EOF) { + return_code = JSONFailure; + } + json_free_serialized_string(serialized_string); + return return_code; +} + +char *json_serialize_to_string_pretty(const JSON_Value *value) { + JSON_Status serialization_result = JSONFailure; + size_t buf_size_bytes = json_serialization_size_pretty(value); + char *buf = NULL; + if (buf_size_bytes == 0) { + return NULL; + } + buf = (char *)parson_malloc(buf_size_bytes); + if (buf == NULL) { + return NULL; + } + serialization_result = + json_serialize_to_buffer_pretty(value, buf, buf_size_bytes); + if (serialization_result == JSONFailure) { + json_free_serialized_string(buf); + return NULL; + } + return buf; +} + +void json_free_serialized_string(char *string) { parson_free(string); } + +JSON_Status json_array_remove(JSON_Array *array, size_t ix) { + size_t to_move_bytes = 0; + if (array == NULL || ix >= json_array_get_count(array)) { + return JSONFailure; + } + json_value_free(json_array_get_value(array, ix)); + to_move_bytes = (json_array_get_count(array) - 1 - ix) * sizeof(JSON_Value *); + memmove(array->items + ix, array->items + ix + 1, to_move_bytes); + array->count -= 1; + return JSONSuccess; +} + +JSON_Status json_array_replace_value(JSON_Array *array, size_t ix, + JSON_Value *value) { + if (array == NULL || value == NULL || value->parent != NULL || + ix >= json_array_get_count(array)) { + return JSONFailure; + } + json_value_free(json_array_get_value(array, ix)); + value->parent = json_array_get_wrapping_value(array); + array->items[ix] = value; + return JSONSuccess; +} + +JSON_Status json_array_replace_string(JSON_Array *array, size_t i, + const char *string) { + JSON_Value *value = json_value_init_string(string); + if (value == NULL) { + return JSONFailure; + } + if (json_array_replace_value(array, i, value) == JSONFailure) { + json_value_free(value); + return JSONFailure; + } + return JSONSuccess; +} + +JSON_Status json_array_replace_string_with_len(JSON_Array *array, size_t i, + const char *string, size_t len) { + JSON_Value *value = json_value_init_string_with_len(string, len); + if (value == NULL) { + return JSONFailure; + } + if (json_array_replace_value(array, i, value) == JSONFailure) { + json_value_free(value); + return JSONFailure; + } + return JSONSuccess; +} + +JSON_Status json_array_replace_number(JSON_Array *array, size_t i, + double number) { + JSON_Value *value = json_value_init_number(number); + if (value == NULL) { + return JSONFailure; + } + if (json_array_replace_value(array, i, value) == JSONFailure) { + json_value_free(value); + return JSONFailure; + } + return JSONSuccess; +} + +JSON_Status json_array_replace_boolean(JSON_Array *array, size_t i, + int boolean) { + JSON_Value *value = json_value_init_boolean(boolean); + if (value == NULL) { + return JSONFailure; + } + if (json_array_replace_value(array, i, value) == JSONFailure) { + json_value_free(value); + return JSONFailure; + } + return JSONSuccess; +} + +JSON_Status json_array_replace_null(JSON_Array *array, size_t i) { + JSON_Value *value = json_value_init_null(); + if (value == NULL) { + return JSONFailure; + } + if (json_array_replace_value(array, i, value) == JSONFailure) { + json_value_free(value); + return JSONFailure; + } + return JSONSuccess; +} + +JSON_Status json_array_clear(JSON_Array *array) { + size_t i = 0; + if (array == NULL) { + return JSONFailure; + } + for (i = 0; i < json_array_get_count(array); i++) { + json_value_free(json_array_get_value(array, i)); + } + array->count = 0; + return JSONSuccess; +} + +JSON_Status json_array_append_value(JSON_Array *array, JSON_Value *value) { + if (array == NULL || value == NULL || value->parent != NULL) { + return JSONFailure; + } + return json_array_add(array, value); +} + +JSON_Status json_array_append_string(JSON_Array *array, const char *string) { + JSON_Value *value = json_value_init_string(string); + if (value == NULL) { + return JSONFailure; + } + if (json_array_append_value(array, value) == JSONFailure) { + json_value_free(value); + return JSONFailure; + } + return JSONSuccess; +} + +JSON_Status json_array_append_string_with_len(JSON_Array *array, + const char *string, size_t len) { + JSON_Value *value = json_value_init_string_with_len(string, len); + if (value == NULL) { + return JSONFailure; + } + if (json_array_append_value(array, value) == JSONFailure) { + json_value_free(value); + return JSONFailure; + } + return JSONSuccess; +} + +JSON_Status json_array_append_number(JSON_Array *array, double number) { + JSON_Value *value = json_value_init_number(number); + if (value == NULL) { + return JSONFailure; + } + if (json_array_append_value(array, value) == JSONFailure) { + json_value_free(value); + return JSONFailure; + } + return JSONSuccess; +} + +JSON_Status json_array_append_boolean(JSON_Array *array, int boolean) { + JSON_Value *value = json_value_init_boolean(boolean); + if (value == NULL) { + return JSONFailure; + } + if (json_array_append_value(array, value) == JSONFailure) { + json_value_free(value); + return JSONFailure; + } + return JSONSuccess; +} + +JSON_Status json_array_append_null(JSON_Array *array) { + JSON_Value *value = json_value_init_null(); + if (value == NULL) { + return JSONFailure; + } + if (json_array_append_value(array, value) == JSONFailure) { + json_value_free(value); + return JSONFailure; + } + return JSONSuccess; +} + +JSON_Status json_object_set_value(JSON_Object *object, const char *name, + JSON_Value *value) { + size_t i = 0; + JSON_Value *old_value; + if (object == NULL || name == NULL || value == NULL || + value->parent != NULL) { + return JSONFailure; + } + old_value = json_object_get_value(object, name); + if (old_value != NULL) { /* free and overwrite old value */ + json_value_free(old_value); + for (i = 0; i < json_object_get_count(object); i++) { + if (strcmp(object->names[i], name) == 0) { + value->parent = json_object_get_wrapping_value(object); + object->values[i] = value; + return JSONSuccess; + } + } + } + /* add new key value pair */ + return json_object_add(object, name, value); +} + +JSON_Status json_object_set_string(JSON_Object *object, const char *name, + const char *string) { + JSON_Value *value = json_value_init_string(string); + JSON_Status status = json_object_set_value(object, name, value); + if (status == JSONFailure) { + json_value_free(value); + } + return status; +} + +JSON_Status json_object_set_string_with_len(JSON_Object *object, + const char *name, + const char *string, size_t len) { + JSON_Value *value = json_value_init_string_with_len(string, len); + JSON_Status status = json_object_set_value(object, name, value); + if (status == JSONFailure) { + json_value_free(value); + } + return status; +} + +JSON_Status json_object_set_number(JSON_Object *object, const char *name, + double number) { + JSON_Value *value = json_value_init_number(number); + JSON_Status status = json_object_set_value(object, name, value); + if (status == JSONFailure) { + json_value_free(value); + } + return status; +} + +JSON_Status json_object_set_boolean(JSON_Object *object, const char *name, + int boolean) { + JSON_Value *value = json_value_init_boolean(boolean); + JSON_Status status = json_object_set_value(object, name, value); + if (status == JSONFailure) { + json_value_free(value); + } + return status; +} + +JSON_Status json_object_set_null(JSON_Object *object, const char *name) { + JSON_Value *value = json_value_init_null(); + JSON_Status status = json_object_set_value(object, name, value); + if (status == JSONFailure) { + json_value_free(value); + } + return status; +} + +JSON_Status json_object_dotset_value(JSON_Object *object, const char *name, + JSON_Value *value) { + const char *dot_pos = NULL; + JSON_Value *temp_value = NULL, *new_value = NULL; + JSON_Object *temp_object = NULL, *new_object = NULL; + JSON_Status status = JSONFailure; + size_t name_len = 0; + if (object == NULL || name == NULL || value == NULL) { + return JSONFailure; + } + dot_pos = strchr(name, '.'); + if (dot_pos == NULL) { + return json_object_set_value(object, name, value); + } + name_len = dot_pos - name; + temp_value = json_object_getn_value(object, name, name_len); + if (temp_value) { + /* Don't overwrite existing non-object (unlike json_object_set_value, but it + * shouldn't be changed at this point) */ + if (json_value_get_type(temp_value) != JSONObject) { + return JSONFailure; + } + temp_object = json_value_get_object(temp_value); + return json_object_dotset_value(temp_object, dot_pos + 1, value); + } + new_value = json_value_init_object(); + if (new_value == NULL) { + return JSONFailure; + } + new_object = json_value_get_object(new_value); + status = json_object_dotset_value(new_object, dot_pos + 1, value); + if (status != JSONSuccess) { + json_value_free(new_value); + return JSONFailure; + } + status = json_object_addn(object, name, name_len, new_value); + if (status != JSONSuccess) { + json_object_dotremove_internal(new_object, dot_pos + 1, 0); + json_value_free(new_value); + return JSONFailure; + } + return JSONSuccess; +} + +JSON_Status json_object_dotset_string(JSON_Object *object, const char *name, + const char *string) { + JSON_Value *value = json_value_init_string(string); + if (value == NULL) { + return JSONFailure; + } + if (json_object_dotset_value(object, name, value) == JSONFailure) { + json_value_free(value); + return JSONFailure; + } + return JSONSuccess; +} + +JSON_Status json_object_dotset_string_with_len(JSON_Object *object, + const char *name, + const char *string, size_t len) { + JSON_Value *value = json_value_init_string_with_len(string, len); + if (value == NULL) { + return JSONFailure; + } + if (json_object_dotset_value(object, name, value) == JSONFailure) { + json_value_free(value); + return JSONFailure; + } + return JSONSuccess; +} + +JSON_Status json_object_dotset_number(JSON_Object *object, const char *name, + double number) { + JSON_Value *value = json_value_init_number(number); + if (value == NULL) { + return JSONFailure; + } + if (json_object_dotset_value(object, name, value) == JSONFailure) { + json_value_free(value); + return JSONFailure; + } + return JSONSuccess; +} + +JSON_Status json_object_dotset_boolean(JSON_Object *object, const char *name, + int boolean) { + JSON_Value *value = json_value_init_boolean(boolean); + if (value == NULL) { + return JSONFailure; + } + if (json_object_dotset_value(object, name, value) == JSONFailure) { + json_value_free(value); + return JSONFailure; + } + return JSONSuccess; +} + +JSON_Status json_object_dotset_null(JSON_Object *object, const char *name) { + JSON_Value *value = json_value_init_null(); + if (value == NULL) { + return JSONFailure; + } + if (json_object_dotset_value(object, name, value) == JSONFailure) { + json_value_free(value); + return JSONFailure; + } + return JSONSuccess; +} + +JSON_Status json_object_remove(JSON_Object *object, const char *name) { + return json_object_remove_internal(object, name, 1); +} + +JSON_Status json_object_dotremove(JSON_Object *object, const char *name) { + return json_object_dotremove_internal(object, name, 1); +} + +JSON_Status json_object_clear(JSON_Object *object) { + size_t i = 0; + if (object == NULL) { + return JSONFailure; + } + for (i = 0; i < json_object_get_count(object); i++) { + parson_free(object->names[i]); + json_value_free(object->values[i]); + } + object->count = 0; + return JSONSuccess; +} + +JSON_Status json_validate(const JSON_Value *schema, const JSON_Value *value) { + JSON_Value *temp_schema_value = NULL, *temp_value = NULL; + JSON_Array *schema_array = NULL, *value_array = NULL; + JSON_Object *schema_object = NULL, *value_object = NULL; + JSON_Value_Type schema_type = JSONError, value_type = JSONError; + const char *key = NULL; + size_t i = 0, count = 0; + if (schema == NULL || value == NULL) { + return JSONFailure; + } + schema_type = json_value_get_type(schema); + value_type = json_value_get_type(value); + if (schema_type != value_type && + schema_type != JSONNull) { /* null represents all values */ + return JSONFailure; + } + switch (schema_type) { + case JSONArray: + schema_array = json_value_get_array(schema); + value_array = json_value_get_array(value); + count = json_array_get_count(schema_array); + if (count == 0) { + return JSONSuccess; /* Empty array allows all types */ + } + /* Get first value from array, rest is ignored */ + temp_schema_value = json_array_get_value(schema_array, 0); + for (i = 0; i < json_array_get_count(value_array); i++) { + temp_value = json_array_get_value(value_array, i); + if (json_validate(temp_schema_value, temp_value) == JSONFailure) { + return JSONFailure; + } + } + return JSONSuccess; + case JSONObject: + schema_object = json_value_get_object(schema); + value_object = json_value_get_object(value); + count = json_object_get_count(schema_object); + if (count == 0) { + return JSONSuccess; /* Empty object allows all objects */ + } else if (json_object_get_count(value_object) < count) { + return JSONFailure; /* Tested object mustn't have less name-value pairs + than schema */ + } + for (i = 0; i < count; i++) { + key = json_object_get_name(schema_object, i); + temp_schema_value = json_object_get_value(schema_object, key); + temp_value = json_object_get_value(value_object, key); + if (temp_value == NULL) { + return JSONFailure; + } + if (json_validate(temp_schema_value, temp_value) == JSONFailure) { + return JSONFailure; + } + } + return JSONSuccess; + case JSONString: + case JSONNumber: + case JSONBoolean: + case JSONNull: + return JSONSuccess; /* equality already tested before switch */ + case JSONError: + default: + return JSONFailure; + } +} + +int json_value_equals(const JSON_Value *a, const JSON_Value *b) { + JSON_Object *a_object = NULL, *b_object = NULL; + JSON_Array *a_array = NULL, *b_array = NULL; + const JSON_String *a_string = NULL, *b_string = NULL; + const char *key = NULL; + size_t a_count = 0, b_count = 0, i = 0; + JSON_Value_Type a_type, b_type; + a_type = json_value_get_type(a); + b_type = json_value_get_type(b); + if (a_type != b_type) { + return 0; + } + switch (a_type) { + case JSONArray: + a_array = json_value_get_array(a); + b_array = json_value_get_array(b); + a_count = json_array_get_count(a_array); + b_count = json_array_get_count(b_array); + if (a_count != b_count) { + return 0; + } + for (i = 0; i < a_count; i++) { + if (!json_value_equals(json_array_get_value(a_array, i), + json_array_get_value(b_array, i))) { + return 0; + } + } + return 1; + case JSONObject: + a_object = json_value_get_object(a); + b_object = json_value_get_object(b); + a_count = json_object_get_count(a_object); + b_count = json_object_get_count(b_object); + if (a_count != b_count) { + return 0; + } + for (i = 0; i < a_count; i++) { + key = json_object_get_name(a_object, i); + if (!json_value_equals(json_object_get_value(a_object, key), + json_object_get_value(b_object, key))) { + return 0; + } + } + return 1; + case JSONString: + a_string = json_value_get_string_desc(a); + b_string = json_value_get_string_desc(b); + if (a_string == NULL || b_string == NULL) { + return 0; /* shouldn't happen */ + } + return a_string->length == b_string->length && + memcmp(a_string->chars, b_string->chars, a_string->length) == 0; + case JSONBoolean: + return json_value_get_boolean(a) == json_value_get_boolean(b); + case JSONNumber: + return fabs(json_value_get_number(a) - json_value_get_number(b)) < + 0.000001; /* EPSILON */ + case JSONError: + return 1; + case JSONNull: + return 1; + default: + return 1; + } +} + +JSON_Value_Type json_type(const JSON_Value *value) { + return json_value_get_type(value); +} + +JSON_Object *json_object(const JSON_Value *value) { + return json_value_get_object(value); +} + +JSON_Array *json_array(const JSON_Value *value) { + return json_value_get_array(value); +} + +const char *json_string(const JSON_Value *value) { + return json_value_get_string(value); +} + +size_t json_string_len(const JSON_Value *value) { + return json_value_get_string_len(value); +} + +double json_number(const JSON_Value *value) { + return json_value_get_number(value); +} + +int json_boolean(const JSON_Value *value) { + return json_value_get_boolean(value); +} + +void json_set_allocation_functions(JSON_Malloc_Function malloc_fun, + JSON_Free_Function free_fun) { + parson_malloc = malloc_fun; + parson_free = free_fun; +} + +void json_set_escape_slashes(int escape_slashes) { + parson_escape_slashes = escape_slashes; +} diff --git a/sensors/bms/packages/jbdtool/parson.h b/sensors/bms/packages/jbdtool/parson.h new file mode 100644 index 00000000..a089ad32 --- /dev/null +++ b/sensors/bms/packages/jbdtool/parson.h @@ -0,0 +1,307 @@ +/* + SPDX-License-Identifier: MIT + + Parson 1.1.0 ( http://kgabis.github.com/parson/ ) + Copyright (c) 2012 - 2020 Krzysztof Gabis + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#ifndef parson_parson_h +#define parson_parson_h + +#ifdef __cplusplus +extern "C" { +#endif + +#include /* size_t */ + +/* Types and enums */ +typedef struct json_object_t JSON_Object; +typedef struct json_array_t JSON_Array; +typedef struct json_value_t JSON_Value; + +enum json_value_type { + JSONError = -1, + JSONNull = 1, + JSONString = 2, + JSONNumber = 3, + JSONObject = 4, + JSONArray = 5, + JSONBoolean = 6 +}; +typedef int JSON_Value_Type; + +enum json_result_t { JSONSuccess = 0, JSONFailure = -1 }; +typedef int JSON_Status; + +typedef void *(*JSON_Malloc_Function)(size_t); +typedef void (*JSON_Free_Function)(void *); + +/* Call only once, before calling any other function from parson API. If not + called, malloc and free from stdlib will be used for all allocations */ +void json_set_allocation_functions(JSON_Malloc_Function malloc_fun, + JSON_Free_Function free_fun); + +/* Sets if slashes should be escaped or not when serializing JSON. By default + slashes are escaped. This function sets a global setting and is not thread + safe. */ +void json_set_escape_slashes(int escape_slashes); + +/* Parses first JSON value in a file, returns NULL in case of error */ +JSON_Value *json_parse_file(const char *filename); + +/* Parses first JSON value in a file and ignores comments (/ * * / and //), + returns NULL in case of error */ +JSON_Value *json_parse_file_with_comments(const char *filename); + +/* Parses first JSON value in a string, returns NULL in case of error */ +JSON_Value *json_parse_string(const char *string); + +/* Parses first JSON value in a string and ignores comments (/ * * / and //), + returns NULL in case of error */ +JSON_Value *json_parse_string_with_comments(const char *string); + +/* Serialization */ +size_t json_serialization_size(const JSON_Value *value); /* returns 0 on fail */ +JSON_Status json_serialize_to_buffer(const JSON_Value *value, char *buf, + size_t buf_size_in_bytes); +JSON_Status json_serialize_to_file(const JSON_Value *value, + const char *filename); +char *json_serialize_to_string(const JSON_Value *value); + +/* Pretty serialization */ +size_t +json_serialization_size_pretty(const JSON_Value *value); /* returns 0 on fail */ +JSON_Status json_serialize_to_buffer_pretty(const JSON_Value *value, char *buf, + size_t buf_size_in_bytes); +JSON_Status json_serialize_to_file_pretty(const JSON_Value *value, + const char *filename); +char *json_serialize_to_string_pretty(const JSON_Value *value); + +void json_free_serialized_string( + char *string); /* frees string from json_serialize_to_string and + json_serialize_to_string_pretty */ + +/* Comparing */ +int json_value_equals(const JSON_Value *a, const JSON_Value *b); + +/* Validation + This is *NOT* JSON Schema. It validates json by checking if object have + identically named fields with matching types. For example schema {"name":"", + "age":0} will validate + {"name":"Joe", "age":25} and {"name":"Joe", "age":25, "gender":"m"}, + but not {"name":"Joe"} or {"name":"Joe", "age":"Cucumber"}. + In case of arrays, only first value in schema is checked against all values + in tested array. Empty objects ({}) validate all objects, empty arrays ([]) + validate all arrays, null validates values of every type. + */ +JSON_Status json_validate(const JSON_Value *schema, const JSON_Value *value); + +/* + * JSON Object + */ +JSON_Value *json_object_get_value(const JSON_Object *object, const char *name); +const char *json_object_get_string(const JSON_Object *object, const char *name); +size_t json_object_get_string_len( + const JSON_Object *object, + const char *name); /* doesn't account for last null character */ +JSON_Object *json_object_get_object(const JSON_Object *object, + const char *name); +JSON_Array *json_object_get_array(const JSON_Object *object, const char *name); +double json_object_get_number(const JSON_Object *object, + const char *name); /* returns 0 on fail */ +int json_object_get_boolean(const JSON_Object *object, + const char *name); /* returns -1 on fail */ + +/* dotget functions enable addressing values with dot notation in nested + objects, just like in structs or c++/java/c# objects (e.g. + objectA.objectB.value). Because valid names in JSON can contain dots, some + values may be inaccessible this way. */ +JSON_Value *json_object_dotget_value(const JSON_Object *object, + const char *name); +const char *json_object_dotget_string(const JSON_Object *object, + const char *name); +size_t json_object_dotget_string_len( + const JSON_Object *object, + const char *name); /* doesn't account for last null character */ +JSON_Object *json_object_dotget_object(const JSON_Object *object, + const char *name); +JSON_Array *json_object_dotget_array(const JSON_Object *object, + const char *name); +double json_object_dotget_number(const JSON_Object *object, + const char *name); /* returns 0 on fail */ +int json_object_dotget_boolean(const JSON_Object *object, + const char *name); /* returns -1 on fail */ + +/* Functions to get available names */ +size_t json_object_get_count(const JSON_Object *object); +const char *json_object_get_name(const JSON_Object *object, size_t index); +JSON_Value *json_object_get_value_at(const JSON_Object *object, size_t index); +JSON_Value *json_object_get_wrapping_value(const JSON_Object *object); + +/* Functions to check if object has a value with a specific name. Returned value + * is 1 if object has a value and 0 if it doesn't. dothas functions behave + * exactly like dotget functions. */ +int json_object_has_value(const JSON_Object *object, const char *name); +int json_object_has_value_of_type(const JSON_Object *object, const char *name, + JSON_Value_Type type); + +int json_object_dothas_value(const JSON_Object *object, const char *name); +int json_object_dothas_value_of_type(const JSON_Object *object, + const char *name, JSON_Value_Type type); + +/* Creates new name-value pair or frees and replaces old value with a new one. + * json_object_set_value does not copy passed value so it shouldn't be freed + * afterwards. */ +JSON_Status json_object_set_value(JSON_Object *object, const char *name, + JSON_Value *value); +JSON_Status json_object_set_string(JSON_Object *object, const char *name, + const char *string); +JSON_Status json_object_set_string_with_len( + JSON_Object *object, const char *name, const char *string, + size_t len); /* length shouldn't include last null character */ +JSON_Status json_object_set_number(JSON_Object *object, const char *name, + double number); +JSON_Status json_object_set_boolean(JSON_Object *object, const char *name, + int boolean); +JSON_Status json_object_set_null(JSON_Object *object, const char *name); + +/* Works like dotget functions, but creates whole hierarchy if necessary. + * json_object_dotset_value does not copy passed value so it shouldn't be freed + * afterwards. */ +JSON_Status json_object_dotset_value(JSON_Object *object, const char *name, + JSON_Value *value); +JSON_Status json_object_dotset_string(JSON_Object *object, const char *name, + const char *string); +JSON_Status json_object_dotset_string_with_len( + JSON_Object *object, const char *name, const char *string, + size_t len); /* length shouldn't include last null character */ +JSON_Status json_object_dotset_number(JSON_Object *object, const char *name, + double number); +JSON_Status json_object_dotset_boolean(JSON_Object *object, const char *name, + int boolean); +JSON_Status json_object_dotset_null(JSON_Object *object, const char *name); + +/* Frees and removes name-value pair */ +JSON_Status json_object_remove(JSON_Object *object, const char *name); + +/* Works like dotget function, but removes name-value pair only on exact match. + */ +JSON_Status json_object_dotremove(JSON_Object *object, const char *key); + +/* Removes all name-value pairs in object */ +JSON_Status json_object_clear(JSON_Object *object); + +/* + *JSON Array + */ +JSON_Value *json_array_get_value(const JSON_Array *array, size_t index); +const char *json_array_get_string(const JSON_Array *array, size_t index); +size_t json_array_get_string_len( + const JSON_Array *array, + size_t index); /* doesn't account for last null character */ +JSON_Object *json_array_get_object(const JSON_Array *array, size_t index); +JSON_Array *json_array_get_array(const JSON_Array *array, size_t index); +double json_array_get_number(const JSON_Array *array, + size_t index); /* returns 0 on fail */ +int json_array_get_boolean(const JSON_Array *array, + size_t index); /* returns -1 on fail */ +size_t json_array_get_count(const JSON_Array *array); +JSON_Value *json_array_get_wrapping_value(const JSON_Array *array); + +/* Frees and removes value at given index, does nothing and returns JSONFailure + * if index doesn't exist. Order of values in array may change during execution. + */ +JSON_Status json_array_remove(JSON_Array *array, size_t i); + +/* Frees and removes from array value at given index and replaces it with given + * one. Does nothing and returns JSONFailure if index doesn't exist. + * json_array_replace_value does not copy passed value so it shouldn't be freed + * afterwards. */ +JSON_Status json_array_replace_value(JSON_Array *array, size_t i, + JSON_Value *value); +JSON_Status json_array_replace_string(JSON_Array *array, size_t i, + const char *string); +JSON_Status json_array_replace_string_with_len( + JSON_Array *array, size_t i, const char *string, + size_t len); /* length shouldn't include last null character */ +JSON_Status json_array_replace_number(JSON_Array *array, size_t i, + double number); +JSON_Status json_array_replace_boolean(JSON_Array *array, size_t i, + int boolean); +JSON_Status json_array_replace_null(JSON_Array *array, size_t i); + +/* Frees and removes all values from array */ +JSON_Status json_array_clear(JSON_Array *array); + +/* Appends new value at the end of array. + * json_array_append_value does not copy passed value so it shouldn't be freed + * afterwards. */ +JSON_Status json_array_append_value(JSON_Array *array, JSON_Value *value); +JSON_Status json_array_append_string(JSON_Array *array, const char *string); +JSON_Status json_array_append_string_with_len( + JSON_Array *array, const char *string, + size_t len); /* length shouldn't include last null character */ +JSON_Status json_array_append_number(JSON_Array *array, double number); +JSON_Status json_array_append_boolean(JSON_Array *array, int boolean); +JSON_Status json_array_append_null(JSON_Array *array); + +/* + *JSON Value + */ +JSON_Value *json_value_init_object(void); +JSON_Value *json_value_init_array(void); +JSON_Value * +json_value_init_string(const char *string); /* copies passed string */ +JSON_Value *json_value_init_string_with_len( + const char *string, + size_t length); /* copies passed string, length shouldn't include last null + character */ +JSON_Value *json_value_init_number(double number); +JSON_Value *json_value_init_boolean(int boolean); +JSON_Value *json_value_init_null(void); +JSON_Value *json_value_deep_copy(const JSON_Value *value); +void json_value_free(JSON_Value *value); + +JSON_Value_Type json_value_get_type(const JSON_Value *value); +JSON_Object *json_value_get_object(const JSON_Value *value); +JSON_Array *json_value_get_array(const JSON_Value *value); +const char *json_value_get_string(const JSON_Value *value); +size_t json_value_get_string_len( + const JSON_Value *value); /* doesn't account for last null character */ +double json_value_get_number(const JSON_Value *value); +int json_value_get_boolean(const JSON_Value *value); +JSON_Value *json_value_get_parent(const JSON_Value *value); + +/* Same as above, but shorter */ +JSON_Value_Type json_type(const JSON_Value *value); +JSON_Object *json_object(const JSON_Value *value); +JSON_Array *json_array(const JSON_Value *value); +const char *json_string(const JSON_Value *value); +size_t json_string_len( + const JSON_Value *value); /* doesn't account for last null character */ +double json_number(const JSON_Value *value); +int json_boolean(const JSON_Value *value); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/sensors/bms/packages/jbdtool/r b/sensors/bms/packages/jbdtool/r new file mode 100644 index 00000000..66da59ba --- /dev/null +++ b/sensors/bms/packages/jbdtool/r @@ -0,0 +1,6 @@ +for r in pi win64 linux +do + make TARGET=$r STATIC=yes clean + make TARGET=$r STATIC=yes zip + make TARGET=$r STATIC=yes clean +done diff --git a/sensors/bms/packages/jbdtool/serial.c b/sensors/bms/packages/jbdtool/serial.c new file mode 100644 index 00000000..9a42291f --- /dev/null +++ b/sensors/bms/packages/jbdtool/serial.c @@ -0,0 +1,589 @@ + +/* +Copyright (c) 2021, Stephen P. Shoecraft +All rights reserved. + +This source code is licensed under the BSD-style license found in the +LICENSE file in the root directory of this source tree. +*/ + +#include +#include +#ifdef __WIN32 +#include +#else +#include +#include +#include +#endif +#if USE_BUFFER +#include "buffer.h" +#endif +#include "mybmm.h" + +#define DEFAULT_SPEED 9600 + +struct serial_session { +#ifdef __WIN32 + HANDLE h; +#else + int fd; +#endif + char target[32]; + int speed, data, stop, parity, vmin, vtime; +#if USE_BUFFER + buffer_t *buffer; +#endif +}; +typedef struct serial_session serial_session_t; + +#if USE_BUFFER +static int serial_get(void *handle, uint8_t *buffer, int buflen) { + serial_session_t *s = handle; + int bytes; + +#ifdef __WIN32 + ReadFile(s->h, buffer, buflen, (LPDWORD)&bytes, 0); + dprintf(7, "bytes: %d\n", bytes); + return bytes; +#else + struct timeval tv; + fd_set rfds; + int num, count; + + /* We are cooked - just read whats avail */ + bytes = read(s->fd, buffer, buflen); + + count = 0; + FD_ZERO(&rfds); + FD_SET(s->fd, &rfds); + // dprintf(8,"waiting...\n"); + // tv.tv_usec = 500000; + tv.tv_usec = 0; + tv.tv_sec = 1; + num = select(s->fd + 1, &rfds, 0, 0, &tv); + dprintf(8, "num: %d\n", num); + if (num < 1) + goto serial_get_done; + + bytes = read(s->fd, buffer, buflen); + if (bytes < 0) { + log_write(LOG_SYSERR | LOG_DEBUG, "serial_get: read"); + return -1; + } + count = bytes; + +serial_get_done: + if (debug >= 8 && count > 0) + bindump("FROM DEVICE", buffer, count); + dprintf(8, "returning: %d\n", count); + return count; +#endif +} +#endif + +static int serial_init(mybmm_config_t *conf) { return 0; } + +static void *serial_new(mybmm_config_t *conf, ...) { + serial_session_t *s; + // char device[MYBMM_TARGET_LEN+1],*p; + // int baud,data,parity,stop; + char *p; + va_list(ap); + char *target, *topts; + + va_start(ap, conf); + target = va_arg(ap, char *); + topts = va_arg(ap, char *); + va_end(ap); + dprintf(1, "target: %s\n", target); + + s = calloc(1, sizeof(*s)); + if (!s) { + log_write(LOG_SYSERR, "serial_new: calloc(%d)", sizeof(*s)); + return 0; + } +#if USE_BUFFER + s->buffer = buffer_init(1024, serial_get, s); + if (!s->buffer) + return 0; +#endif + +#ifdef __WIN32 + s->h = INVALID_HANDLE_VALUE; +#else + s->fd = -1; +#endif + strncat(s->target, strele(0, ",", (char *)target), sizeof(s->target) - 1); + + /* Baud rate */ + p = strele(0, ",", topts); + s->speed = atoi(p); + if (!s->speed) + s->speed = DEFAULT_SPEED; + + /* Data bits */ + p = strele(1, ",", topts); + s->data = atoi(p); + if (!s->data) + s->data = 8; + + /* Parity */ + p = strele(2, ",", topts); + if (strlen(p)) { + switch (p[0]) { + case 'N': + default: + s->parity = 0; + break; + case 'E': + s->parity = 1; + break; + case 'O': + s->parity = 2; + break; + } + } + + /* Stop */ + p = strele(3, ",", topts); + s->stop = atoi(p); + if (!s->stop) + s->stop = 1; + + /* vmin */ + p = strele(4, ",", topts); + s->vmin = atoi(p); + if (!s->vmin) + s->vmin = 0; + + /* vtime */ + p = strele(5, ",", topts); + s->vtime = atoi(p); + if (!s->vtime) + s->vtime = 5; + + dprintf(1, + "target: %s, speed: %d, data: %d, parity: %c, stop: %d, vmin: %d, " + "vtime: %d\n", + s->target, s->speed, s->data, + s->parity == 0 ? 'N' + : s->parity == 2 ? 'O' + : 'E', + s->stop, s->vmin, s->vtime); + + return s; +} + +#ifndef __WIN32 +static int set_interface_attribs(int fd, int speed, int data, int parity, + int stop, int vmin, int vtime) { + struct termios tty; + int rate; + + dprintf(1, + "fd: %d, speed: %d, data: %d, parity: %d, stop: %d, vmin: %d, vtime: " + "%d\n", + fd, speed, data, parity, stop, vmin, vtime); + + // Don't block + fcntl(fd, F_SETFL, FNDELAY); + + // Get current device tty + tcgetattr(fd, &tty); + +#if 0 + // Set the transport port in RAW Mode (non cooked) + tty.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL|IXON); + tty.c_cflag |= (CLOCAL | CREAD); + tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + tty.c_oflag &= ~OPOST; +#else + /* Or we can just use this :) */ + cfmakeraw(&tty); +#endif + + tty.c_cc[VMIN] = vmin; + tty.c_cc[VTIME] = vtime; + + /* Baud */ + switch (speed) { + case 9600: + rate = B9600; + break; + case 19200: + rate = B19200; + break; + case 1200: + rate = B1200; + break; + case 2400: + rate = B2400; + break; + case 4800: + rate = B4800; + break; + case 38400: + rate = B38400; + break; + case 57600: + rate = B57600; + break; + case 115200: + rate = B115200; + break; + case 230400: + rate = B230400; + break; + case 460800: + rate = B460800; + break; + case 500000: + rate = B500000; + break; + case 576000: + rate = B576000; + break; + case 921600: + rate = B921600; + break; + case 600: + rate = B600; + break; + case 300: + rate = B300; + break; + case 150: + rate = B150; + break; + case 110: + rate = B110; + break; + default: + tty.c_cflag &= ~CBAUD; + tty.c_cflag |= CBAUDEX; + rate = speed; + break; + } + cfsetispeed(&tty, rate); + cfsetospeed(&tty, rate); + + /* Data bits */ + switch (data) { + case 5: + tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS5; + break; + case 6: + tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS6; + break; + case 7: + tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS7; + break; + case 8: + default: + tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; + break; + } + + /* Stop bits (2 = 2, any other value = 1) */ + tty.c_cflag &= ~CSTOPB; + if (stop == 2) + tty.c_cflag |= CSTOPB; + else + tty.c_cflag &= ~CSTOPB; + + /* Parity (0=none, 1=even, 2=odd */ + tty.c_cflag &= ~(PARENB | PARODD); + switch (parity) { + case 1: + tty.c_cflag |= PARENB; + break; + case 2: + tty.c_cflag |= PARODD; + break; + default: + break; + } + + // No flow control + // tty.c_cflag &= ~CRTSCTS; + + // do it + tcsetattr(fd, TCSANOW, &tty); + tcflush(fd, TCIOFLUSH); + return 0; +} + +static int serial_open(void *handle) { + serial_session_t *s = handle; + char path[256]; + + dprintf(1, "target: %s\n", s->target); + strcpy(path, s->target); + if ((access(path, 0)) == 0) { + if (strncmp(path, "/dev", 4) != 0) { + sprintf(path, "/dev/%s", s->target); + dprintf(1, "path: %s\n", path); + } + } + dprintf(1, "path: %s\n", path); + // s->fd = open(path, O_RDWR | O_NOCTTY | O_NDELAY ); + s->fd = open(path, O_RDWR | O_NOCTTY); + if (s->fd < 0) { + log_write(LOG_SYSERR, "open %s\n", path); + return 1; + } + set_interface_attribs(s->fd, s->speed, s->data, s->parity, s->stop, s->vmin, + s->vtime); + + return 0; +} +#else +static int set_interface_attribs(HANDLE h, int speed, int data, int parity, + int stop, int vmin, int vtime) { + DCB Dcb; + COMMTIMEOUTS timeout; + + GetCommState(h, &Dcb); + + /* Baud */ + Dcb.BaudRate = speed; + + /* Stop bits (2 = 2, any other value = 1) */ + if (stop == 2) + Dcb.StopBits = TWOSTOPBITS; + else + Dcb.StopBits = ONESTOPBIT; + + /* Data bits */ + Dcb.ByteSize = data; + + /* Parity (0=none, 1=even, 2=odd */ + switch (parity) { + case 1: + Dcb.Parity = EVENPARITY; + Dcb.fParity = 1; + break; + case 2: + Dcb.Parity = ODDPARITY; + Dcb.fParity = 1; + break; + default: + Dcb.Parity = NOPARITY; + Dcb.fParity = 0; + break; + } + + Dcb.fBinary = 1; + Dcb.fOutxCtsFlow = 0; + Dcb.fOutxDsrFlow = 0; + Dcb.fDsrSensitivity = 0; + Dcb.fTXContinueOnXoff = TRUE; + Dcb.fOutX = 0; + Dcb.fInX = 0; + Dcb.fNull = 0; + Dcb.fErrorChar = 0; + Dcb.fAbortOnError = 0; + Dcb.fRtsControl = RTS_CONTROL_DISABLE; + Dcb.fDtrControl = DTR_CONTROL_DISABLE; + + SetCommState(h, &Dcb); + + // Set read timeouts + if (vtime > 0) { + if (vmin > 0) { // Intercharacter timeout + timeout.ReadIntervalTimeout = vtime * 100; // Deciseconds to milliseconds + } else { // Total timeout + timeout.ReadTotalTimeoutConstant = + vtime * 100; // Deciseconds to milliseconds + } + } else { + if (vmin > 0) { // Counted read + // Handled by length parameter of serialRead(); timeouts remain 0 (unused) + } else { //"Nonblocking" read + timeout.ReadTotalTimeoutConstant = + 1; // Wait as little as possible for a blocking read (1 millisecond) + } + } + if (!SetCommTimeouts(h, &timeout)) { + log_write(LOG_ERROR, "Unable to setting serial port timeout: %lu\n", + GetLastError()); + } + return 0; +} + +static int serial_open(void *handle) { + serial_session_t *s = handle; + char path[256]; + + dprintf(1, "target: %s\n", s->target); + sprintf(path, "\\\\.\\%s", s->target); + dprintf(1, "path: %s\n", path); + s->h = CreateFileA(path, // port name + GENERIC_READ | GENERIC_WRITE, // Read/Write + 0, // No Sharing + NULL, // No Security + OPEN_EXISTING, // Open existing port only + 0, // Non Overlapped I/O + NULL); // Null for Comm Devices + + dprintf(1, "h: %p\n", s->h); + if (s->h == INVALID_HANDLE_VALUE) + return 1; + + set_interface_attribs(s->h, s->speed, s->data, s->parity, s->stop, s->vmin, + s->vtime); + + dprintf(1, "done!\n"); + return 0; +} +#endif + +#if USE_BUFFER +static int serial_read(void *handle, void *buf, int buflen) { + serial_session_t *s = handle; + + return buffer_get(s->buffer, buf, buflen); +} +#else +static int serial_read(void *handle, ...) { + serial_session_t *s = handle; + uint8_t *buf; + int bytes, buflen; + va_list ap; +#ifndef __WIN32 + struct timeval tv; + fd_set rfds; + int num, count; +#endif + + va_start(ap, handle); + buf = va_arg(ap, uint8_t *); + buflen = va_arg(ap, int); + va_end(ap); + dprintf(8, "buf: %p, buflen: %d\n", buf, buflen); + if (!buf || !buflen) + return 0; + +#ifdef __WIN32 + ReadFile(s->h, buf, buflen, (LPDWORD)&bytes, 0); + dprintf(1, "bytes: %d\n", bytes); + return bytes; +#else +#if 0 + time(&start); + for(i=0; i < buflen; i++) { + bytes = read(s->fd,p,1); + dprintf(8,"bytes: %d\n", bytes); + if (bytes < 0) { + if (errno == EAGAIN) break; + else return -1; + } + if (bytes == 0) break; + time(&cur); + if (cur - start >= 1) break; + dprintf(8,"ch: %02X\n", *p); + if (*p == 0x11 || *p == 0x13) { + printf("***** p: %02X ******\n",*p); + exit(0); + } + p++; + } + count = i; +#else + /* See if there's anything to read */ + count = 0; + FD_ZERO(&rfds); + FD_SET(s->fd, &rfds); + // dprintf(8,"waiting...\n"); + tv.tv_usec = 500000; + tv.tv_sec = 0; + num = select(s->fd + 1, &rfds, 0, 0, &tv); + // dprintf(8,"num: %d\n", num); + if (num < 1) + goto serial_read_done; + + /* Read */ + bytes = read(s->fd, buf, buflen); + if (bytes < 0) { + log_write(LOG_SYSERR | LOG_DEBUG, "serial_read: read"); + return -1; + } + count = bytes; +#endif +#endif + +#if 0 + /* See how many bytes are waiting to be read */ + bytes = ioctl(s->fd, FIONREAD, &count); +// dprintf(8,"bytes: %d\n", bytes); + if (bytes < 0) { + log_write(LOG_SYSERR|LOG_DEBUG,"serial_read: iotcl"); + return -1; + } +// dprintf(8,"count: %d\n", count); + /* select said there was data yet there is none? */ + if (count == 0) goto serial_read_done; + +#endif + +#ifndef __WIN32 +serial_read_done: + if (debug >= 8 && count > 0) + bindump("FROM DEVICE", buf, count); + dprintf(8, "returning: %d\n", count); + return count; +#endif +} +#endif + +static int serial_write(void *handle, ...) { + serial_session_t *s = handle; + uint8_t *buf; + int bytes, buflen; + va_list ap; + + va_start(ap, handle); + buf = va_arg(ap, uint8_t *); + buflen = va_arg(ap, int); + va_end(ap); + + dprintf(8, "buf: %p, buflen: %d\n", buf, buflen); + +#ifdef __WIN32 + WriteFile(s->h, buf, buflen, (LPDWORD)&bytes, 0); +#else + bytes = write(s->fd, buf, buflen); + dprintf(8, "bytes: %d\n", bytes); +#endif + if (debug >= 8 && bytes > 0) + bindump("TO DEVICE", buf, buflen); + // usleep ((buflen + 25) * 10000); + return bytes; +} + +static int serial_close(void *handle) { + serial_session_t *s = handle; + +#ifdef _WIN32 + CloseHandle(s->h); + s->h = INVALID_HANDLE_VALUE; +#else + dprintf(1, "fd: %d\n", s->fd); + if (s->fd >= 0) { + close(s->fd); + s->fd = -1; + } +#endif + return 0; +} + +mybmm_module_t serial_module = {MYBMM_MODTYPE_TRANSPORT, + "serial", + 0, + serial_init, + serial_new, + serial_open, + serial_read, + serial_write, + serial_close}; diff --git a/sensors/bms/packages/jbdtool/si.h b/sensors/bms/packages/jbdtool/si.h new file mode 100644 index 00000000..b519b33d --- /dev/null +++ b/sensors/bms/packages/jbdtool/si.h @@ -0,0 +1,19 @@ + +#ifndef __MYBMM_SI_H +#define __MYBMM_SI_H + +struct si_session { + mybmm_config_t *conf; + mybmm_inverter_t *inv; + pthread_t th; + uint32_t bitmap; + uint8_t messages[16][8]; + mybmm_module_t *tp; + void *tp_handle; + int stop; + int open; + int (*get_data)(struct si_session *, int id, uint8_t *data, int len); +}; +typedef struct si_session si_session_t; + +#endif /* __MYBMM_SI_H */ diff --git a/sensors/bms/packages/jbdtool/utils.c b/sensors/bms/packages/jbdtool/utils.c new file mode 100644 index 00000000..ec1a13d2 --- /dev/null +++ b/sensors/bms/packages/jbdtool/utils.c @@ -0,0 +1,446 @@ + +/* +Copyright (c) 2021, Stephen P. Shoecraft +All rights reserved. + +This source code is licensed under the BSD-style license found in the +LICENSE file in the root directory of this source tree. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef DEBUG +#undef DEBUG +#endif +//#define DEBUG 1 +#include "debug.h" +#include "utils.h" + +void _bindump(long offset, void *bptr, int len) { + char line[128]; + unsigned char *buf = bptr; + int end; + register char *ptr; + register int x, y; + +// printf("buf: %p, len: %d\n", buf, len); +#ifdef __WIN32__ + if (buf == (void *)0xBAADF00D) + return; +#endif + + for (x = y = 0; x < len; x += 16) { + sprintf(line, "%04lX: ", offset); + ptr = line + strlen(line); + end = (x + 16 >= len ? len : x + 16); + for (y = x; y < end; y++) { + sprintf(ptr, "%02X ", buf[y]); + ptr += 3; + } + for (y = end; y < x + 17; y++) { + sprintf(ptr, " "); + ptr += 3; + } + for (y = x; y < end; y++) { + if (buf[y] > 31 && buf[y] < 127) + *ptr++ = buf[y]; + else + *ptr++ = '.'; + } + for (y = end; y < x + 16; y++) + *ptr++ = ' '; + *ptr = 0; + printf("%s\n", line); + offset += 16; + } +} + +void bindump(char *label, void *bptr, int len) { + printf("%s(%d):\n", label, len); + _bindump(0, bptr, len); +} + +char *trim(char *string) { + register char *src, *dest; + + /* If string is empty, just return it */ + if (*string == '\0') + return string; + + /* Trim the front */ + src = string; + // while(isspace((int)*src) && *src != '\t') src++; + while (isspace((int)*src) || (*src > 0 && *src < 32)) + src++; + dest = string; + while (*src != '\0') + *dest++ = *src++; + + /* Trim the back */ + *dest-- = '\0'; + while ((dest >= string) && isspace((int)*dest)) + dest--; + *(dest + 1) = '\0'; + + return string; +} + +char *strele(int num, char *delimiter, char *string) { + static char return_info[1024]; + register char *src, *dptr, *eptr, *cptr; + register char *dest, qc; + register int count; + +#ifdef DEBUG_STRELE + printf("Element: %d, delimiter: %s, string: %s\n", num, delimiter, string); +#endif + + eptr = string; + dptr = delimiter; + dest = return_info; + count = 0; + qc = 0; + for (src = string; *src != '\0'; src++) { +#ifdef DEBUG_STRELE + printf("src: %d, qc: %d\n", *src, qc); +#endif + if (qc) { + if (*src == qc) + qc = 0; + continue; + } else { + if (*src == '\"' || *src == '\'') { + qc = *src; + cptr++; + } + } + if (isspace(*src)) + *src = 32; +#ifdef DEBUG_STRELE + if (*src) + printf("src: %c == ", *src); + else + printf("src: (null) == "); + if (*dptr != 32) + printf("dptr: %c\n", *dptr); + else if (*dptr == 32) + printf("dptr: (space)\n"); + else + printf("dptr: (null)\n"); +#endif + if (*src == *dptr) { + cptr = src + 1; + dptr++; +#ifdef DEBUG_STRELE + if (*cptr != '\0') + printf("cptr: %c == ", *cptr); + else + printf("cptr: (null) == "); + if (*dptr != '\0') + printf("dptr: %c\n", *dptr); + else + printf("dptr: (null)\n"); +#endif + while (*cptr == *dptr && *cptr != '\0' && *dptr != '\0') { + cptr++; + dptr++; +#ifdef DEBUG_STRELE + if (*cptr != '\0') + printf("cptr: %c == ", *cptr); + else + printf("cptr: (null) == "); + if (*dptr != '\0') + printf("dptr: %c\n", *dptr); + else + printf("dptr: (null)\n"); +#endif + if (*dptr == '\0' || *cptr == '\0') { +#ifdef DEBUG_STRELE + printf("Breaking...\n"); +#endif + break; + } + /* + dptr++; + if (*dptr == '\0') break; + cptr++; + if (*cptr == '\0') break; + */ + } +#ifdef DEBUG_STRELE + if (*cptr != '\0') + printf("cptr: %c == ", *cptr); + else + printf("cptr: (null) == "); + if (*dptr != '\0') + printf("dptr: %c\n", *dptr); + else + printf("dptr: (null)\n"); +#endif + if (*dptr == '\0') { +#ifdef DEBUG_STRELE + printf("Count: %d, num: %d\n", count, num); +#endif + if (count == num) + break; + if (cptr > src + 1) + src = cptr - 1; + eptr = src + 1; + count++; + // printf("eptr[0]: %c\n", eptr[0]); + if (*eptr == '\"' || *eptr == '\'') + eptr++; +#ifdef DEBUG_STRELE + printf("eptr: %s, src: %s\n", eptr, src + 1); +#endif + } + dptr = delimiter; + } + } +#ifdef DEBUG_STRELE + printf("Count: %d, num: %d\n", count, num); +#endif + if (count == num) { +#ifdef DEBUG_STRELE + printf("eptr: %s\n", eptr); + printf("src: %s\n", src); +#endif + while (eptr < src) { + if (*eptr == '\"' || *eptr == '\'') + break; + *dest++ = *eptr++; + } + } + *dest = '\0'; +#ifdef DEBUG_STRELE + printf("Returning: %s\n", return_info); +#endif + return (return_info); +} + +int is_ip(char *string) { + register char *ptr; + int dots, digits; + + dprintf(7, "string: %s\n", string); + + digits = dots = 0; + for (ptr = string; *ptr; ptr++) { + if (*ptr == '.') { + if (!digits) + goto is_ip_error; + if (++dots > 4) + goto is_ip_error; + digits = 0; + } else if (isdigit((int)*ptr)) { + if (++digits > 3) + goto is_ip_error; + } + } + dprintf(7, "dots: %d\n", dots); + + return (dots == 4 ? 1 : 0); +is_ip_error: + return 0; +} + +FILE *logfp = (FILE *)0; +static int logopts; + +int log_open(char *ident, char *filename, int opts) { + DPRINTF("filename: %s\n", filename); + if (filename) { + char *op; + + /* Open the file */ + op = (opts & LOG_CREATE ? "w+" : "a+"); + logfp = fopen(filename, op); + if (!logfp) { + perror("log_open: unable to create logfile"); + return 1; + } + } else if (opts & LOG_STDERR) { + DPRINTF("logging to stderr\n"); + logfp = stderr; + } else { + DPRINTF("logging to stdout\n"); + logfp = stdout; + } + logopts = opts; + + DPRINTF("log is opened.\n"); + return 0; +} + +int get_timestamp(char *ts, int tslen, int local) { + struct tm *tptr; + time_t t; + char temp[32]; + int len; + + if (!ts || !tslen) + return 1; + + /* Fill the tm struct */ + t = time(NULL); + tptr = 0; + DPRINTF("local: %d\n", local); + if (local) + tptr = localtime(&t); + else + tptr = gmtime(&t); + if (!tptr) { + DPRINTF("unable to get %s!\n", local ? "localtime" : "gmtime"); + return 1; + } + + /* Set month to 1 if month is out of range */ + if (tptr->tm_mon < 0 || tptr->tm_mon > 11) + tptr->tm_mon = 0; + + /* Fill string with yyyymmddhhmmss */ + sprintf(temp, "%04d-%02d-%02d %02d:%02d:%02d", 1900 + tptr->tm_year, + tptr->tm_mon + 1, tptr->tm_mday, tptr->tm_hour, tptr->tm_min, + tptr->tm_sec); + + ts[0] = 0; + len = tslen < strlen(temp) - 1 ? strlen(temp) - 1 : tslen; + DPRINTF("returning: %s\n", ts); + strncat(ts, temp, len); + return 0; +} + +static char message[32767]; + +int log_write(int type, char *format, ...) { + va_list ap; + char dt[32], error[128]; + register char *ptr; + + /* Make sure log_open was called */ + if (!logfp) + log_open("", 0, + LOG_INFO | LOG_WARNING | LOG_ERROR | LOG_SYSERR | LOG_DEBUG); + + /* Do we even log this type? */ + DPRINTF("logopts: %0x, type: %0x\n", logopts, type); + if ((logopts | type) != logopts) + return 0; + + /* get the error text asap before it's gone */ + if (type & LOG_SYSERR) { + error[0] = 0; + strncat(error, strerror(errno), sizeof(error)); + } + + /* Prepend the time? */ + ptr = message; + if (logopts & LOG_TIME || type & LOG_TIME) { + // struct tm *tptr; + // time_t t; + + DPRINTF("prepending time...\n"); + get_timestamp(dt, sizeof(dt), 1); +#if 0 + /* Fill the tm struct */ + t = time(NULL); + tptr = 0; + DPRINTF("getting localtime\n"); + tptr = localtime(&t); + if (!tptr) { + DPRINTF("unable to get localtime!\n"); + return 1; + } + + /* Set month to 1 if month is out of range */ + if (tptr->tm_mon < 0 || tptr->tm_mon > 11) tptr->tm_mon = 0; + + /* Fill string with yyyymmddhhmmss */ + sprintf(dt,"%04d-%02d-%02d %02d:%02d:%02d", + 1900+tptr->tm_year,tptr->tm_mon+1,tptr->tm_mday, + tptr->tm_hour,tptr->tm_min,tptr->tm_sec); +#endif + + strcat(dt, " "); + ptr += sprintf(ptr, "%s", dt); + } + + /* If it's a warning, prepend warning: */ + if (type & LOG_WARNING) { + DPRINTF("prepending warning...\n"); + sprintf(ptr, "warning: "); + ptr += strlen(ptr); + } + + /* If it's an error, prepend error: */ + else if ((type & LOG_ERROR) || (type & LOG_SYSERR)) { + DPRINTF("prepending error...\n"); + sprintf(ptr, "error: "); + ptr += strlen(ptr); + } + + /* Build the rest of the message */ + DPRINTF("adding message...\n"); + va_start(ap, format); + vsprintf(ptr, format, ap); + va_end(ap); + + /* Trim */ + trim(message); + + /* If it's a system error, concat the system message */ + if (type & LOG_SYSERR) { + DPRINTF("adding error text...\n"); + strcat(message, ": "); + strcat(message, error); + } + + /* Strip all CRs and LFs */ + DPRINTF("stripping newlines...\n"); + for (ptr = message; *ptr; ptr++) { + if (*ptr == '\n' || *ptr == '\r') + strcpy(ptr, ptr + 1); + } + + /* Write the message */ + DPRINTF("message: %s\n", message); + fprintf(logfp, "%s\n", message); + fflush(logfp); + return 0; +} + +#ifndef WINDOWS +int lock_file(char *path, int wait) { + struct flock fl; + int fd, op; + + fd = open(path, O_CREAT | O_RDWR, 0666); + if (fd < 0) { + log_syserror("lock_file: open(%s)", path); + return -1; + } + + fl.l_type = F_WRLCK; + fl.l_whence = SEEK_SET; + fl.l_start = 0; + fl.l_len = 1; + op = (wait ? F_SETLKW : F_SETLK); + if (fcntl(fd, op, &fl) < 0) { + log_syserror("lock_file: fcntl"); + close(fd); + return -1; + } + return fd; +} + +void unlock_file(int fd) { + close(fd); + return; +} +#endif diff --git a/sensors/bms/packages/jbdtool/utils.h b/sensors/bms/packages/jbdtool/utils.h new file mode 100644 index 00000000..0c72d58a --- /dev/null +++ b/sensors/bms/packages/jbdtool/utils.h @@ -0,0 +1,51 @@ + +#ifndef __UTILS_H +#define __UTILS_H + +#include "cfg.h" + +float pct(float, float); +void bindump(char *label, void *bptr, int len); +void _bindump(long offset, void *bptr, int len); +char *trim(char *); +char *strele(int num, char *delimiter, char *string); +int is_ip(char *); +int get_timestamp(char *ts, int tslen, int local); + +/* Define the log options */ +#define LOG_CREATE 0x0001 /* Create a new logfile */ +#define LOG_TIME 0x0002 /* Prepend the time */ +#define LOG_STDERR 0x0004 /* Log to stderr */ +#define LOG_INFO 0x0008 /* Informational messages */ +#define LOG_VERBOSE 0x0010 /* Full info messages */ +#define LOG_WARNING 0x0020 /* Program warnings */ +#define LOG_ERROR 0x0040 /* Program errors */ +#define LOG_SYSERR 0x0080 /* System errors */ +#define LOG_DEBUG 0x0100 /* Misc debug messages */ +#define LOG_DEBUG2 0x0200 /* func entry/exit */ +#define LOG_DEBUG3 0x0400 /* inner loops! */ +#define LOG_DEBUG4 0x0800 /* The dolly-lamma */ +#define LOG_WX 0x8000 /* Log to wxMessage */ +#define LOG_ALL 0x7FFF /* Whoa, Nellie */ +#define LOG_DEFAULT (LOG_INFO | LOG_WARNING | LOG_ERROR | LOG_SYSERR) + +#define LOG_SYSERROR LOG_SYSERR + +/* Function definitions */ +int log_open(char *, char *, int); +int log_read(char *, int); +int log_write(int, char *, ...); +#define log_info(args...) log_write(LOG_INFO, args) +#define log_error(args...) log_write(LOG_ERROR, args) +#define log_syserror(args...) log_write(LOG_SYSERROR, args) +int log_debug(char *, ...); +void log_close(void); +void log_writeopts(void); +char *log_nextname(void); + +#define lprintf(mask, format, args...) log_write(mask, format, ##args) + +int lock_file(char *, int); +void unlock_file(int); + +#endif diff --git a/sensors/bms/packages/jbdtool/uuid.h b/sensors/bms/packages/jbdtool/uuid.h new file mode 100644 index 00000000..9d6e7d67 --- /dev/null +++ b/sensors/bms/packages/jbdtool/uuid.h @@ -0,0 +1,19 @@ + +#ifndef __UUID_H +#define __UUID_H + +typedef unsigned char uuid_t[16]; + +#if 0 +typedef struct { + unsigned char b[16]; +} uuid_t; +#endif + +void uuid_generate_random(uuid_t); +int uuid_parse(const char *uuid, uuid_t *u); +void my_uuid_unparse(uuid_t uu, char *out); +void uuid_unparse_upper(uuid_t uu, char *out); +void uuid_unparse_lower(uuid_t uu, char *out); + +#endif diff --git a/sensors/bms/packages/jbdtool/worker.h b/sensors/bms/packages/jbdtool/worker.h new file mode 100644 index 00000000..c0c18a2e --- /dev/null +++ b/sensors/bms/packages/jbdtool/worker.h @@ -0,0 +1,24 @@ + +#ifndef __WORKER_H +#define __WORKER_H + +typedef void (*worker_func_t)(void *); + +struct worker_info; +typedef struct worker_info worker_info_t; + +struct worker_pool { + worker_info_t *workers; + int count; +}; +typedef struct worker_pool worker_pool_t; + +extern worker_pool_t *worker_create_pool(int); +extern int worker_destroy_pool(worker_pool_t *, int); +extern int worker_exec(worker_pool_t *, worker_func_t func, void *arg); +extern void worker_wait(worker_pool_t *, int); +extern void worker_killbusy(worker_pool_t *); +extern void worker_kill(worker_pool_t *); +extern void worker_finish(worker_pool_t *, int); + +#endif /* __WORKER_H */ diff --git a/sensors/bms/readme.md b/sensors/bms/readme.md index 28d2748b..3aad312a 100644 --- a/sensors/bms/readme.md +++ b/sensors/bms/readme.md @@ -19,7 +19,7 @@ The node publishes a message of type BatteryState to the bms topics, and a Diagn The node can be run with: ``` -ros2 launch bms_launch.py +ros2 launch bms bms_launch.py ``` or alternatively @@ -82,27 +82,12 @@ create_key_value_pair(key: str, value) -> KeyValue: creates KeyValue object from supplied key and value ``` -## Startup service +## Service file bootup -Note: The .service file may need to be edited in order to source the setup.bash file -from the correct workspace. - -This package also contains a .service file for running the node on startup. To enable this, just run (from this directory) -``` -sudo cp startup_script/bms_startup.service /etc/systemd/system/bms_startup.service -``` -then run the following commands -``` -cd /etc/systemd/system -``` -``` -sudo systemctl daemon-reload && sudo systemctl enable bms_startup.service -``` -To verify that the service is enabled, run +To start the BMS publishing automaticaly every time on bootup just run this command: ``` -systemctl list-unit-files | grep enabled +./vortex-asv/add_service_files_to_bootup_sequence.sh ``` -The file will now run automatically on startup the next time the system starts. -If this fails, try checking if systemd is installed. +Enjoy :) diff --git a/sensors/bms/startup_script/bms_startup.service b/sensors/bms/startup_script/bms_startup.service deleted file mode 100644 index 53f33a25..00000000 --- a/sensors/bms/startup_script/bms_startup.service +++ /dev/null @@ -1,23 +0,0 @@ -[Unit] -Description=launch ros2 bms node -After=network.target - -[Service] -ExecStart=/bin/bash -c 'source /opt/ros/humble/setup.bash; source /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash; source /usr/share/colcon_cd/function/colcon_cd.sh colcon build; source /home/vortex/david_test_ws/install/setup.bash; ros2 launch bms bms_launch.py; sleep 60' -RemainAfterExit=yes -WorkingDirectory=/home/vortex/david_test_ws -StandardOutput=journal+console - - -Environment="HOME=root" -Environment="ROS_DOMAIN_ID=1" -Environment="ROS_LOCALHOST_ONLY=1" -Environment="_colcon_cd_root=/top/ros/rolling/" - -User=vortex - -Restart=no -RestartSec=2 - -[Install] -WantedBy=multi-user.target diff --git a/sensors/bms/startup_scripts/bms.service b/sensors/bms/startup_scripts/bms.service new file mode 100644 index 00000000..34a7953d --- /dev/null +++ b/sensors/bms/startup_scripts/bms.service @@ -0,0 +1,17 @@ +[Unit] +Description=Launch internal bms node +After=network.target + +[Service] +# Use the wrapper script for ExecStart +# DONT CHANGE THIS LINE!!!! +# Use vortex-asv/scripts/add_service_files_to_bootup_sequence.sh to automaticaly set everything up +ExecStart=/bin/bash 'bms.sh' +WorkingDirectory=/home/vortex/ +StandardOutput=journal+console +User=vortex +Restart=no +RestartSec=2 + +[Install] +WantedBy=multi-user.target \ No newline at end of file diff --git a/sensors/bms/startup_scripts/bms.sh b/sensors/bms/startup_scripts/bms.sh new file mode 100644 index 00000000..429d803c --- /dev/null +++ b/sensors/bms/startup_scripts/bms.sh @@ -0,0 +1,7 @@ +# Determine the directory where the script is located +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" + +cd $SCRIPT_DIR +cd ../../../../.. # Go back to workspace +source install/setup.bash +ros2 launch bms bms_launch.py \ No newline at end of file diff --git a/sensors/temperature/README b/sensors/temperature/README new file mode 100644 index 00000000..8ad1b40e --- /dev/null +++ b/sensors/temperature/README @@ -0,0 +1,11 @@ +## Temperature + +We messure temperature of the 4 ESCs that run the thrusters as well as 2 temperure sensors for the ambient temperature in the electrical housing + +## Service file bootup + +To start the Temperature publishing automaticaly every time on bootup just run this command: +./vortex-asv/add_service_files_to_bootup_sequence.sh + +Enjoy :) + diff --git a/sensors/temperature/startup_scripts/temperature.service b/sensors/temperature/startup_scripts/temperature.service new file mode 100644 index 00000000..a1b44d36 --- /dev/null +++ b/sensors/temperature/startup_scripts/temperature.service @@ -0,0 +1,17 @@ +[Unit] +Description=Launch internal status node +After=network.target + +[Service] +# Use the wrapper script for ExecStart +# DONT CHANGE THIS LINE!!!! +# Use vortex-asv/scripts/add_service_files_to_bootup_sequence.sh to automaticaly set everything up +ExecStart=/bin/bash 'temperature.sh' +WorkingDirectory=/home/vortex/ +StandardOutput=journal+console +User=vortex +Restart=no +RestartSec=2 + +[Install] +WantedBy=multi-user.target \ No newline at end of file diff --git a/sensors/temperature/startup_scripts/temperature.sh b/sensors/temperature/startup_scripts/temperature.sh new file mode 100644 index 00000000..c3ac25ee --- /dev/null +++ b/sensors/temperature/startup_scripts/temperature.sh @@ -0,0 +1,7 @@ +# Determine the directory where the script is located +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" + +cd $SCRIPT_DIR +cd ../../../../.. # Go back to workspace +source install/setup.bash +ros2 launch temperature temperature_system_launch.py \ No newline at end of file