diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml index 37e299b38b2..6519fcaa9d8 100644 --- a/.github/workflows/gradle.yml +++ b/.github/workflows/gradle.yml @@ -12,9 +12,12 @@ jobs: fail-fast: false matrix: include: - - container: wpilib/roborio-cross-ubuntu:2024-22.04 - artifact-name: Athena - build-options: "-Ponlylinuxathena" + # - container: wpilib/roborio-cross-ubuntu:2025-22.04 + # artifact-name: Athena + # build-options: "-Ponlylinuxathena" + - container: wpilib/systemcore-cross-ubuntu:2025-22.04 + artifact-name: SystemCore + build-options: "-Ponlylinuxsystemcore" - container: wpilib/raspbian-cross-ubuntu:bullseye-22.04 artifact-name: Arm32 build-options: "-Ponlylinuxarm32" diff --git a/.github/workflows/tools.yml b/.github/workflows/tools.yml index 3e71c5e8301..69ad1680340 100644 --- a/.github/workflows/tools.yml +++ b/.github/workflows/tools.yml @@ -42,52 +42,52 @@ jobs: development retention-days: 1 - Robotbuilder: - name: "Build - RobotBuilder" - needs: [build-artifacts] - runs-on: ubuntu-22.04 - env: - DISPLAY: ':10' - steps: - - uses: actions/checkout@v4 - with: - repository: wpilibsuite/robotbuilder - fetch-depth: 0 - - uses: actions/download-artifact@v4 - with: - name: MavenArtifacts - - name: Patch RobotBuilder to use local development - run: cd src/main/resources/export && echo "wpi.maven.useLocal = false" >> java/build.gradle && echo "wpi.maven.useFrcMavenLocalDevelopment = true" >> java/build.gradle && echo "wpi.versions.wpilibVersion = '$YEAR.424242.+'" >> java/build.gradle && echo "wpi.versions.wpimathVersion = '$YEAR.424242.+'" >> java/build.gradle && echo "wpi.maven.useLocal = false" >> cpp/build.gradle && echo "wpi.maven.useFrcMavenLocalDevelopment = true" >> cpp/build.gradle && echo "wpi.versions.wpilibVersion = '$YEAR.424242.+'" >> cpp/build.gradle && echo "wpi.versions.wpimathVersion = '$YEAR.424242.+'" >> cpp/build.gradle - - name: Install and run xvfb - run: sudo apt-get update && sudo apt-get install -y xvfb && Xvfb $DISPLAY & - - name: Move artifacts - run: mkdir -p ~/releases/maven/development && cp -r edu ~/releases/maven/development - - uses: actions/setup-java@v4 - with: - java-version: 17 - distribution: 'temurin' - - name: Build RobotBuilder with Gradle - run: ./gradlew build test --tests 'robotbuilder.exporters.*' -x htmlSanityCheck -PbuildServer -PreleaseMode ; cat build/test-results/test/TEST-robotbuilder.exporters.*.xml ; - - name: Summarize RobotBuilder Test Results - uses: EnricoMi/publish-unit-test-result-action@v2 - if: always() - with: - files: | - build/test-results/test/TEST*.xml - check_run: false - comment_mode: off - - uses: actions/upload-artifact@v4 - if: always() - with: - name: RobotBuilderTestResults - path: | - build/reports/ - - uses: actions/upload-artifact@v4 - with: - name: RobotBuilder Build - path: | - build/libs/ - retention-days: 7 + # Robotbuilder: + # name: "Build - RobotBuilder" + # needs: [build-artifacts] + # runs-on: ubuntu-22.04 + # env: + # DISPLAY: ':10' + # steps: + # - uses: actions/checkout@v4 + # with: + # repository: wpilibsuite/robotbuilder + # fetch-depth: 0 + # - uses: actions/download-artifact@v4 + # with: + # name: MavenArtifacts + # - name: Patch RobotBuilder to use local development + # run: cd src/main/resources/export && echo "wpi.maven.useLocal = false" >> java/build.gradle && echo "wpi.maven.useFrcMavenLocalDevelopment = true" >> java/build.gradle && echo "wpi.versions.wpilibVersion = '$YEAR.424242.+'" >> java/build.gradle && echo "wpi.versions.wpimathVersion = '$YEAR.424242.+'" >> java/build.gradle && echo "wpi.maven.useLocal = false" >> cpp/build.gradle && echo "wpi.maven.useFrcMavenLocalDevelopment = true" >> cpp/build.gradle && echo "wpi.versions.wpilibVersion = '$YEAR.424242.+'" >> cpp/build.gradle && echo "wpi.versions.wpimathVersion = '$YEAR.424242.+'" >> cpp/build.gradle + # - name: Install and run xvfb + # run: sudo apt-get update && sudo apt-get install -y xvfb && Xvfb $DISPLAY & + # - name: Move artifacts + # run: mkdir -p ~/releases/maven/development && cp -r edu ~/releases/maven/development + # - uses: actions/setup-java@v4 + # with: + # java-version: 17 + # distribution: 'temurin' + # - name: Build RobotBuilder with Gradle + # run: ./gradlew build test --tests 'robotbuilder.exporters.*' -x htmlSanityCheck -PbuildServer -PreleaseMode ; cat build/test-results/test/TEST-robotbuilder.exporters.*.xml ; + # - name: Summarize RobotBuilder Test Results + # uses: EnricoMi/publish-unit-test-result-action@v2 + # if: always() + # with: + # files: | + # build/test-results/test/TEST*.xml + # check_run: false + # comment_mode: off + # - uses: actions/upload-artifact@v4 + # if: always() + # with: + # name: RobotBuilderTestResults + # path: | + # build/reports/ + # - uses: actions/upload-artifact@v4 + # with: + # name: RobotBuilder Build + # path: | + # build/libs/ + # retention-days: 7 Shuffleboard: name: "Build - Shuffleboard" diff --git a/build.gradle b/build.gradle index 8a51182b787..327f434456a 100644 --- a/build.gradle +++ b/build.gradle @@ -11,7 +11,7 @@ buildscript { plugins { id 'base' id 'edu.wpi.first.wpilib.versioning.WPILibVersioningPlugin' version '2023.0.1' - id 'edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin' version '2020.2' + id 'edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin' version '2025.0' id 'edu.wpi.first.NativeUtils' apply false id 'edu.wpi.first.GradleJni' version '1.1.0' id 'edu.wpi.first.GradleVsCode' @@ -32,6 +32,7 @@ allprojects { url = 'https://frcmaven.wpi.edu/artifactory/ex-mvn' } } + wpilibRepositories.use2027Repos() if (project.hasProperty('releaseMode')) { wpilibRepositories.addAllReleaseRepositories(it) } else { diff --git a/buildSrc/build.gradle b/buildSrc/build.gradle index 99d4892632a..5c555c1c53a 100644 --- a/buildSrc/build.gradle +++ b/buildSrc/build.gradle @@ -9,5 +9,5 @@ repositories { } } dependencies { - implementation "edu.wpi.first:native-utils:2025.3.0" + implementation "edu.wpi.first:native-utils:2025.7.1" } diff --git a/cscore/build.gradle b/cscore/build.gradle index 5b2299548d7..9cbf42ff857 100644 --- a/cscore/build.gradle +++ b/cscore/build.gradle @@ -23,6 +23,7 @@ model { enableCheckTask true javaCompileTasks << compileJava jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.roborio) + jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.systemcore) jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.linuxarm32) jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.linuxarm64) @@ -177,7 +178,7 @@ model { components { examplesMap.each { key, value -> if (key == "usbviewer") { - if (!project.hasProperty('onlylinuxathena')) { + if (!project.hasProperty('onlylinuxathena') && !project.hasProperty('onlylinuxsystemcore')) { "${key}"(NativeExecutableSpec) { targetBuildTypes 'debug' binaries.all { diff --git a/datalogtool/build.gradle b/datalogtool/build.gradle index 96f3c85d1d9..7ebfffe8cca 100644 --- a/datalogtool/build.gradle +++ b/datalogtool/build.gradle @@ -1,6 +1,6 @@ import org.gradle.internal.os.OperatingSystem -if (project.hasProperty('onlylinuxathena')) { +if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxsystemcore')) { return; } diff --git a/developerRobot/build.gradle b/developerRobot/build.gradle index 684d0ae63d4..7d8a627b232 100644 --- a/developerRobot/build.gradle +++ b/developerRobot/build.gradle @@ -316,5 +316,20 @@ model { } } } + + installSystemCore(Task) { + $.binaries.each { + if (it in NativeExecutableBinarySpec && it.targetPlatform.name == nativeUtils.wpi.platforms.systemcore && it.component.name == 'developerRobotCpp') { + dependsOn it.tasks.install + } + } + } + installSystemCoreStatic(Task) { + $.binaries.each { + if (it in NativeExecutableBinarySpec && it.targetPlatform.name == nativeUtils.wpi.platforms.systemcore && it.component.name == 'developerRobotCppStatic') { + dependsOn it.tasks.install + } + } + } } } diff --git a/fieldImages/build.gradle b/fieldImages/build.gradle index 399ef351fb7..dd38080dd5a 100644 --- a/fieldImages/build.gradle +++ b/fieldImages/build.gradle @@ -1,6 +1,6 @@ import org.gradle.internal.os.OperatingSystem -if (project.hasProperty('onlylinuxathena')) { +if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxsystemcore')) { return; } diff --git a/glass/build.gradle b/glass/build.gradle index 551e6ffaefe..2b7a6105178 100644 --- a/glass/build.gradle +++ b/glass/build.gradle @@ -1,6 +1,6 @@ import org.gradle.internal.os.OperatingSystem -if (project.hasProperty('onlylinuxathena')) { +if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxsystemcore')) { return; } diff --git a/hal/.styleguide b/hal/.styleguide index c04d378d074..82c085c0894 100644 --- a/hal/.styleguide +++ b/hal/.styleguide @@ -14,6 +14,8 @@ generatedFileExclude { hal/src/main/native/athena/frccansae/ hal/src/main/native/athena/visa/ hal/src/main/native/include/ctre/ + hal/src/main/native/systemcore/ctre/ + hal/src/main/native/systemcore/rev/ UsageReporting\.h$ } diff --git a/hal/build.gradle b/hal/build.gradle index 8af0ddad83c..60b8b5ca8f9 100644 --- a/hal/build.gradle +++ b/hal/build.gradle @@ -26,7 +26,20 @@ ext { } } } - } else { + } else if (it.targetPlatform.name == nativeUtils.wpi.platforms.systemcore) { + it.sources { + systemCoreCpp(CppSourceSet) { + source { + srcDirs = ['src/main/native/systemcore'] + include '**/*.cpp' + } + exportedHeaders { + srcDir 'src/main/native/include' + srcDir generatedHeaders + } + } + } + } else { it.sources { simCpp(CppSourceSet) { source { @@ -57,6 +70,10 @@ cppSourcesZip { into '/athena' } + from('src/main/native/systemcore') { + into '/systemcore' + } + from('src/main/native/sim') { into '/sim' } diff --git a/hal/src/main/native/include/hal/HALBase.h b/hal/src/main/native/include/hal/HALBase.h index 56b6ca978b3..9ce3408966c 100644 --- a/hal/src/main/native/include/hal/HALBase.h +++ b/hal/src/main/native/include/hal/HALBase.h @@ -33,7 +33,9 @@ HAL_ENUM(HAL_RuntimeType) { /** roboRIO 2.0 */ HAL_Runtime_RoboRIO2, /** Simulation runtime */ - HAL_Runtime_Simulation + HAL_Runtime_Simulation, + /** SystemCore */ + HAL_Runtime_SystemCore, }; #ifdef __cplusplus diff --git a/hal/src/main/native/systemcore/Accelerometer.cpp b/hal/src/main/native/systemcore/Accelerometer.cpp new file mode 100644 index 00000000000..b94de603bba --- /dev/null +++ b/hal/src/main/native/systemcore/Accelerometer.cpp @@ -0,0 +1,58 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/Accelerometer.h" + +#include + +#include +#include +#include + +#include "HALInitializer.h" +#include "hal/HAL.h" + +using namespace hal; + +namespace hal::init { +void InitializeAccelerometer() {} +} // namespace hal::init + +namespace hal { + +/** + * Initialize the accelerometer. + */ +static void initializeAccelerometer() { + hal::init::CheckInit(); +} + +} // namespace hal + +extern "C" { + +void HAL_SetAccelerometerActive(HAL_Bool active) { + initializeAccelerometer(); +} + +void HAL_SetAccelerometerRange(HAL_AccelerometerRange range) { + initializeAccelerometer(); +} + +double HAL_GetAccelerometerX(void) { + initializeAccelerometer(); + return 0.0; +} + +double HAL_GetAccelerometerY(void) { + initializeAccelerometer(); + return 0.0; +} + +double HAL_GetAccelerometerZ(void) { + initializeAccelerometer(); + return 0.0; +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/AddressableLED.cpp b/hal/src/main/native/systemcore/AddressableLED.cpp new file mode 100644 index 00000000000..76ce8513441 --- /dev/null +++ b/hal/src/main/native/systemcore/AddressableLED.cpp @@ -0,0 +1,84 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/AddressableLED.h" + +#include +#include + +#include + +#include "HALInitializer.h" +#include "hal/Errors.h" + +using namespace hal; + +namespace hal::init { +void InitializeAddressableLED() {} +} // namespace hal::init + +extern "C" { + +HAL_AddressableLEDHandle HAL_InitializeAddressableLED( + HAL_DigitalHandle outputPort, int32_t* status) { + hal::init::CheckInit(); + + *status = HAL_HANDLE_ERROR; + return HAL_kInvalidHandle; +} + +void HAL_FreeAddressableLED(HAL_AddressableLEDHandle handle) {} + +void HAL_SetAddressableLEDOutputPort(HAL_AddressableLEDHandle handle, + HAL_DigitalHandle outputPort, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetAddressableLEDLength(HAL_AddressableLEDHandle handle, + int32_t length, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +static_assert(sizeof(HAL_AddressableLEDData) == sizeof(uint32_t), + "LED Data must be 32 bit"); + +void HAL_WriteAddressableLEDData(HAL_AddressableLEDHandle handle, + const struct HAL_AddressableLEDData* data, + int32_t length, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetAddressableLEDBitTiming(HAL_AddressableLEDHandle handle, + int32_t highTime0NanoSeconds, + int32_t lowTime0NanoSeconds, + int32_t highTime1NanoSeconds, + int32_t lowTime1NanoSeconds, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetAddressableLEDSyncTime(HAL_AddressableLEDHandle handle, + int32_t syncTimeMicroSeconds, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_StartAddressableLEDOutput(HAL_AddressableLEDHandle handle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_StopAddressableLEDOutput(HAL_AddressableLEDHandle handle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} +} // extern "C" diff --git a/hal/src/main/native/systemcore/AnalogAccumulator.cpp b/hal/src/main/native/systemcore/AnalogAccumulator.cpp new file mode 100644 index 00000000000..312c6561fc7 --- /dev/null +++ b/hal/src/main/native/systemcore/AnalogAccumulator.cpp @@ -0,0 +1,65 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/AnalogAccumulator.h" + +#include "hal/HAL.h" + +using namespace hal; + +namespace hal::init { +void InitializeAnalogAccumulator() {} +} // namespace hal::init + +extern "C" { + +HAL_Bool HAL_IsAccumulatorChannel(HAL_AnalogInputHandle analogPortHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return false; +} + +void HAL_InitAccumulator(HAL_AnalogInputHandle analogPortHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_ResetAccumulator(HAL_AnalogInputHandle analogPortHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetAccumulatorCenter(HAL_AnalogInputHandle analogPortHandle, + int32_t center, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetAccumulatorDeadband(HAL_AnalogInputHandle analogPortHandle, + int32_t deadband, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +int64_t HAL_GetAccumulatorValue(HAL_AnalogInputHandle analogPortHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +int64_t HAL_GetAccumulatorCount(HAL_AnalogInputHandle analogPortHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +void HAL_GetAccumulatorOutput(HAL_AnalogInputHandle analogPortHandle, + int64_t* value, int64_t* count, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/AnalogGyro.cpp b/hal/src/main/native/systemcore/AnalogGyro.cpp new file mode 100644 index 00000000000..bfcfd42b8bf --- /dev/null +++ b/hal/src/main/native/systemcore/AnalogGyro.cpp @@ -0,0 +1,92 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/AnalogGyro.h" + +#include +#include +#include + +#include + +#include "HALInitializer.h" +#include "HALInternal.h" +#include "hal/AnalogAccumulator.h" +#include "hal/AnalogInput.h" +#include "hal/handles/IndexedHandleResource.h" + +using namespace hal; + +namespace hal::init { +void InitializeAnalogGyro() {} +} // namespace hal::init + +extern "C" { + +HAL_GyroHandle HAL_InitializeAnalogGyro(HAL_AnalogInputHandle analogHandle, + const char* allocationLocation, + int32_t* status) { + hal::init::CheckInit(); + *status = HAL_HANDLE_ERROR; + return HAL_kInvalidHandle; +} + +void HAL_SetupAnalogGyro(HAL_GyroHandle handle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_FreeAnalogGyro(HAL_GyroHandle handle) {} + +void HAL_SetAnalogGyroParameters(HAL_GyroHandle handle, + double voltsPerDegreePerSecond, double offset, + int32_t center, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetAnalogGyroVoltsPerDegreePerSecond(HAL_GyroHandle handle, + double voltsPerDegreePerSecond, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_ResetAnalogGyro(HAL_GyroHandle handle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_CalibrateAnalogGyro(HAL_GyroHandle handle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetAnalogGyroDeadband(HAL_GyroHandle handle, double volts, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +double HAL_GetAnalogGyroAngle(HAL_GyroHandle handle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +double HAL_GetAnalogGyroRate(HAL_GyroHandle handle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +double HAL_GetAnalogGyroOffset(HAL_GyroHandle handle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +int32_t HAL_GetAnalogGyroCenter(HAL_GyroHandle handle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/AnalogInput.cpp b/hal/src/main/native/systemcore/AnalogInput.cpp new file mode 100644 index 00000000000..920cf3fd439 --- /dev/null +++ b/hal/src/main/native/systemcore/AnalogInput.cpp @@ -0,0 +1,127 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/AnalogInput.h" + +#include + +#include "HALInitializer.h" +#include "HALInternal.h" +#include "PortsInternal.h" +#include "hal/AnalogAccumulator.h" +#include "hal/Errors.h" +#include "hal/handles/HandlesInternal.h" + +namespace hal::init { +void InitializeAnalogInput() {} +} // namespace hal::init + +using namespace hal; + +extern "C" { + +HAL_AnalogInputHandle HAL_InitializeAnalogInputPort( + HAL_PortHandle portHandle, const char* allocationLocation, + int32_t* status) { + hal::init::CheckInit(); + *status = HAL_HANDLE_ERROR; + return HAL_kInvalidHandle; +} + +void HAL_FreeAnalogInputPort(HAL_AnalogInputHandle analogPortHandle) {} + +HAL_Bool HAL_CheckAnalogModule(int32_t module) { + return module == 1; +} + +HAL_Bool HAL_CheckAnalogInputChannel(int32_t channel) { + return channel < kNumAnalogInputs && channel >= 0; +} + +void HAL_SetAnalogInputSimDevice(HAL_AnalogInputHandle handle, + HAL_SimDeviceHandle device) {} + +void HAL_SetAnalogSampleRate(double samplesPerSecond, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +double HAL_GetAnalogSampleRate(int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +void HAL_SetAnalogAverageBits(HAL_AnalogInputHandle analogPortHandle, + int32_t bits, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +int32_t HAL_GetAnalogAverageBits(HAL_AnalogInputHandle analogPortHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +void HAL_SetAnalogOversampleBits(HAL_AnalogInputHandle analogPortHandle, + int32_t bits, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +int32_t HAL_GetAnalogOversampleBits(HAL_AnalogInputHandle analogPortHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +int32_t HAL_GetAnalogValue(HAL_AnalogInputHandle analogPortHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +int32_t HAL_GetAnalogAverageValue(HAL_AnalogInputHandle analogPortHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +int32_t HAL_GetAnalogVoltsToValue(HAL_AnalogInputHandle analogPortHandle, + double voltage, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +double HAL_GetAnalogVoltage(HAL_AnalogInputHandle analogPortHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +double HAL_GetAnalogValueToVolts(HAL_AnalogInputHandle analogPortHandle, + int32_t rawValue, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +double HAL_GetAnalogAverageVoltage(HAL_AnalogInputHandle analogPortHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +int32_t HAL_GetAnalogLSBWeight(HAL_AnalogInputHandle analogPortHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +int32_t HAL_GetAnalogOffset(HAL_AnalogInputHandle analogPortHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/AnalogOutput.cpp b/hal/src/main/native/systemcore/AnalogOutput.cpp new file mode 100644 index 00000000000..fa918417833 --- /dev/null +++ b/hal/src/main/native/systemcore/AnalogOutput.cpp @@ -0,0 +1,51 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/AnalogOutput.h" + +#include + +#include "HALInitializer.h" +#include "HALInternal.h" +#include "PortsInternal.h" +#include "hal/Errors.h" +#include "hal/handles/HandlesInternal.h" +#include "hal/handles/IndexedHandleResource.h" + +using namespace hal; + +namespace hal::init { +void InitializeAnalogOutput() {} +} // namespace hal::init + +extern "C" { + +HAL_AnalogOutputHandle HAL_InitializeAnalogOutputPort( + HAL_PortHandle portHandle, const char* allocationLocation, + int32_t* status) { + hal::init::CheckInit(); + + *status = HAL_HANDLE_ERROR; + return HAL_kInvalidHandle; +} + +HAL_Bool HAL_CheckAnalogOutputChannel(int32_t channel) { + return channel < kNumAnalogOutputs && channel >= 0; +} + +void HAL_FreeAnalogOutputPort(HAL_AnalogOutputHandle analogOutputHandle) {} + +void HAL_SetAnalogOutput(HAL_AnalogOutputHandle analogOutputHandle, + double voltage, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +double HAL_GetAnalogOutput(HAL_AnalogOutputHandle analogOutputHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/AnalogTrigger.cpp b/hal/src/main/native/systemcore/AnalogTrigger.cpp new file mode 100644 index 00000000000..73773cf11a8 --- /dev/null +++ b/hal/src/main/native/systemcore/AnalogTrigger.cpp @@ -0,0 +1,100 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/AnalogTrigger.h" + +#include + +#include "HALInitializer.h" +#include "HALInternal.h" +#include "PortsInternal.h" +#include "hal/AnalogInput.h" +#include "hal/DutyCycle.h" +#include "hal/Errors.h" +#include "hal/handles/HandlesInternal.h" +#include "hal/handles/LimitedHandleResource.h" + +using namespace hal; + +namespace hal::init { +void InitializeAnalogTrigger() {} +} // namespace hal::init + +extern "C" { + +HAL_AnalogTriggerHandle HAL_InitializeAnalogTrigger( + HAL_AnalogInputHandle portHandle, int32_t* status) { + hal::init::CheckInit(); + *status = HAL_HANDLE_ERROR; + return HAL_kInvalidHandle; +} + +HAL_AnalogTriggerHandle HAL_InitializeAnalogTriggerDutyCycle( + HAL_DutyCycleHandle dutyCycleHandle, int32_t* status) { + hal::init::CheckInit(); + *status = HAL_HANDLE_ERROR; + return HAL_kInvalidHandle; +} + +void HAL_CleanAnalogTrigger(HAL_AnalogTriggerHandle analogTriggerHandle) {} + +void HAL_SetAnalogTriggerLimitsRaw(HAL_AnalogTriggerHandle analogTriggerHandle, + int32_t lower, int32_t upper, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetAnalogTriggerLimitsDutyCycle( + HAL_AnalogTriggerHandle analogTriggerHandle, double lower, double upper, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetAnalogTriggerLimitsVoltage( + HAL_AnalogTriggerHandle analogTriggerHandle, double lower, double upper, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetAnalogTriggerAveraged(HAL_AnalogTriggerHandle analogTriggerHandle, + HAL_Bool useAveragedValue, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetAnalogTriggerFiltered(HAL_AnalogTriggerHandle analogTriggerHandle, + HAL_Bool useFilteredValue, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +HAL_Bool HAL_GetAnalogTriggerInWindow( + HAL_AnalogTriggerHandle analogTriggerHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return false; +} + +HAL_Bool HAL_GetAnalogTriggerTriggerState( + HAL_AnalogTriggerHandle analogTriggerHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return false; +} + +HAL_Bool HAL_GetAnalogTriggerOutput(HAL_AnalogTriggerHandle analogTriggerHandle, + HAL_AnalogTriggerType type, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return false; +} + +int32_t HAL_GetAnalogTriggerFPGAIndex( + HAL_AnalogTriggerHandle analogTriggerHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/CAN.cpp b/hal/src/main/native/systemcore/CAN.cpp new file mode 100644 index 00000000000..3c822fc8c5c --- /dev/null +++ b/hal/src/main/native/systemcore/CAN.cpp @@ -0,0 +1,47 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/CAN.h" + +#include "hal/Errors.h" + +namespace hal::init { +void InitializeCAN() {} +} // namespace hal::init + +extern "C" { + +void HAL_CAN_SendMessage(uint32_t messageID, const uint8_t* data, + uint8_t dataSize, int32_t periodMs, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} +void HAL_CAN_ReceiveMessage(uint32_t* messageID, uint32_t messageIDMask, + uint8_t* data, uint8_t* dataSize, + uint32_t* timeStamp, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} +void HAL_CAN_OpenStreamSession(uint32_t* sessionHandle, uint32_t messageID, + uint32_t messageIDMask, uint32_t maxMessages, + int32_t* status) { + *sessionHandle = 0; + *status = HAL_HANDLE_ERROR; + return; +} +void HAL_CAN_CloseStreamSession(uint32_t sessionHandle) {} +void HAL_CAN_ReadStreamSession(uint32_t sessionHandle, + struct HAL_CANStreamMessage* messages, + uint32_t messagesToRead, uint32_t* messagesRead, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} +void HAL_CAN_GetCANStatus(float* percentBusUtilization, uint32_t* busOffCount, + uint32_t* txFullCount, uint32_t* receiveErrorCount, + uint32_t* transmitErrorCount, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} +} // extern "C" diff --git a/hal/src/main/native/systemcore/CANAPI.cpp b/hal/src/main/native/systemcore/CANAPI.cpp new file mode 100644 index 00000000000..751b0253f55 --- /dev/null +++ b/hal/src/main/native/systemcore/CANAPI.cpp @@ -0,0 +1,286 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/CANAPI.h" + +#include +#include + +#include +#include + +#include "HALInitializer.h" +#include "hal/CAN.h" +#include "hal/Errors.h" +#include "hal/handles/UnlimitedHandleResource.h" + +using namespace hal; + +namespace { +struct Receives { + uint32_t lastTimeStamp; + uint8_t data[8]; + uint8_t length; +}; + +struct CANStorage { + HAL_CANManufacturer manufacturer; + HAL_CANDeviceType deviceType; + uint8_t deviceId; + wpi::mutex periodicSendsMutex; + wpi::SmallDenseMap periodicSends; + wpi::mutex receivesMutex; + wpi::SmallDenseMap receives; +}; +} // namespace + +static UnlimitedHandleResource* + canHandles; + +namespace hal::init { +void InitializeCANAPI() { + static UnlimitedHandleResource + cH; + canHandles = &cH; +} +} // namespace hal::init + +static int32_t CreateCANId(CANStorage* storage, int32_t apiId) { + int32_t createdId = 0; + createdId |= (static_cast(storage->deviceType) & 0x1F) << 24; + createdId |= (static_cast(storage->manufacturer) & 0xFF) << 16; + createdId |= (apiId & 0x3FF) << 6; + createdId |= (storage->deviceId & 0x3F); + return createdId; +} + +extern "C" { + +uint32_t HAL_GetCANPacketBaseTime(void) { + timespec t; + clock_gettime(CLOCK_MONOTONIC, &t); + + // Convert t to milliseconds + uint64_t ms = t.tv_sec * 1000ull + t.tv_nsec / 1000000ull; + return ms & 0xFFFFFFFF; +} + +HAL_CANHandle HAL_InitializeCAN(HAL_CANManufacturer manufacturer, + int32_t deviceId, HAL_CANDeviceType deviceType, + int32_t* status) { + hal::init::CheckInit(); + auto can = std::make_shared(); + + auto handle = canHandles->Allocate(can); + + if (handle == HAL_kInvalidHandle) { + *status = NO_AVAILABLE_RESOURCES; + return HAL_kInvalidHandle; + } + + can->deviceId = deviceId; + can->deviceType = deviceType; + can->manufacturer = manufacturer; + + return handle; +} + +void HAL_CleanCAN(HAL_CANHandle handle) { + auto data = canHandles->Free(handle); + if (data == nullptr) { + return; + } + + std::scoped_lock lock(data->periodicSendsMutex); + + for (auto&& i : data->periodicSends) { + int32_t s = 0; + auto id = CreateCANId(data.get(), i.first); + HAL_CAN_SendMessage(id, nullptr, 0, HAL_CAN_SEND_PERIOD_STOP_REPEATING, &s); + i.second = -1; + } +} + +void HAL_WriteCANPacket(HAL_CANHandle handle, const uint8_t* data, + int32_t length, int32_t apiId, int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return; + } + auto id = CreateCANId(can.get(), apiId); + + std::scoped_lock lock(can->periodicSendsMutex); + HAL_CAN_SendMessage(id, data, length, HAL_CAN_SEND_PERIOD_NO_REPEAT, status); + can->periodicSends[apiId] = -1; +} + +void HAL_WriteCANPacketRepeating(HAL_CANHandle handle, const uint8_t* data, + int32_t length, int32_t apiId, + int32_t repeatMs, int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return; + } + auto id = CreateCANId(can.get(), apiId); + + std::scoped_lock lock(can->periodicSendsMutex); + HAL_CAN_SendMessage(id, data, length, repeatMs, status); + can->periodicSends[apiId] = repeatMs; +} + +void HAL_WriteCANRTRFrame(HAL_CANHandle handle, int32_t length, int32_t apiId, + int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return; + } + auto id = CreateCANId(can.get(), apiId); + id |= HAL_CAN_IS_FRAME_REMOTE; + uint8_t data[8]; + std::memset(data, 0, sizeof(data)); + + std::scoped_lock lock(can->periodicSendsMutex); + HAL_CAN_SendMessage(id, data, length, HAL_CAN_SEND_PERIOD_NO_REPEAT, status); + can->periodicSends[apiId] = -1; +} + +void HAL_StopCANPacketRepeating(HAL_CANHandle handle, int32_t apiId, + int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return; + } + auto id = CreateCANId(can.get(), apiId); + + std::scoped_lock lock(can->periodicSendsMutex); + HAL_CAN_SendMessage(id, nullptr, 0, HAL_CAN_SEND_PERIOD_STOP_REPEATING, + status); + can->periodicSends[apiId] = -1; +} + +void HAL_ReadCANPacketNew(HAL_CANHandle handle, int32_t apiId, uint8_t* data, + int32_t* length, uint64_t* receivedTimestamp, + int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return; + } + + uint32_t messageId = CreateCANId(can.get(), apiId); + uint8_t dataSize = 0; + uint32_t ts = 0; + HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status); + + if (*status == 0) { + std::scoped_lock lock(can->receivesMutex); + auto& msg = can->receives[messageId]; + msg.length = dataSize; + msg.lastTimeStamp = ts; + // The NetComm call placed in data, copy into the msg + std::memcpy(msg.data, data, dataSize); + } + *length = dataSize; + *receivedTimestamp = ts; +} + +void HAL_ReadCANPacketLatest(HAL_CANHandle handle, int32_t apiId, uint8_t* data, + int32_t* length, uint64_t* receivedTimestamp, + int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return; + } + + uint32_t messageId = CreateCANId(can.get(), apiId); + uint8_t dataSize = 0; + uint32_t ts = 0; + HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status); + + std::scoped_lock lock(can->receivesMutex); + if (*status == 0) { + // fresh update + auto& msg = can->receives[messageId]; + msg.length = dataSize; + *length = dataSize; + msg.lastTimeStamp = ts; + *receivedTimestamp = ts; + // The NetComm call placed in data, copy into the msg + std::memcpy(msg.data, data, dataSize); + } else { + auto i = can->receives.find(messageId); + if (i != can->receives.end()) { + // Read the data from the stored message into the output + std::memcpy(data, i->second.data, i->second.length); + *length = i->second.length; + *receivedTimestamp = i->second.lastTimeStamp; + *status = 0; + } + } +} + +void HAL_ReadCANPacketTimeout(HAL_CANHandle handle, int32_t apiId, + uint8_t* data, int32_t* length, + uint64_t* receivedTimestamp, int32_t timeoutMs, + int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return; + } + + uint32_t messageId = CreateCANId(can.get(), apiId); + uint8_t dataSize = 0; + uint32_t ts = 0; + HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status); + + std::scoped_lock lock(can->receivesMutex); + if (*status == 0) { + // fresh update + auto& msg = can->receives[messageId]; + msg.length = dataSize; + *length = dataSize; + msg.lastTimeStamp = ts; + *receivedTimestamp = ts; + // The NetComm call placed in data, copy into the msg + std::memcpy(msg.data, data, dataSize); + } else { + auto i = can->receives.find(messageId); + if (i != can->receives.end()) { + // Found, check if new enough + uint32_t now = HAL_GetCANPacketBaseTime(); + if (now - i->second.lastTimeStamp > static_cast(timeoutMs)) { + // Timeout, return bad status + *status = HAL_CAN_TIMEOUT; + return; + } + // Read the data from the stored message into the output + std::memcpy(data, i->second.data, i->second.length); + *length = i->second.length; + *receivedTimestamp = i->second.lastTimeStamp; + *status = 0; + } + } +} + +uint32_t HAL_StartCANStream(HAL_CANHandle handle, int32_t apiId, int32_t depth, + int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + uint32_t messageId = CreateCANId(can.get(), apiId); + + uint32_t session = 0; + HAL_CAN_OpenStreamSession(&session, messageId, 0x1FFFFFFF, depth, status); + return session; +} +} // extern "C" diff --git a/hal/src/main/native/systemcore/CTREPCM.cpp b/hal/src/main/native/systemcore/CTREPCM.cpp new file mode 100644 index 00000000000..4c104da5e63 --- /dev/null +++ b/hal/src/main/native/systemcore/CTREPCM.cpp @@ -0,0 +1,407 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/CTREPCM.h" + +#include + +#include + +#include "HALInitializer.h" +#include "HALInternal.h" +#include "PortsInternal.h" +#include "hal/CANAPI.h" +#include "hal/Errors.h" +#include "hal/handles/IndexedHandleResource.h" + +using namespace hal; + +static constexpr HAL_CANManufacturer manufacturer = + HAL_CANManufacturer::HAL_CAN_Man_kCTRE; + +static constexpr HAL_CANDeviceType deviceType = + HAL_CANDeviceType::HAL_CAN_Dev_kPneumatics; + +static constexpr int32_t Status1 = 0x50; +static constexpr int32_t StatusSolFaults = 0x51; +static constexpr int32_t StatusDebug = 0x52; + +static constexpr int32_t Control1 = 0x70; +static constexpr int32_t Control2 = 0x71; +static constexpr int32_t Control3 = 0x72; + +static constexpr int32_t TimeoutMs = 100; +static constexpr int32_t SendPeriod = 20; + +union PcmStatus { + uint8_t data[8]; + struct Bits { + /* Byte 0 */ + unsigned SolenoidBits : 8; + /* Byte 1 */ + unsigned compressorOn : 1; + unsigned stickyFaultFuseTripped : 1; + unsigned stickyFaultCompCurrentTooHigh : 1; + unsigned faultFuseTripped : 1; + unsigned faultCompCurrentTooHigh : 1; + unsigned faultHardwareFailure : 1; + unsigned isCloseloopEnabled : 1; + unsigned pressureSwitchEn : 1; + /* Byte 2*/ + unsigned battVoltage : 8; + /* Byte 3 */ + unsigned solenoidVoltageTop8 : 8; + /* Byte 4 */ + unsigned compressorCurrentTop6 : 6; + unsigned solenoidVoltageBtm2 : 2; + /* Byte 5 */ + unsigned StickyFault_dItooHigh : 1; + unsigned Fault_dItooHigh : 1; + unsigned moduleEnabled : 1; + unsigned closedLoopOutput : 1; + unsigned compressorCurrentBtm4 : 4; + /* Byte 6 */ + unsigned tokenSeedTop8 : 8; + /* Byte 7 */ + unsigned tokenSeedBtm8 : 8; + } bits; +}; + +union PcmControl { + uint8_t data[8]; + struct Bits { + /* Byte 0 */ + unsigned tokenTop8 : 8; + /* Byte 1 */ + unsigned tokenBtm8 : 8; + /* Byte 2 */ + unsigned solenoidBits : 8; + /* Byte 3*/ + unsigned reserved : 4; + unsigned closeLoopOutput : 1; + unsigned compressorOn : 1; + unsigned closedLoopEnable : 1; + unsigned clearStickyFaults : 1; + /* Byte 4 */ + unsigned OneShotField_h8 : 8; + /* Byte 5 */ + unsigned OneShotField_l8 : 8; + } bits; +}; + +struct PcmControlSetOneShotDur { + uint8_t sol10MsPerUnit[8]; +}; + +union PcmStatusFault { + uint8_t data[8]; + struct Bits { + /* Byte 0 */ + unsigned SolenoidDisabledList : 8; + /* Byte 1 */ + unsigned reserved_bit0 : 1; + unsigned reserved_bit1 : 1; + unsigned reserved_bit2 : 1; + unsigned reserved_bit3 : 1; + unsigned StickyFault_CompNoCurrent : 1; + unsigned Fault_CompNoCurrent : 1; + unsigned StickyFault_SolenoidJumper : 1; + unsigned Fault_SolenoidJumper : 1; + } bits; +}; + +union PcmDebug { + uint8_t data[8]; + struct Bits { + unsigned tokFailsTop8 : 8; + unsigned tokFailsBtm8 : 8; + unsigned lastFailedTokTop8 : 8; + unsigned lastFailedTokBtm8 : 8; + unsigned tokSuccessTop8 : 8; + unsigned tokSuccessBtm8 : 8; + } bits; +}; + +namespace { +struct PCM { + HAL_CANHandle canHandle; + wpi::mutex lock; + std::string previousAllocation; + PcmControl control; + PcmControlSetOneShotDur oneShot; +}; +} // namespace + +static IndexedHandleResource* pcmHandles; + +namespace hal::init { +void InitializeCTREPCM() { + static IndexedHandleResource + pH; + pcmHandles = &pH; +} +} // namespace hal::init + +#define READ_PACKET(type, frame, failureValue) \ + auto pcm = pcmHandles->Get(handle); \ + if (pcm == nullptr) { \ + *status = HAL_HANDLE_ERROR; \ + return failureValue; \ + } \ + type pcmStatus; \ + int32_t length = 0; \ + uint64_t receivedTimestamp = 0; \ + HAL_ReadCANPacketTimeout(pcm->canHandle, frame, pcmStatus.data, &length, \ + &receivedTimestamp, TimeoutMs, status); \ + if (*status != 0) { \ + return failureValue; \ + } + +#define READ_STATUS(failureValue) READ_PACKET(PcmStatus, Status1, failureValue) +#define READ_SOL_FAULTS(failureValue) \ + READ_PACKET(PcmStatusFault, StatusSolFaults, failureValue) + +static void SendControl(PCM* pcm, int32_t* status) { + HAL_WriteCANPacketRepeating(pcm->canHandle, pcm->control.data, 8, Control1, + SendPeriod, status); +} + +extern "C" { + +HAL_CTREPCMHandle HAL_InitializeCTREPCM(int32_t module, + const char* allocationLocation, + int32_t* status) { + hal::init::CheckInit(); + + HAL_CTREPCMHandle handle; + auto pcm = pcmHandles->Allocate(module, &handle, status); + + if (*status != 0) { + if (pcm) { + hal::SetLastErrorPreviouslyAllocated(status, "CTRE PCM", module, + pcm->previousAllocation); + } else { + hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for CTRE PCM", 0, + kNumCTREPCMModules - 1, module); + } + return HAL_kInvalidHandle; // failed to allocate. Pass error back. + } + + pcm->canHandle = HAL_InitializeCAN(manufacturer, module, deviceType, status); + if (*status != 0) { + pcmHandles->Free(handle); + return HAL_kInvalidHandle; + } + + std::memset(&pcm->oneShot, 0, sizeof(pcm->oneShot)); + std::memset(&pcm->control, 0, sizeof(pcm->control)); + + pcm->previousAllocation = allocationLocation ? allocationLocation : ""; + + // Enable closed loop control + HAL_SetCTREPCMClosedLoopControl(handle, true, status); + if (*status != 0) { + HAL_FreeCTREPCM(handle); + return HAL_kInvalidHandle; + } + return handle; +} + +void HAL_FreeCTREPCM(HAL_CTREPCMHandle handle) { + auto pcm = pcmHandles->Get(handle); + if (pcm) { + HAL_CleanCAN(pcm->canHandle); + } + pcmHandles->Free(handle); +} + +HAL_Bool HAL_CheckCTREPCMSolenoidChannel(int32_t channel) { + return channel < kNumCTRESolenoidChannels && channel >= 0; +} + +HAL_Bool HAL_GetCTREPCMCompressor(HAL_CTREPCMHandle handle, int32_t* status) { + READ_STATUS(false); + return pcmStatus.bits.compressorOn; +} + +void HAL_SetCTREPCMClosedLoopControl(HAL_CTREPCMHandle handle, HAL_Bool enabled, + int32_t* status) { + auto pcm = pcmHandles->Get(handle); + if (pcm == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + int32_t can_status = 0; + + std::scoped_lock lock{pcm->lock}; + pcm->control.bits.closedLoopEnable = enabled ? 1 : 0; + SendControl(pcm.get(), &can_status); +} + +HAL_Bool HAL_GetCTREPCMClosedLoopControl(HAL_CTREPCMHandle handle, + int32_t* status) { + READ_STATUS(false); + return pcmStatus.bits.isCloseloopEnabled; +} + +HAL_Bool HAL_GetCTREPCMPressureSwitch(HAL_CTREPCMHandle handle, + int32_t* status) { + READ_STATUS(false); + return pcmStatus.bits.pressureSwitchEn; +} + +double HAL_GetCTREPCMCompressorCurrent(HAL_CTREPCMHandle handle, + int32_t* status) { + READ_STATUS(0); + uint32_t result = pcmStatus.bits.compressorCurrentTop6; + result <<= 4; + result |= pcmStatus.bits.compressorCurrentBtm4; + return result * 0.03125; /* 5.5 fixed pt value in Amps */ +} + +HAL_Bool HAL_GetCTREPCMCompressorCurrentTooHighFault(HAL_CTREPCMHandle handle, + int32_t* status) { + READ_STATUS(false); + return pcmStatus.bits.faultCompCurrentTooHigh; +} + +HAL_Bool HAL_GetCTREPCMCompressorCurrentTooHighStickyFault( + HAL_CTREPCMHandle handle, int32_t* status) { + READ_STATUS(false); + return pcmStatus.bits.stickyFaultCompCurrentTooHigh; +} +HAL_Bool HAL_GetCTREPCMCompressorShortedStickyFault(HAL_CTREPCMHandle handle, + int32_t* status) { + READ_STATUS(false); + return pcmStatus.bits.Fault_dItooHigh; +} +HAL_Bool HAL_GetCTREPCMCompressorShortedFault(HAL_CTREPCMHandle handle, + int32_t* status) { + READ_STATUS(false); + return pcmStatus.bits.StickyFault_dItooHigh; +} +HAL_Bool HAL_GetCTREPCMCompressorNotConnectedStickyFault( + HAL_CTREPCMHandle handle, int32_t* status) { + READ_SOL_FAULTS(false); + return pcmStatus.bits.StickyFault_CompNoCurrent; +} +HAL_Bool HAL_GetCTREPCMCompressorNotConnectedFault(HAL_CTREPCMHandle handle, + int32_t* status) { + READ_SOL_FAULTS(false); + return pcmStatus.bits.Fault_CompNoCurrent; +} + +int32_t HAL_GetCTREPCMSolenoids(HAL_CTREPCMHandle handle, int32_t* status) { + READ_STATUS(0); + return pcmStatus.bits.SolenoidBits & 0xFF; +} + +void HAL_SetCTREPCMSolenoids(HAL_CTREPCMHandle handle, int32_t mask, + int32_t values, int32_t* status) { + auto pcm = pcmHandles->Get(handle); + if (pcm == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + uint8_t smallMask = mask & 0xFF; + uint8_t smallValues = + (values & 0xFF) & smallMask; // Enforce only masked values are set + uint8_t invertMask = ~smallMask; + + std::scoped_lock lock{pcm->lock}; + uint8_t existingValue = invertMask & pcm->control.bits.solenoidBits; + pcm->control.bits.solenoidBits = existingValue | smallValues; + SendControl(pcm.get(), status); +} + +int32_t HAL_GetCTREPCMSolenoidDisabledList(HAL_CTREPCMHandle handle, + int32_t* status) { + READ_SOL_FAULTS(0); + return pcmStatus.bits.SolenoidDisabledList; +} + +HAL_Bool HAL_GetCTREPCMSolenoidVoltageStickyFault(HAL_CTREPCMHandle handle, + int32_t* status) { + READ_STATUS(false); + return pcmStatus.bits.stickyFaultFuseTripped; +} + +HAL_Bool HAL_GetCTREPCMSolenoidVoltageFault(HAL_CTREPCMHandle handle, + int32_t* status) { + READ_STATUS(false); + return pcmStatus.bits.faultFuseTripped; +} + +void HAL_ClearAllCTREPCMStickyFaults(HAL_CTREPCMHandle handle, + int32_t* status) { + auto pcm = pcmHandles->Get(handle); + if (pcm == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + uint8_t controlData[] = {0, 0, 0, 0x80}; + HAL_WriteCANPacket(pcm->canHandle, controlData, sizeof(controlData), Control2, + status); +} + +void HAL_FireCTREPCMOneShot(HAL_CTREPCMHandle handle, int32_t index, + int32_t* status) { + if (index > 7 || index < 0) { + *status = PARAMETER_OUT_OF_RANGE; + hal::SetLastError( + status, + fmt::format("Only [0-7] are valid index values. Requested {}", index)); + return; + } + + auto pcm = pcmHandles->Get(handle); + if (pcm == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + std::scoped_lock lock{pcm->lock}; + uint16_t oneShotField = pcm->control.bits.OneShotField_h8; + oneShotField <<= 8; + oneShotField |= pcm->control.bits.OneShotField_l8; + + uint16_t shift = 2 * index; + uint16_t mask = 3; + uint8_t chBits = (oneShotField >> shift) & mask; + chBits = (chBits % 3) + 1; + oneShotField &= ~(mask << shift); + oneShotField |= (chBits << shift); + pcm->control.bits.OneShotField_h8 = oneShotField >> 8; + pcm->control.bits.OneShotField_l8 = oneShotField; + SendControl(pcm.get(), status); +} + +void HAL_SetCTREPCMOneShotDuration(HAL_CTREPCMHandle handle, int32_t index, + int32_t durMs, int32_t* status) { + if (index > 7 || index < 0) { + *status = PARAMETER_OUT_OF_RANGE; + hal::SetLastError( + status, + fmt::format("Only [0-7] are valid index values. Requested {}", index)); + return; + } + + auto pcm = pcmHandles->Get(handle); + if (pcm == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + std::scoped_lock lock{pcm->lock}; + pcm->oneShot.sol10MsPerUnit[index] = (std::min)( + static_cast(durMs) / 10, static_cast(0xFF)); + HAL_WriteCANPacketRepeating(pcm->canHandle, pcm->oneShot.sol10MsPerUnit, 8, + Control3, SendPeriod, status); +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/CTREPDP.cpp b/hal/src/main/native/systemcore/CTREPDP.cpp new file mode 100644 index 00000000000..e9da90c921d --- /dev/null +++ b/hal/src/main/native/systemcore/CTREPDP.cpp @@ -0,0 +1,769 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "CTREPDP.h" + +#include + +#include +#include + +#include "HALInitializer.h" +#include "HALInternal.h" +#include "PortsInternal.h" +#include "hal/CAN.h" +#include "hal/CANAPI.h" +#include "hal/Errors.h" +#include "hal/handles/IndexedHandleResource.h" + +using namespace hal; + +static constexpr HAL_CANManufacturer manufacturer = + HAL_CANManufacturer::HAL_CAN_Man_kCTRE; + +static constexpr HAL_CANDeviceType deviceType = + HAL_CANDeviceType::HAL_CAN_Dev_kPowerDistribution; + +static constexpr int32_t Status1 = 0x50; +static constexpr int32_t Status2 = 0x51; +static constexpr int32_t Status3 = 0x52; +static constexpr int32_t StatusEnergy = 0x5D; + +static constexpr int32_t Control1 = 0x70; + +static constexpr int32_t TimeoutMs = 100; + +/* encoder/decoders */ +union PdpStatus1 { + uint8_t data[8]; + struct Bits { + unsigned chan1_h8 : 8; + unsigned chan2_h6 : 6; + unsigned chan1_l2 : 2; + unsigned chan3_h4 : 4; + unsigned chan2_l4 : 4; + unsigned chan4_h2 : 2; + unsigned chan3_l6 : 6; + unsigned chan4_l8 : 8; + unsigned chan5_h8 : 8; + unsigned chan6_h6 : 6; + unsigned chan5_l2 : 2; + unsigned reserved4 : 4; + unsigned chan6_l4 : 4; + } bits; +}; + +union PdpStatus2 { + uint8_t data[8]; + struct Bits { + unsigned chan7_h8 : 8; + unsigned chan8_h6 : 6; + unsigned chan7_l2 : 2; + unsigned chan9_h4 : 4; + unsigned chan8_l4 : 4; + unsigned chan10_h2 : 2; + unsigned chan9_l6 : 6; + unsigned chan10_l8 : 8; + unsigned chan11_h8 : 8; + unsigned chan12_h6 : 6; + unsigned chan11_l2 : 2; + unsigned reserved4 : 4; + unsigned chan12_l4 : 4; + } bits; +}; + +union PdpStatus3 { + uint8_t data[8]; + struct Bits { + unsigned chan13_h8 : 8; + unsigned chan14_h6 : 6; + unsigned chan13_l2 : 2; + unsigned chan15_h4 : 4; + unsigned chan14_l4 : 4; + unsigned chan16_h2 : 2; + unsigned chan15_l6 : 6; + unsigned chan16_l8 : 8; + unsigned internalResBattery_mOhms : 8; + unsigned busVoltage : 8; + unsigned temp : 8; + } bits; +}; + +union PdpStatusEnergy { + uint8_t data[8]; + struct Bits { + unsigned TmeasMs_likelywillbe20ms_ : 8; + unsigned TotalCurrent_125mAperunit_h8 : 8; + unsigned Power_125mWperunit_h4 : 4; + unsigned TotalCurrent_125mAperunit_l4 : 4; + unsigned Power_125mWperunit_m8 : 8; + unsigned Energy_125mWPerUnitXTmeas_h4 : 4; + unsigned Power_125mWperunit_l4 : 4; + unsigned Energy_125mWPerUnitXTmeas_mh8 : 8; + unsigned Energy_125mWPerUnitXTmeas_ml8 : 8; + unsigned Energy_125mWPerUnitXTmeas_l8 : 8; + } bits; +}; + +namespace { +struct PDP { + HAL_CANHandle canHandle; + std::string previousAllocation; + bool streamHandleAllocated{false}; + uint32_t streamSessionHandles[3]; +}; +} // namespace + +static IndexedHandleResource* pdpHandles; + +namespace hal::init { +void InitializeCTREPDP() { + static IndexedHandleResource + pH; + pdpHandles = &pH; +} +} // namespace hal::init + +extern "C" { + +HAL_PDPHandle HAL_InitializePDP(int32_t module, const char* allocationLocation, + int32_t* status) { + hal::init::CheckInit(); + if (!HAL_CheckPDPModule(module)) { + *status = RESOURCE_OUT_OF_RANGE; + hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for CTRE PDP", 0, + kNumCTREPDPModules - 1, module); + return HAL_kInvalidHandle; + } + + HAL_PDPHandle handle; + auto pdp = pdpHandles->Allocate(module, &handle, status); + + if (*status != 0) { + if (pdp) { + hal::SetLastErrorPreviouslyAllocated(status, "CTRE PDP", module, + pdp->previousAllocation); + } else { + hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for CTRE PDP", 0, + kNumCTREPDPModules - 1, module); + } + return HAL_kInvalidHandle; // failed to allocate. Pass error back. + } + + pdp->canHandle = HAL_InitializeCAN(manufacturer, module, deviceType, status); + if (*status != 0) { + pdpHandles->Free(handle); + return HAL_kInvalidHandle; + } + + pdp->previousAllocation = allocationLocation ? allocationLocation : ""; + + return handle; +} + +void HAL_CleanPDP(HAL_PDPHandle handle) { + auto pdp = pdpHandles->Get(handle); + if (pdp) { + HAL_CleanCAN(pdp->canHandle); + } + pdpHandles->Free(handle); +} + +int32_t HAL_GetPDPModuleNumber(HAL_PDPHandle handle, int32_t* status) { + return hal::getHandleIndex(handle); +} + +HAL_Bool HAL_CheckPDPModule(int32_t module) { + return module < kNumCTREPDPModules && module >= 0; +} + +HAL_Bool HAL_CheckPDPChannel(int32_t channel) { + return channel < kNumCTREPDPChannels && channel >= 0; +} + +double HAL_GetPDPTemperature(HAL_PDPHandle handle, int32_t* status) { + auto pdp = pdpHandles->Get(handle); + if (pdp == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + PdpStatus3 pdpStatus; + int32_t length = 0; + uint64_t receivedTimestamp = 0; + + HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, pdpStatus.data, &length, + &receivedTimestamp, TimeoutMs, status); + + if (*status != 0) { + return 0; + } else { + return pdpStatus.bits.temp * 1.03250836957542 - 67.8564500484966; + } +} + +double HAL_GetPDPVoltage(HAL_PDPHandle handle, int32_t* status) { + auto pdp = pdpHandles->Get(handle); + if (pdp == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + PdpStatus3 pdpStatus; + int32_t length = 0; + uint64_t receivedTimestamp = 0; + + HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, pdpStatus.data, &length, + &receivedTimestamp, TimeoutMs, status); + + if (*status != 0) { + return 0; + } else { + return pdpStatus.bits.busVoltage * 0.05 + 4.0; /* 50mV per unit plus 4V. */ + } +} + +double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel, + int32_t* status) { + if (!HAL_CheckPDPChannel(channel)) { + *status = PARAMETER_OUT_OF_RANGE; + hal::SetLastError(status, fmt::format("Invalid pdp channel {}", channel)); + return 0; + } + + auto pdp = pdpHandles->Get(handle); + if (pdp == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + int32_t length = 0; + uint64_t receivedTimestamp = 0; + + double raw = 0; + + if (channel <= 5) { + PdpStatus1 pdpStatus; + HAL_ReadCANPacketTimeout(pdp->canHandle, Status1, pdpStatus.data, &length, + &receivedTimestamp, TimeoutMs, status); + if (*status != 0) { + return 0; + } + switch (channel) { + case 0: + raw = (static_cast(pdpStatus.bits.chan1_h8) << 2) | + pdpStatus.bits.chan1_l2; + break; + case 1: + raw = (static_cast(pdpStatus.bits.chan2_h6) << 4) | + pdpStatus.bits.chan2_l4; + break; + case 2: + raw = (static_cast(pdpStatus.bits.chan3_h4) << 6) | + pdpStatus.bits.chan3_l6; + break; + case 3: + raw = (static_cast(pdpStatus.bits.chan4_h2) << 8) | + pdpStatus.bits.chan4_l8; + break; + case 4: + raw = (static_cast(pdpStatus.bits.chan5_h8) << 2) | + pdpStatus.bits.chan5_l2; + break; + case 5: + raw = (static_cast(pdpStatus.bits.chan6_h6) << 4) | + pdpStatus.bits.chan6_l4; + break; + } + } else if (channel <= 11) { + PdpStatus2 pdpStatus; + HAL_ReadCANPacketTimeout(pdp->canHandle, Status2, pdpStatus.data, &length, + &receivedTimestamp, TimeoutMs, status); + if (*status != 0) { + return 0; + } + switch (channel) { + case 6: + raw = (static_cast(pdpStatus.bits.chan7_h8) << 2) | + pdpStatus.bits.chan7_l2; + break; + case 7: + raw = (static_cast(pdpStatus.bits.chan8_h6) << 4) | + pdpStatus.bits.chan8_l4; + break; + case 8: + raw = (static_cast(pdpStatus.bits.chan9_h4) << 6) | + pdpStatus.bits.chan9_l6; + break; + case 9: + raw = (static_cast(pdpStatus.bits.chan10_h2) << 8) | + pdpStatus.bits.chan10_l8; + break; + case 10: + raw = (static_cast(pdpStatus.bits.chan11_h8) << 2) | + pdpStatus.bits.chan11_l2; + break; + case 11: + raw = (static_cast(pdpStatus.bits.chan12_h6) << 4) | + pdpStatus.bits.chan12_l4; + break; + } + } else { + PdpStatus3 pdpStatus; + HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, pdpStatus.data, &length, + &receivedTimestamp, TimeoutMs, status); + if (*status != 0) { + return 0; + } + switch (channel) { + case 12: + raw = (static_cast(pdpStatus.bits.chan13_h8) << 2) | + pdpStatus.bits.chan13_l2; + break; + case 13: + raw = (static_cast(pdpStatus.bits.chan14_h6) << 4) | + pdpStatus.bits.chan14_l4; + break; + case 14: + raw = (static_cast(pdpStatus.bits.chan15_h4) << 6) | + pdpStatus.bits.chan15_l6; + break; + case 15: + raw = (static_cast(pdpStatus.bits.chan16_h2) << 8) | + pdpStatus.bits.chan16_l8; + break; + } + } + + /* convert to amps */ + return raw * 0.125; /* 7.3 fixed pt value in Amps */ +} + +void HAL_GetPDPAllChannelCurrents(HAL_PDPHandle handle, double* currents, + int32_t* status) { + auto pdp = pdpHandles->Get(handle); + if (pdp == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + int32_t length = 0; + uint64_t receivedTimestamp = 0; + PdpStatus1 pdpStatus; + HAL_ReadCANPacketTimeout(pdp->canHandle, Status1, pdpStatus.data, &length, + &receivedTimestamp, TimeoutMs, status); + if (*status != 0) { + return; + } + PdpStatus2 pdpStatus2; + HAL_ReadCANPacketTimeout(pdp->canHandle, Status2, pdpStatus2.data, &length, + &receivedTimestamp, TimeoutMs, status); + if (*status != 0) { + return; + } + PdpStatus3 pdpStatus3; + HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, pdpStatus3.data, &length, + &receivedTimestamp, TimeoutMs, status); + if (*status != 0) { + return; + } + + currents[0] = ((static_cast(pdpStatus.bits.chan1_h8) << 2) | + pdpStatus.bits.chan1_l2) * + 0.125; + currents[1] = ((static_cast(pdpStatus.bits.chan2_h6) << 4) | + pdpStatus.bits.chan2_l4) * + 0.125; + currents[2] = ((static_cast(pdpStatus.bits.chan3_h4) << 6) | + pdpStatus.bits.chan3_l6) * + 0.125; + currents[3] = ((static_cast(pdpStatus.bits.chan4_h2) << 8) | + pdpStatus.bits.chan4_l8) * + 0.125; + currents[4] = ((static_cast(pdpStatus.bits.chan5_h8) << 2) | + pdpStatus.bits.chan5_l2) * + 0.125; + currents[5] = ((static_cast(pdpStatus.bits.chan6_h6) << 4) | + pdpStatus.bits.chan6_l4) * + 0.125; + + currents[6] = ((static_cast(pdpStatus2.bits.chan7_h8) << 2) | + pdpStatus2.bits.chan7_l2) * + 0.125; + currents[7] = ((static_cast(pdpStatus2.bits.chan8_h6) << 4) | + pdpStatus2.bits.chan8_l4) * + 0.125; + currents[8] = ((static_cast(pdpStatus2.bits.chan9_h4) << 6) | + pdpStatus2.bits.chan9_l6) * + 0.125; + currents[9] = ((static_cast(pdpStatus2.bits.chan10_h2) << 8) | + pdpStatus2.bits.chan10_l8) * + 0.125; + currents[10] = ((static_cast(pdpStatus2.bits.chan11_h8) << 2) | + pdpStatus2.bits.chan11_l2) * + 0.125; + currents[11] = ((static_cast(pdpStatus2.bits.chan12_h6) << 4) | + pdpStatus2.bits.chan12_l4) * + 0.125; + + currents[12] = ((static_cast(pdpStatus3.bits.chan13_h8) << 2) | + pdpStatus3.bits.chan13_l2) * + 0.125; + currents[13] = ((static_cast(pdpStatus3.bits.chan14_h6) << 4) | + pdpStatus3.bits.chan14_l4) * + 0.125; + currents[14] = ((static_cast(pdpStatus3.bits.chan15_h4) << 6) | + pdpStatus3.bits.chan15_l6) * + 0.125; + currents[15] = ((static_cast(pdpStatus3.bits.chan16_h2) << 8) | + pdpStatus3.bits.chan16_l8) * + 0.125; +} + +double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status) { + auto pdp = pdpHandles->Get(handle); + if (pdp == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + PdpStatusEnergy pdpStatus; + int32_t length = 0; + uint64_t receivedTimestamp = 0; + + HAL_ReadCANPacketTimeout(pdp->canHandle, StatusEnergy, pdpStatus.data, + &length, &receivedTimestamp, TimeoutMs, status); + if (*status != 0) { + return 0; + } + + uint32_t raw; + raw = pdpStatus.bits.TotalCurrent_125mAperunit_h8; + raw <<= 4; + raw |= pdpStatus.bits.TotalCurrent_125mAperunit_l4; + return 0.125 * raw; /* 7.3 fixed pt value in Amps */ +} + +double HAL_GetPDPTotalPower(HAL_PDPHandle handle, int32_t* status) { + auto pdp = pdpHandles->Get(handle); + if (pdp == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + PdpStatusEnergy pdpStatus; + int32_t length = 0; + uint64_t receivedTimestamp = 0; + + HAL_ReadCANPacketTimeout(pdp->canHandle, StatusEnergy, pdpStatus.data, + &length, &receivedTimestamp, TimeoutMs, status); + if (*status != 0) { + return 0; + } + + uint32_t raw; + raw = pdpStatus.bits.Power_125mWperunit_h4; + raw <<= 8; + raw |= pdpStatus.bits.Power_125mWperunit_m8; + raw <<= 4; + raw |= pdpStatus.bits.Power_125mWperunit_l4; + return 0.125 * raw; /* 7.3 fixed pt value in Watts */ +} + +double HAL_GetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) { + auto pdp = pdpHandles->Get(handle); + if (pdp == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + PdpStatusEnergy pdpStatus; + int32_t length = 0; + uint64_t receivedTimestamp = 0; + + HAL_ReadCANPacketTimeout(pdp->canHandle, StatusEnergy, pdpStatus.data, + &length, &receivedTimestamp, TimeoutMs, status); + if (*status != 0) { + return 0; + } + + uint32_t raw; + raw = pdpStatus.bits.Energy_125mWPerUnitXTmeas_h4; + raw <<= 8; + raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_mh8; + raw <<= 8; + raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_ml8; + raw <<= 8; + raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_l8; + + double energyJoules = 0.125 * raw; /* mW integrated every TmeasMs */ + energyJoules *= 0.001; /* convert from mW to W */ + energyJoules *= + pdpStatus.bits + .TmeasMs_likelywillbe20ms_; /* multiplied by TmeasMs = joules */ + return energyJoules; +} + +void HAL_ResetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) { + auto pdp = pdpHandles->Get(handle); + if (pdp == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + uint8_t pdpControl[] = {0x40}; /* only bit set is ResetEnergy */ + HAL_WriteCANPacket(pdp->canHandle, pdpControl, 1, Control1, status); +} + +void HAL_ClearPDPStickyFaults(HAL_PDPHandle handle, int32_t* status) { + auto pdp = pdpHandles->Get(handle); + if (pdp == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + uint8_t pdpControl[] = {0x80}; /* only bit set is ClearStickyFaults */ + HAL_WriteCANPacket(pdp->canHandle, pdpControl, 1, Control1, status); +} + +uint32_t HAL_StartCANStream(HAL_CANHandle handle, int32_t apiId, int32_t depth, + int32_t* status); + +void HAL_StartPDPStream(HAL_PDPHandle handle, int32_t* status) { + auto pdp = pdpHandles->Get(handle); + if (pdp == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + if (pdp->streamHandleAllocated) { + *status = RESOURCE_IS_ALLOCATED; + return; + } + + pdp->streamSessionHandles[0] = + HAL_StartCANStream(pdp->canHandle, Status1, 50, status); + if (*status != 0) { + return; + } + pdp->streamSessionHandles[1] = + HAL_StartCANStream(pdp->canHandle, Status2, 50, status); + if (*status != 0) { + HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[0]); + return; + } + pdp->streamSessionHandles[2] = + HAL_StartCANStream(pdp->canHandle, Status3, 50, status); + if (*status != 0) { + HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[0]); + HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[1]); + return; + } + pdp->streamHandleAllocated = true; +} + +HAL_PowerDistributionChannelData* HAL_GetPDPStreamData(HAL_PDPHandle handle, + int32_t* count, + int32_t* status) { + auto pdp = pdpHandles->Get(handle); + if (pdp == nullptr) { + *status = HAL_HANDLE_ERROR; + return nullptr; + } + + if (!pdp->streamHandleAllocated) { + *status = RESOURCE_OUT_OF_RANGE; + return nullptr; + } + + *count = 0; + // 3 streams, 6 channels per stream, 50 depth per stream + HAL_PowerDistributionChannelData* retData = + new HAL_PowerDistributionChannelData[3 * 6 * 50]; + + HAL_CANStreamMessage messages[50]; + uint32_t messagesRead = 0; + HAL_CAN_ReadStreamSession(pdp->streamSessionHandles[0], messages, 50, + &messagesRead, status); + if (*status < 0) { + goto Exit; + } + + for (uint32_t i = 0; i < messagesRead; i++) { + PdpStatus1 pdpStatus; + std::memcpy(pdpStatus.data, messages[i].data, sizeof(messages[i].data)); + uint32_t timestamp = messages[i].timeStamp; + + retData[*count].current = + ((static_cast(pdpStatus.bits.chan1_h8) << 2) | + pdpStatus.bits.chan1_l2) * + 0.125; + retData[*count].channel = 1; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + ((static_cast(pdpStatus.bits.chan2_h6) << 4) | + pdpStatus.bits.chan2_l4) * + 0.125; + retData[*count].channel = 2; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + ((static_cast(pdpStatus.bits.chan3_h4) << 6) | + pdpStatus.bits.chan3_l6) * + 0.125; + retData[*count].channel = 3; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + ((static_cast(pdpStatus.bits.chan4_h2) << 8) | + pdpStatus.bits.chan4_l8) * + 0.125; + retData[*count].channel = 4; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + ((static_cast(pdpStatus.bits.chan5_h8) << 2) | + pdpStatus.bits.chan5_l2) * + 0.125; + retData[*count].channel = 5; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + ((static_cast(pdpStatus.bits.chan6_h6) << 4) | + pdpStatus.bits.chan6_l4) * + 0.125; + retData[*count].channel = 6; + retData[*count].timestamp = timestamp; + (*count)++; + } + + messagesRead = 0; + HAL_CAN_ReadStreamSession(pdp->streamSessionHandles[1], messages, 50, + &messagesRead, status); + if (*status < 0) { + goto Exit; + } + + for (uint32_t i = 0; i < messagesRead; i++) { + PdpStatus2 pdpStatus; + std::memcpy(pdpStatus.data, messages[i].data, sizeof(messages[i].data)); + uint32_t timestamp = messages[i].timeStamp; + + retData[*count].current = + ((static_cast(pdpStatus.bits.chan7_h8) << 2) | + pdpStatus.bits.chan7_l2) * + 0.125; + retData[*count].channel = 7; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + ((static_cast(pdpStatus.bits.chan8_h6) << 4) | + pdpStatus.bits.chan8_l4) * + 0.125; + retData[*count].channel = 8; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + ((static_cast(pdpStatus.bits.chan9_h4) << 6) | + pdpStatus.bits.chan9_l6) * + 0.125; + retData[*count].channel = 9; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + ((static_cast(pdpStatus.bits.chan10_h2) << 8) | + pdpStatus.bits.chan10_l8) * + 0.125; + retData[*count].channel = 10; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + ((static_cast(pdpStatus.bits.chan11_h8) << 2) | + pdpStatus.bits.chan11_l2) * + 0.125; + retData[*count].channel = 11; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + ((static_cast(pdpStatus.bits.chan12_h6) << 4) | + pdpStatus.bits.chan12_l4) * + 0.125; + retData[*count].channel = 12; + retData[*count].timestamp = timestamp; + (*count)++; + } + + messagesRead = 0; + HAL_CAN_ReadStreamSession(pdp->streamSessionHandles[2], messages, 50, + &messagesRead, status); + if (*status < 0) { + goto Exit; + } + + for (uint32_t i = 0; i < messagesRead; i++) { + PdpStatus3 pdpStatus; + std::memcpy(pdpStatus.data, messages[i].data, sizeof(messages[i].data)); + uint32_t timestamp = messages[i].timeStamp; + + retData[*count].current = + ((static_cast(pdpStatus.bits.chan13_h8) << 2) | + pdpStatus.bits.chan13_l2) * + 0.125; + retData[*count].channel = 13; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + ((static_cast(pdpStatus.bits.chan14_h6) << 4) | + pdpStatus.bits.chan14_l4) * + 0.125; + retData[*count].channel = 14; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + ((static_cast(pdpStatus.bits.chan15_h4) << 6) | + pdpStatus.bits.chan15_l6) * + 0.125; + retData[*count].channel = 15; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + ((static_cast(pdpStatus.bits.chan16_h2) << 8) | + pdpStatus.bits.chan16_l8) * + 0.125; + retData[*count].channel = 16; + retData[*count].timestamp = timestamp; + (*count)++; + } + +Exit: + if (*status < 0) { + delete[] retData; + retData = nullptr; + } + return retData; +} + +void HAL_StopPDPStream(HAL_PDPHandle handle, int32_t* status) { + auto pdp = pdpHandles->Get(handle); + if (pdp == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + if (!pdp->streamHandleAllocated) { + *status = RESOURCE_OUT_OF_RANGE; + return; + } + + HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[0]); + HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[1]); + HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[2]); + + pdp->streamHandleAllocated = false; +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/CTREPDP.h b/hal/src/main/native/systemcore/CTREPDP.h new file mode 100644 index 00000000000..99606e05754 --- /dev/null +++ b/hal/src/main/native/systemcore/CTREPDP.h @@ -0,0 +1,146 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "hal/PowerDistribution.h" +#include "hal/Types.h" + +/** + * @defgroup hal_pdp PDP Functions + * @ingroup hal_capi + * Functions to control the Power Distribution Panel. + * @{ + */ + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Initializes a Power Distribution Panel. + * + * @param module the module number to initialize + * @return the created PDP + */ +HAL_PDPHandle HAL_InitializePDP(int32_t module, const char* allocationLocation, + int32_t* status); + +/** + * Cleans a PDP module. + * + * @param handle the module handle + */ +void HAL_CleanPDP(HAL_PDPHandle handle); + +/** + * Gets the module number for a pdp. + */ +int32_t HAL_GetPDPModuleNumber(HAL_PDPHandle handle, int32_t* status); + +/** + * Checks if a PDP channel is valid. + * + * @param channel the channel to check + * @return true if the channel is valid, otherwise false + */ +HAL_Bool HAL_CheckPDPChannel(int32_t channel); + +/** + * Checks if a PDP module is valid. + * + * @param channel the module to check + * @return true if the module is valid, otherwise false + */ +HAL_Bool HAL_CheckPDPModule(int32_t module); + +/** + * Gets the temperature of the PDP. + * + * @param handle the module handle + * @return the module temperature (celsius) + */ +double HAL_GetPDPTemperature(HAL_PDPHandle handle, int32_t* status); + +/** + * Gets the PDP input voltage. + * + * @param handle the module handle + * @return the input voltage (volts) + */ +double HAL_GetPDPVoltage(HAL_PDPHandle handle, int32_t* status); + +/** + * Gets the current of a specific PDP channel. + * + * @param module the module + * @param channel the channel + * @return the channel current (amps) + */ +double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel, + int32_t* status); + +/** + * Gets the current of all 16 channels on the PDP. + * + * The array must be large enough to hold all channels. + * + * @param handle the module handle + * @param current the currents (output) + */ +void HAL_GetPDPAllChannelCurrents(HAL_PDPHandle handle, double* currents, + int32_t* status); + +/** + * Gets the total current of the PDP. + * + * @param handle the module handle + * @return the total current (amps) + */ +double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status); + +/** + * Gets the total power of the PDP. + * + * @param handle the module handle + * @return the total power (watts) + */ +double HAL_GetPDPTotalPower(HAL_PDPHandle handle, int32_t* status); + +/** + * Gets the total energy of the PDP. + * + * @param handle the module handle + * @return the total energy (joules) + */ +double HAL_GetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status); + +/** + * Resets the PDP accumulated energy. + * + * @param handle the module handle + */ +void HAL_ResetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status); + +/** + * Clears any PDP sticky faults. + * + * @param handle the module handle + */ +void HAL_ClearPDPStickyFaults(HAL_PDPHandle handle, int32_t* status); + +void HAL_StartPDPStream(HAL_PDPHandle handle, int32_t* status); + +HAL_PowerDistributionChannelData* HAL_GetPDPStreamData(HAL_PDPHandle handle, + int32_t* count, + int32_t* status); + +void HAL_StopPDPStream(HAL_PDPHandle handle, int32_t* status); + +#ifdef __cplusplus +} // extern "C" +#endif +/** @} */ diff --git a/hal/src/main/native/systemcore/Constants.cpp b/hal/src/main/native/systemcore/Constants.cpp new file mode 100644 index 00000000000..6c1361ed612 --- /dev/null +++ b/hal/src/main/native/systemcore/Constants.cpp @@ -0,0 +1,19 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/Constants.h" + +#include "ConstantsInternal.h" + +using namespace hal; + +namespace hal::init { +void InitializeConstants() {} +} // namespace hal::init + +extern "C" { +int32_t HAL_GetSystemClockTicksPerMicrosecond(void) { + return kSystemClockTicksPerMicrosecond; +} +} // extern "C" diff --git a/hal/src/main/native/systemcore/ConstantsInternal.h b/hal/src/main/native/systemcore/ConstantsInternal.h new file mode 100644 index 00000000000..f4af049d4f1 --- /dev/null +++ b/hal/src/main/native/systemcore/ConstantsInternal.h @@ -0,0 +1,11 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +namespace hal { +constexpr int32_t kSystemClockTicksPerMicrosecond = 40; +} // namespace hal diff --git a/hal/src/main/native/systemcore/Counter.cpp b/hal/src/main/native/systemcore/Counter.cpp new file mode 100644 index 00000000000..5645bda8c1b --- /dev/null +++ b/hal/src/main/native/systemcore/Counter.cpp @@ -0,0 +1,165 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/Counter.h" + +#include +#include + +#include + +#include "HALInitializer.h" +#include "HALInternal.h" +#include "PortsInternal.h" +#include "hal/HAL.h" +#include "hal/handles/LimitedHandleResource.h" + +using namespace hal; + +namespace hal::init { +void InitializeCounter() {} +} // namespace hal::init + +extern "C" { + +HAL_CounterHandle HAL_InitializeCounter(HAL_Counter_Mode mode, int32_t* index, + int32_t* status) { + hal::init::CheckInit(); + *status = HAL_HANDLE_ERROR; + return HAL_kInvalidHandle; +} + +void HAL_FreeCounter(HAL_CounterHandle counterHandle) {} + +void HAL_SetCounterAverageSize(HAL_CounterHandle counterHandle, int32_t size, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetCounterUpSource(HAL_CounterHandle counterHandle, + HAL_Handle digitalSourceHandle, + HAL_AnalogTriggerType analogTriggerType, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetCounterUpSourceEdge(HAL_CounterHandle counterHandle, + HAL_Bool risingEdge, HAL_Bool fallingEdge, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_ClearCounterUpSource(HAL_CounterHandle counterHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetCounterDownSource(HAL_CounterHandle counterHandle, + HAL_Handle digitalSourceHandle, + HAL_AnalogTriggerType analogTriggerType, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetCounterDownSourceEdge(HAL_CounterHandle counterHandle, + HAL_Bool risingEdge, HAL_Bool fallingEdge, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_ClearCounterDownSource(HAL_CounterHandle counterHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetCounterUpDownMode(HAL_CounterHandle counterHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetCounterExternalDirectionMode(HAL_CounterHandle counterHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetCounterSemiPeriodMode(HAL_CounterHandle counterHandle, + HAL_Bool highSemiPeriod, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetCounterPulseLengthMode(HAL_CounterHandle counterHandle, + double threshold, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +int32_t HAL_GetCounterSamplesToAverage(HAL_CounterHandle counterHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +void HAL_SetCounterSamplesToAverage(HAL_CounterHandle counterHandle, + int32_t samplesToAverage, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_ResetCounter(HAL_CounterHandle counterHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +int32_t HAL_GetCounter(HAL_CounterHandle counterHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +double HAL_GetCounterPeriod(HAL_CounterHandle counterHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +void HAL_SetCounterMaxPeriod(HAL_CounterHandle counterHandle, double maxPeriod, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetCounterUpdateWhenEmpty(HAL_CounterHandle counterHandle, + HAL_Bool enabled, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +HAL_Bool HAL_GetCounterStopped(HAL_CounterHandle counterHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return false; +} + +HAL_Bool HAL_GetCounterDirection(HAL_CounterHandle counterHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return false; +} + +void HAL_SetCounterReverseDirection(HAL_CounterHandle counterHandle, + HAL_Bool reverseDirection, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/DIO.cpp b/hal/src/main/native/systemcore/DIO.cpp new file mode 100644 index 00000000000..4360f9788a9 --- /dev/null +++ b/hal/src/main/native/systemcore/DIO.cpp @@ -0,0 +1,140 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/DIO.h" + +#include +#include +#include + +#include "HALInitializer.h" +#include "HALInternal.h" +#include "PortsInternal.h" +#include "hal/Errors.h" +#include "hal/cpp/fpga_clock.h" +#include "hal/handles/HandlesInternal.h" +#include "hal/handles/LimitedHandleResource.h" + +using namespace hal; + +namespace hal::init { +void InitializeDIO() {} +} // namespace hal::init + +extern "C" { + +HAL_DigitalHandle HAL_InitializeDIOPort(HAL_PortHandle portHandle, + HAL_Bool input, + const char* allocationLocation, + int32_t* status) { + hal::init::CheckInit(); + *status = HAL_HANDLE_ERROR; + return HAL_kInvalidHandle; +} + +HAL_Bool HAL_CheckDIOChannel(int32_t channel) { + return channel < kNumDigitalChannels && channel >= 0; +} + +void HAL_FreeDIOPort(HAL_DigitalHandle dioPortHandle) {} + +void HAL_SetDIOSimDevice(HAL_DigitalHandle handle, HAL_SimDeviceHandle device) { +} + +HAL_DigitalPWMHandle HAL_AllocateDigitalPWM(int32_t* status) { + *status = HAL_HANDLE_ERROR; + return HAL_kInvalidHandle; +} + +void HAL_FreeDigitalPWM(HAL_DigitalPWMHandle pwmGenerator) {} + +void HAL_SetDigitalPWMRate(double rate, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetDigitalPWMDutyCycle(HAL_DigitalPWMHandle pwmGenerator, + double dutyCycle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetDigitalPWMPPS(HAL_DigitalPWMHandle pwmGenerator, double dutyCycle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetDigitalPWMOutputChannel(HAL_DigitalPWMHandle pwmGenerator, + int32_t channel, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetDIO(HAL_DigitalHandle dioPortHandle, HAL_Bool value, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetDIODirection(HAL_DigitalHandle dioPortHandle, HAL_Bool input, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +HAL_Bool HAL_GetDIO(HAL_DigitalHandle dioPortHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return false; +} + +HAL_Bool HAL_GetDIODirection(HAL_DigitalHandle dioPortHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return false; +} + +void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLengthSeconds, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_PulseMultiple(uint32_t channelMask, double pulseLengthSeconds, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +HAL_Bool HAL_IsPulsing(HAL_DigitalHandle dioPortHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return false; +} + +HAL_Bool HAL_IsAnyPulsing(int32_t* status) { + *status = HAL_HANDLE_ERROR; + return false; +} + +void HAL_SetFilterSelect(HAL_DigitalHandle dioPortHandle, int32_t filterIndex, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +int32_t HAL_GetFilterSelect(HAL_DigitalHandle dioPortHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +void HAL_SetFilterPeriod(int32_t filterIndex, int64_t value, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +int64_t HAL_GetFilterPeriod(int32_t filterIndex, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/DMA.cpp b/hal/src/main/native/systemcore/DMA.cpp new file mode 100644 index 00000000000..b470ea70b4c --- /dev/null +++ b/hal/src/main/native/systemcore/DMA.cpp @@ -0,0 +1,232 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/DMA.h" + +#include +#include +#include +#include +#include + +#include + +#include "PortsInternal.h" +#include "hal/AnalogAccumulator.h" +#include "hal/AnalogGyro.h" +#include "hal/AnalogInput.h" +#include "hal/Errors.h" +#include "hal/HALBase.h" +#include "hal/handles/HandlesInternal.h" +#include "hal/handles/LimitedHandleResource.h" +#include "hal/handles/UnlimitedHandleResource.h" + +using namespace hal; + +static_assert(std::is_standard_layout_v, + "HAL_DMASample must have standard layout"); + +namespace hal::init { +void InitializeDMA() {} +} // namespace hal::init + +extern "C" { + +HAL_DMAHandle HAL_InitializeDMA(int32_t* status) { + *status = HAL_HANDLE_ERROR; + return HAL_kInvalidHandle; +} + +void HAL_FreeDMA(HAL_DMAHandle handle) {} + +void HAL_SetDMAPause(HAL_DMAHandle handle, HAL_Bool pause, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetDMATimedTrigger(HAL_DMAHandle handle, double seconds, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetDMATimedTriggerCycles(HAL_DMAHandle handle, uint32_t cycles, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_AddDMAEncoder(HAL_DMAHandle handle, HAL_EncoderHandle encoderHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_AddDMAEncoderPeriod(HAL_DMAHandle handle, + HAL_EncoderHandle encoderHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_AddDMACounter(HAL_DMAHandle handle, HAL_CounterHandle counterHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_AddDMACounterPeriod(HAL_DMAHandle handle, + HAL_CounterHandle counterHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_AddDMADigitalSource(HAL_DMAHandle handle, + HAL_Handle digitalSourceHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_AddDMAAnalogInput(HAL_DMAHandle handle, + HAL_AnalogInputHandle aInHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_AddDMADutyCycle(HAL_DMAHandle handle, + HAL_DutyCycleHandle dutyCycleHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_AddDMAAveragedAnalogInput(HAL_DMAHandle handle, + HAL_AnalogInputHandle aInHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_AddDMAAnalogAccumulator(HAL_DMAHandle handle, + HAL_AnalogInputHandle aInHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +int32_t HAL_SetDMAExternalTrigger(HAL_DMAHandle handle, + HAL_Handle digitalSourceHandle, + HAL_AnalogTriggerType analogTriggerType, + HAL_Bool rising, HAL_Bool falling, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +void HAL_ClearDMASensors(HAL_DMAHandle handle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_ClearDMAExternalTriggers(HAL_DMAHandle handle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_StartDMA(HAL_DMAHandle handle, int32_t queueDepth, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_StopDMA(HAL_DMAHandle handle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void* HAL_GetDMADirectPointer(HAL_DMAHandle handle) { + return nullptr; +} + +enum HAL_DMAReadStatus HAL_ReadDMADirect(void* dmaPointer, + HAL_DMASample* dmaSample, + double timeoutSeconds, + int32_t* remainingOut, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return HAL_DMAReadStatus::HAL_DMA_ERROR; +} + +enum HAL_DMAReadStatus HAL_ReadDMA(HAL_DMAHandle handle, + HAL_DMASample* dmaSample, + double timeoutSeconds, int32_t* remainingOut, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return HAL_DMAReadStatus::HAL_DMA_ERROR; +} + +uint64_t HAL_GetDMASampleTime(const HAL_DMASample* dmaSample, int32_t* status) { + return dmaSample->timeStamp; +} + +int32_t HAL_GetDMASampleEncoderRaw(const HAL_DMASample* dmaSample, + HAL_EncoderHandle encoderHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +int32_t HAL_GetDMASampleEncoderPeriodRaw(const HAL_DMASample* dmaSample, + HAL_EncoderHandle encoderHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +int32_t HAL_GetDMASampleCounter(const HAL_DMASample* dmaSample, + HAL_CounterHandle counterHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +int32_t HAL_GetDMASampleCounterPeriod(const HAL_DMASample* dmaSample, + HAL_CounterHandle counterHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +HAL_Bool HAL_GetDMASampleDigitalSource(const HAL_DMASample* dmaSample, + HAL_Handle dSourceHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return false; +} +int32_t HAL_GetDMASampleAnalogInputRaw(const HAL_DMASample* dmaSample, + HAL_AnalogInputHandle aInHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +int32_t HAL_GetDMASampleAveragedAnalogInputRaw(const HAL_DMASample* dmaSample, + HAL_AnalogInputHandle aInHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +void HAL_GetDMASampleAnalogAccumulator(const HAL_DMASample* dmaSample, + HAL_AnalogInputHandle aInHandle, + int64_t* count, int64_t* value, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +int32_t HAL_GetDMASampleDutyCycleOutputRaw(const HAL_DMASample* dmaSample, + HAL_DutyCycleHandle dutyCycleHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} +} // extern "C" diff --git a/hal/src/main/native/systemcore/DutyCycle.cpp b/hal/src/main/native/systemcore/DutyCycle.cpp new file mode 100644 index 00000000000..e6b2597688a --- /dev/null +++ b/hal/src/main/native/systemcore/DutyCycle.cpp @@ -0,0 +1,64 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/DutyCycle.h" + +#include + +#include "HALInitializer.h" +#include "PortsInternal.h" +#include "hal/Errors.h" +#include "hal/handles/HandlesInternal.h" +#include "hal/handles/LimitedHandleResource.h" + +using namespace hal; + +namespace hal::init { +void InitializeDutyCycle() {} +} // namespace hal::init + +extern "C" { +HAL_DutyCycleHandle HAL_InitializeDutyCycle(HAL_Handle digitalSourceHandle, + HAL_AnalogTriggerType triggerType, + int32_t* status) { + hal::init::CheckInit(); + + *status = HAL_HANDLE_ERROR; + return HAL_kInvalidHandle; +} +void HAL_FreeDutyCycle(HAL_DutyCycleHandle dutyCycleHandle) {} + +void HAL_SetDutyCycleSimDevice(HAL_EncoderHandle handle, + HAL_SimDeviceHandle device) {} + +int32_t HAL_GetDutyCycleFrequency(HAL_DutyCycleHandle dutyCycleHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +double HAL_GetDutyCycleOutput(HAL_DutyCycleHandle dutyCycleHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +int32_t HAL_GetDutyCycleHighTime(HAL_DutyCycleHandle dutyCycleHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +int32_t HAL_GetDutyCycleOutputScaleFactor(HAL_DutyCycleHandle dutyCycleHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +int32_t HAL_GetDutyCycleFPGAIndex(HAL_DutyCycleHandle dutyCycleHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} +} // extern "C" diff --git a/hal/src/main/native/systemcore/Encoder.cpp b/hal/src/main/native/systemcore/Encoder.cpp new file mode 100644 index 00000000000..1f2c4216dfb --- /dev/null +++ b/hal/src/main/native/systemcore/Encoder.cpp @@ -0,0 +1,167 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/Encoder.h" + +#include + +#include + +#include "HALInitializer.h" +#include "HALInternal.h" +#include "PortsInternal.h" +#include "hal/Counter.h" +#include "hal/Errors.h" +#include "hal/handles/LimitedClassedHandleResource.h" + +using namespace hal; + +namespace hal { + +namespace init { +void InitializeEncoder() {} +} // namespace init + +bool GetEncoderBaseHandle(HAL_EncoderHandle handle, + HAL_FPGAEncoderHandle* fpgaHandle, + HAL_CounterHandle* counterHandle) { + return false; +} +} // namespace hal + +extern "C" { +HAL_EncoderHandle HAL_InitializeEncoder( + HAL_Handle digitalSourceHandleA, HAL_AnalogTriggerType analogTriggerTypeA, + HAL_Handle digitalSourceHandleB, HAL_AnalogTriggerType analogTriggerTypeB, + HAL_Bool reverseDirection, HAL_EncoderEncodingType encodingType, + int32_t* status) { + hal::init::CheckInit(); + *status = HAL_HANDLE_ERROR; + return HAL_kInvalidHandle; +} + +void HAL_FreeEncoder(HAL_EncoderHandle encoderHandle) {} + +void HAL_SetEncoderSimDevice(HAL_EncoderHandle handle, + HAL_SimDeviceHandle device) {} + +int32_t HAL_GetEncoder(HAL_EncoderHandle encoderHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +int32_t HAL_GetEncoderRaw(HAL_EncoderHandle encoderHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +int32_t HAL_GetEncoderEncodingScale(HAL_EncoderHandle encoderHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +void HAL_ResetEncoder(HAL_EncoderHandle encoderHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +double HAL_GetEncoderPeriod(HAL_EncoderHandle encoderHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +void HAL_SetEncoderMaxPeriod(HAL_EncoderHandle encoderHandle, double maxPeriod, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +HAL_Bool HAL_GetEncoderStopped(HAL_EncoderHandle encoderHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return false; +} + +HAL_Bool HAL_GetEncoderDirection(HAL_EncoderHandle encoderHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return false; +} + +double HAL_GetEncoderDistance(HAL_EncoderHandle encoderHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +double HAL_GetEncoderRate(HAL_EncoderHandle encoderHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +void HAL_SetEncoderMinRate(HAL_EncoderHandle encoderHandle, double minRate, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetEncoderDistancePerPulse(HAL_EncoderHandle encoderHandle, + double distancePerPulse, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetEncoderReverseDirection(HAL_EncoderHandle encoderHandle, + HAL_Bool reverseDirection, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetEncoderSamplesToAverage(HAL_EncoderHandle encoderHandle, + int32_t samplesToAverage, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +int32_t HAL_GetEncoderSamplesToAverage(HAL_EncoderHandle encoderHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +double HAL_GetEncoderDecodingScaleFactor(HAL_EncoderHandle encoderHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +double HAL_GetEncoderDistancePerPulse(HAL_EncoderHandle encoderHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +HAL_EncoderEncodingType HAL_GetEncoderEncodingType( + HAL_EncoderHandle encoderHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return HAL_Encoder_k4X; +} + +void HAL_SetEncoderIndexSource(HAL_EncoderHandle encoderHandle, + HAL_Handle digitalSourceHandle, + HAL_AnalogTriggerType analogTriggerType, + HAL_EncoderIndexingType type, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +int32_t HAL_GetEncoderFPGAIndex(HAL_EncoderHandle encoderHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/FRCDriverStation.cpp b/hal/src/main/native/systemcore/FRCDriverStation.cpp new file mode 100644 index 00000000000..8178aabcf56 --- /dev/null +++ b/hal/src/main/native/systemcore/FRCDriverStation.cpp @@ -0,0 +1,612 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include +#include +#include +#include +#include +#include +#include + +// #include +// #include +#include +#include +#include +#include +#include +#include + +#include "HALInitializer.h" +#include "hal/DriverStation.h" +#include "hal/Errors.h" + +static_assert(sizeof(int32_t) >= sizeof(int), + "FRC_NetworkComm status variable is larger than 32 bits"); + +namespace { +struct HAL_JoystickAxesInt { + int16_t count; + int16_t axes[HAL_kMaxJoystickAxes]; +}; +} // namespace + +namespace { +struct JoystickDataCache { + JoystickDataCache() { std::memset(this, 0, sizeof(*this)); } + void Update(); + + HAL_JoystickAxes axes[HAL_kMaxJoysticks]; + HAL_JoystickPOVs povs[HAL_kMaxJoysticks]; + HAL_JoystickButtons buttons[HAL_kMaxJoysticks]; + HAL_AllianceStationID allianceStation; + float matchTime; + HAL_ControlWord controlWord; +}; +static_assert(std::is_standard_layout_v); +// static_assert(std::is_trivial_v); + +struct FRCDriverStation { + wpi::EventVector newDataEvents; +}; +} // namespace + +static ::FRCDriverStation* driverStation; + +// Message and Data variables +static wpi::mutex msgMutex; + +// static int32_t HAL_GetJoystickAxesInternal(int32_t joystickNum, +// HAL_JoystickAxes* axes) { +// return 0; +// // HAL_JoystickAxesInt netcommAxes; + +// // int retVal = FRC_NetworkCommunication_getJoystickAxes( +// // joystickNum, reinterpret_cast(&netcommAxes), +// // HAL_kMaxJoystickAxes); + +// // // copy integer values to double values +// // axes->count = netcommAxes.count; +// // // current scaling is -128 to 127, can easily be patched in the future +// by +// // // changing this function. +// // for (int32_t i = 0; i < netcommAxes.count; i++) { +// // int8_t value = netcommAxes.axes[i]; +// // axes->raw[i] = value; +// // if (value < 0) { +// // axes->axes[i] = value / 128.0; +// // } else { +// // axes->axes[i] = value / 127.0; +// // } +// // } + +// // return retVal; +// } + +// static int32_t HAL_GetJoystickPOVsInternal(int32_t joystickNum, +// HAL_JoystickPOVs* povs) { +// return 0; +// // return FRC_NetworkCommunication_getJoystickPOVs( +// // joystickNum, reinterpret_cast(povs), +// // HAL_kMaxJoystickPOVs); +// } + +// static int32_t HAL_GetJoystickButtonsInternal(int32_t joystickNum, +// HAL_JoystickButtons* buttons) { +// return 0; +// // return FRC_NetworkCommunication_getJoystickButtons( +// // joystickNum, &buttons->buttons, &buttons->count); +// } + +// void JoystickDataCache::Update() { +// // for (int i = 0; i < HAL_kMaxJoysticks; i++) { +// // HAL_GetJoystickAxesInternal(i, &axes[i]); +// // HAL_GetJoystickPOVsInternal(i, &povs[i]); +// // HAL_GetJoystickButtonsInternal(i, &buttons[i]); +// // } +// // AllianceStationID_t alliance = kAllianceStationID_red1; +// // FRC_NetworkCommunication_getAllianceStation(&alliance); +// // int allianceInt = alliance; +// // allianceInt += 1; +// // allianceStation = static_cast(allianceInt); +// // FRC_NetworkCommunication_getMatchTime(&matchTime); +// // FRC_NetworkCommunication_getControlWord( +// // reinterpret_cast(&controlWord)); +// } + +#define CHECK_JOYSTICK_NUMBER(stickNum) \ + if ((stickNum) < 0 || (stickNum) >= HAL_kMaxJoysticks) \ + return PARAMETER_OUT_OF_RANGE + +static HAL_ControlWord newestControlWord; +static JoystickDataCache caches[3]; +static JoystickDataCache* currentRead = &caches[0]; +// static JoystickDataCache* currentReadLocal = &caches[0]; +static std::atomic currentCache{nullptr}; +// static JoystickDataCache* lastGiven = &caches[1]; +// static JoystickDataCache* cacheToUpdate = &caches[2]; + +static wpi::mutex cacheMutex; + +/** + * Retrieve the Joystick Descriptor for particular slot. + * + * @param[out] desc descriptor (data transfer object) to fill in. desc is filled + * in regardless of success. In other words, if descriptor is + * not available, desc is filled in with default values + * matching the init-values in Java and C++ Driverstation for + * when caller requests a too-large joystick index. + * @return error code reported from Network Comm back-end. Zero is good, + * nonzero is bad. + */ +// static int32_t HAL_GetJoystickDescriptorInternal(int32_t joystickNum, +// HAL_JoystickDescriptor* +// desc) { +// return 0; +// // desc->isXbox = 0; +// // desc->type = (std::numeric_limits::max)(); +// // desc->name[0] = '\0'; +// // desc->axisCount = +// // HAL_kMaxJoystickAxes; /* set to the desc->axisTypes's capacity */ +// // desc->buttonCount = 0; +// // desc->povCount = 0; +// // int retval = FRC_NetworkCommunication_getJoystickDesc( +// // joystickNum, &desc->isXbox, &desc->type, +// // reinterpret_cast(&desc->name), &desc->axisCount, +// // reinterpret_cast(&desc->axisTypes), &desc->buttonCount, +// // &desc->povCount); +// // /* check the return, if there is an error and the RIOimage predates +// FRC2017, +// // * then axisCount needs to be cleared */ +// // if (retval != 0) { +// // /* set count to zero so downstream code doesn't decode invalid +// axisTypes. */ +// // desc->axisCount = 0; +// // } +// // return retval; +// } + +// static int32_t HAL_GetMatchInfoInternal(HAL_MatchInfo* info) { +// return 0; +// // MatchType_t matchType = MatchType_t::kMatchType_none; +// // info->gameSpecificMessageSize = sizeof(info->gameSpecificMessage); +// // int status = FRC_NetworkCommunication_getMatchInfo( +// // info->eventName, &matchType, &info->matchNumber, +// &info->replayNumber, +// // info->gameSpecificMessage, &info->gameSpecificMessageSize); + +// // if (info->gameSpecificMessageSize > sizeof(info->gameSpecificMessage)) { +// // info->gameSpecificMessageSize = 0; +// // } + +// // info->matchType = static_cast(matchType); + +// // *(std::end(info->eventName) - 1) = '\0'; + +// // return status; +// } + +namespace { +struct TcpCache { + TcpCache() { std::memset(this, 0, sizeof(*this)); } + void Update(uint32_t mask); + void CloneTo(TcpCache* other) { std::memcpy(other, this, sizeof(*this)); } + + bool hasReadMatchInfo = false; + HAL_MatchInfo matchInfo; + HAL_JoystickDescriptor descriptors[HAL_kMaxJoysticks]; +}; +static_assert(std::is_standard_layout_v); +} // namespace + +static std::atomic_uint32_t tcpMask{0xFFFFFFFF}; +static TcpCache tcpCache; +static TcpCache tcpCurrent; +static wpi::mutex tcpCacheMutex; + +// constexpr uint32_t combinedMatchInfoMask = kTcpRecvMask_MatchInfoOld | +// kTcpRecvMask_MatchInfo | +// kTcpRecvMask_GameSpecific; + +void TcpCache::Update(uint32_t mask) { + // bool failedToReadInfo = false; + // if ((mask & combinedMatchInfoMask) != 0) { + // int status = HAL_GetMatchInfoInternal(&matchInfo); + // if (status != 0) { + // failedToReadInfo = true; + // if (!hasReadMatchInfo) { + // std::memset(&matchInfo, 0, sizeof(matchInfo)); + // } + // } else { + // hasReadMatchInfo = true; + // } + // } + // for (int i = 0; i < HAL_kMaxJoysticks; i++) { + // if ((mask & (1 << i)) != 0) { + // HAL_GetJoystickDescriptorInternal(i, &descriptors[i]); + // } + // } + // return failedToReadInfo; +} + +namespace hal::init { +void InitializeFRCDriverStation() { + std::memset(&newestControlWord, 0, sizeof(newestControlWord)); + static FRCDriverStation ds; + driverStation = &ds; +} +} // namespace hal::init + +namespace hal { +static void DefaultPrintErrorImpl(const char* line, size_t size) { + std::fwrite(line, size, 1, stderr); +} +} // namespace hal + +static std::atomic gPrintErrorImpl{ + hal::DefaultPrintErrorImpl}; + +extern "C" { + +int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode, + const char* details, const char* location, + const char* callStack, HAL_Bool printMsg) { + return 0; + // // Avoid flooding console by keeping track of previous 5 error + // // messages and only printing again if they're longer than 1 second old. + // static constexpr int KEEP_MSGS = 5; + // std::scoped_lock lock(msgMutex); + // static std::string prevMsg[KEEP_MSGS]; + // static std::chrono::time_point + // prevMsgTime[KEEP_MSGS]; + // static bool initialized = false; + // if (!initialized) { + // for (int i = 0; i < KEEP_MSGS; i++) { + // prevMsgTime[i] = + // std::chrono::steady_clock::now() - std::chrono::seconds(2); + // } + // initialized = true; + // } + + // auto curTime = std::chrono::steady_clock::now(); + // int i; + // for (i = 0; i < KEEP_MSGS; ++i) { + // if (prevMsg[i] == details) { + // break; + // } + // } + // int retval = 0; + // if (i == KEEP_MSGS || (curTime - prevMsgTime[i]) >= + // std::chrono::seconds(1)) { + // std::string_view detailsRef{details}; + // std::string_view locationRef{location}; + // std::string_view callStackRef{callStack}; + + // // 2 size, 1 tag, 4 timestamp, 2 seqnum + // // 2 numOccur, 4 error code, 1 flags, 6 strlen + // // 1 extra needed for padding on Netcomm end. + // size_t baseLength = 23; + + // if (baseLength + detailsRef.size() + locationRef.size() + + // callStackRef.size() <= + // 65535) { + // // Pass through + // retval = FRC_NetworkCommunication_sendError(isError, errorCode, + // isLVCode, + // details, location, + // callStack); + // } else if (baseLength + detailsRef.size() > 65535) { + // // Details too long, cut both location and stack + // auto newLen = 65535 - baseLength; + // std::string newDetails{details, newLen}; + // char empty = '\0'; + // retval = FRC_NetworkCommunication_sendError( + // isError, errorCode, isLVCode, newDetails.c_str(), &empty, &empty); + // } else if (baseLength + detailsRef.size() + locationRef.size() > 65535) { + // // Location too long, cut stack + // auto newLen = 65535 - baseLength - detailsRef.size(); + // std::string newLocation{location, newLen}; + // char empty = '\0'; + // retval = FRC_NetworkCommunication_sendError( + // isError, errorCode, isLVCode, details, newLocation.c_str(), + // &empty); + // } else { + // // Stack too long + // auto newLen = 65535 - baseLength - detailsRef.size() - + // locationRef.size(); std::string newCallStack{callStack, newLen}; retval + // = FRC_NetworkCommunication_sendError(isError, errorCode, isLVCode, + // details, location, + // newCallStack.c_str()); + // } + // if (printMsg) { + // fmt::memory_buffer buf; + // if (location && location[0] != '\0') { + // fmt::format_to(fmt::appender{buf}, + // "{} at {}: ", isError ? "Error" : "Warning", + // location); + // } + // fmt::format_to(fmt::appender{buf}, "{}\n", details); + // if (callStack && callStack[0] != '\0') { + // fmt::format_to(fmt::appender{buf}, "{}\n", callStack); + // } + // auto printError = gPrintErrorImpl.load(); + // printError(buf.data(), buf.size()); + // } + // if (i == KEEP_MSGS) { + // // replace the oldest one + // i = 0; + // auto first = prevMsgTime[0]; + // for (int j = 1; j < KEEP_MSGS; ++j) { + // if (prevMsgTime[j] < first) { + // first = prevMsgTime[j]; + // i = j; + // } + // } + // prevMsg[i] = details; + // } + // prevMsgTime[i] = curTime; + // } + // return retval; +} + +void HAL_SetPrintErrorImpl(void (*func)(const char* line, size_t size)) { + gPrintErrorImpl.store(func ? func : hal::DefaultPrintErrorImpl); +} + +int32_t HAL_SendConsoleLine(const char* line) { + return 0; + // std::string_view lineRef{line}; + // if (lineRef.size() <= 65535) { + // // Send directly + // return FRC_NetworkCommunication_sendConsoleLine(line); + // } else { + // // Need to truncate + // std::string newLine{line, 65535}; + // return FRC_NetworkCommunication_sendConsoleLine(newLine.c_str()); + // } +} + +int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) { + std::scoped_lock lock{cacheMutex}; + *controlWord = newestControlWord; + return 0; +} + +int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) { + CHECK_JOYSTICK_NUMBER(joystickNum); + std::scoped_lock lock{cacheMutex}; + *axes = currentRead->axes[joystickNum]; + return 0; +} + +int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) { + CHECK_JOYSTICK_NUMBER(joystickNum); + std::scoped_lock lock{cacheMutex}; + *povs = currentRead->povs[joystickNum]; + return 0; +} + +int32_t HAL_GetJoystickButtons(int32_t joystickNum, + HAL_JoystickButtons* buttons) { + CHECK_JOYSTICK_NUMBER(joystickNum); + std::scoped_lock lock{cacheMutex}; + *buttons = currentRead->buttons[joystickNum]; + return 0; +} + +void HAL_GetAllJoystickData(HAL_JoystickAxes* axes, HAL_JoystickPOVs* povs, + HAL_JoystickButtons* buttons) { + std::scoped_lock lock{cacheMutex}; + std::memcpy(axes, currentRead->axes, sizeof(currentRead->axes)); + std::memcpy(povs, currentRead->povs, sizeof(currentRead->povs)); + std::memcpy(buttons, currentRead->buttons, sizeof(currentRead->buttons)); +} + +int32_t HAL_GetJoystickDescriptor(int32_t joystickNum, + HAL_JoystickDescriptor* desc) { + CHECK_JOYSTICK_NUMBER(joystickNum); + std::scoped_lock lock{tcpCacheMutex}; + *desc = tcpCurrent.descriptors[joystickNum]; + return 0; +} + +int32_t HAL_GetMatchInfo(HAL_MatchInfo* info) { + std::scoped_lock lock{tcpCacheMutex}; + *info = tcpCurrent.matchInfo; + return 0; +} + +HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) { + std::scoped_lock lock{cacheMutex}; + return currentRead->allianceStation; +} + +HAL_Bool HAL_GetJoystickIsXbox(int32_t joystickNum) { + HAL_JoystickDescriptor joystickDesc; + if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) { + return 0; + } else { + return joystickDesc.isXbox; + } +} + +int32_t HAL_GetJoystickType(int32_t joystickNum) { + HAL_JoystickDescriptor joystickDesc; + if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) { + return -1; + } else { + return joystickDesc.type; + } +} + +void HAL_GetJoystickName(struct WPI_String* name, int32_t joystickNum) { + HAL_JoystickDescriptor joystickDesc; + const char* cName = joystickDesc.name; + if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) { + cName = ""; + } + auto len = std::strlen(cName); + auto write = WPI_AllocateString(name, len); + std::memcpy(write, cName, len); +} + +int32_t HAL_GetJoystickAxisType(int32_t joystickNum, int32_t axis) { + HAL_JoystickDescriptor joystickDesc; + if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) { + return -1; + } else { + return joystickDesc.axisTypes[axis]; + } +} + +int32_t HAL_SetJoystickOutputs(int32_t joystickNum, int64_t outputs, + int32_t leftRumble, int32_t rightRumble) { + CHECK_JOYSTICK_NUMBER(joystickNum); + return 0; + // return FRC_NetworkCommunication_setJoystickOutputs(joystickNum, outputs, + // leftRumble, + // rightRumble); +} + +double HAL_GetMatchTime(int32_t* status) { + std::scoped_lock lock{cacheMutex}; + return currentRead->matchTime; +} + +void HAL_ObserveUserProgramStarting(void) { + // FRC_NetworkCommunication_observeUserProgramStarting(); +} + +void HAL_ObserveUserProgramDisabled(void) { + // FRC_NetworkCommunication_observeUserProgramDisabled(); +} + +void HAL_ObserveUserProgramAutonomous(void) { + // FRC_NetworkCommunication_observeUserProgramAutonomous(); +} + +void HAL_ObserveUserProgramTeleop(void) { + // FRC_NetworkCommunication_observeUserProgramTeleop(); +} + +void HAL_ObserveUserProgramTest(void) { + // FRC_NetworkCommunication_observeUserProgramTest(); +} + +// // Constant number to be used for our occur handle +// constexpr int32_t refNumber = 42; +// constexpr int32_t tcpRefNumber = 94; + +// static void tcpOccur(void) { +// uint32_t mask = FRC_NetworkCommunication_getNewTcpRecvMask(); +// tcpMask.fetch_or(mask); +// } + +// static void udpOccur(void) { +// cacheToUpdate->Update(); + +// JoystickDataCache* given = cacheToUpdate; +// JoystickDataCache* prev = currentCache.exchange(cacheToUpdate); +// if (prev == nullptr) { +// cacheToUpdate = currentReadLocal; +// currentReadLocal = lastGiven; +// } else { +// // Current read local does not update +// cacheToUpdate = prev; +// } +// lastGiven = given; + +// driverStation->newDataEvents.Wakeup(); +// } + +// static void newDataOccur(uint32_t refNum) { +// switch (refNum) { +// case refNumber: +// udpOccur(); +// break; + +// case tcpRefNumber: +// tcpOccur(); +// break; + +// default: +// std::printf("Unknown occur %u\n", refNum); +// break; +// } +// } + +HAL_Bool HAL_RefreshDSData(void) { + HAL_ControlWord controlWord; + std::memset(&controlWord, 0, sizeof(controlWord)); + // FRC_NetworkCommunication_getControlWord( + // reinterpret_cast(&controlWord)); + JoystickDataCache* prev; + { + std::scoped_lock lock{cacheMutex}; + prev = currentCache.exchange(nullptr); + if (prev != nullptr) { + currentRead = prev; + } + // If newest state shows we have a DS attached, just use the + // control word out of the cache, As it will be the one in sync + // with the data. If no data has been updated, at this point, + // and a DS wasn't attached previously, this will still return + // a zeroed out control word, with is the correct state for + // no new data. + if (!controlWord.dsAttached) { + // If the DS is not attached, we need to zero out the control word. + // This is because HAL_RefreshDSData is called asynchronously from + // the DS data. The dsAttached variable comes directly from netcomm + // and could be updated before the caches are. If that happens, + // we would end up returning the previous cached control word, + // which is out of sync with the current control word and could + // break invariants such as which alliance station is in used. + // Also, when the DS has never been connected the rest of the fields + // in control word are garbage, so we also need to zero out in that + // case too + std::memset(¤tRead->controlWord, 0, + sizeof(currentRead->controlWord)); + } + newestControlWord = currentRead->controlWord; + } + + uint32_t mask = tcpMask.exchange(0); + if (mask != 0) { + tcpCache.Update(mask); + std::scoped_lock tcpLock(tcpCacheMutex); + tcpCache.CloneTo(&tcpCurrent); + } + return prev != nullptr; +} + +void HAL_ProvideNewDataEventHandle(WPI_EventHandle handle) { + hal::init::CheckInit(); + driverStation->newDataEvents.Add(handle); +} + +void HAL_RemoveNewDataEventHandle(WPI_EventHandle handle) { + driverStation->newDataEvents.Remove(handle); +} + +HAL_Bool HAL_GetOutputsEnabled(void) { + return false; + // return FRC_NetworkCommunication_getWatchdogActive(); +} + +} // extern "C" + +namespace hal { +void InitializeDriverStation() {} + +void WaitForInitialPacket() { + wpi::Event waitForInitEvent; + driverStation->newDataEvents.Add(waitForInitEvent.GetHandle()); + bool timed_out = false; + wpi::WaitForObject(waitForInitEvent.GetHandle(), 0.1, &timed_out); + // Don't care what the result is, just want to give it a chance. + driverStation->newDataEvents.Remove(waitForInitEvent.GetHandle()); +} +} // namespace hal diff --git a/hal/src/main/native/systemcore/HAL.cpp b/hal/src/main/native/systemcore/HAL.cpp new file mode 100644 index 00000000000..a58581b8042 --- /dev/null +++ b/hal/src/main/native/systemcore/HAL.cpp @@ -0,0 +1,380 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/HAL.h" + +#include +#include // linux for kill +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "HALInitializer.h" +#include "HALInternal.h" +#include "hal/DriverStation.h" +#include "hal/Errors.h" +#include "hal/Notifier.h" +#include "hal/handles/HandlesInternal.h" +#include "hal/roborio/HMB.h" + +using namespace hal; + +static uint64_t dsStartTime; + +static int32_t teamNumber = -1; + +using namespace hal; + +namespace hal { +void InitializeDriverStation(); +void WaitForInitialPacket(); +namespace init { +void InitializeHAL() { + InitializeCTREPCM(); + InitializeREVPH(); + InitializeAddressableLED(); + InitializeAccelerometer(); + InitializeAnalogAccumulator(); + InitializeAnalogGyro(); + InitializeAnalogInput(); + InitializeAnalogOutput(); + InitializeAnalogTrigger(); + InitializeCAN(); + InitializeCANAPI(); + InitializeConstants(); + InitializeCounter(); + InitializeDIO(); + InitializeDMA(); + InitializeDutyCycle(); + InitializeEncoder(); + InitializeFRCDriverStation(); + InitializeI2C(); + InitializeInterrupts(); + InitializeLEDs(); + InitializeMain(); + InitializeNotifier(); + InitializeCTREPDP(); + InitializeREVPDH(); + InitializePorts(); + InitializePower(); + InitializePWM(); + InitializeRelay(); + InitializeSerialPort(); + InitializeSPI(); + InitializeThreads(); +} +} // namespace init + +uint64_t GetDSInitializeTime() { + return dsStartTime; +} + +} // namespace hal + +extern "C" { + +HAL_PortHandle HAL_GetPort(int32_t channel) { + // Dont allow a number that wouldn't fit in a uint8_t + if (channel < 0 || channel >= 255) { + return HAL_kInvalidHandle; + } + return createPortHandle(channel, 1); +} + +HAL_PortHandle HAL_GetPortWithModule(int32_t module, int32_t channel) { + // Dont allow a number that wouldn't fit in a uint8_t + if (channel < 0 || channel >= 255) { + return HAL_kInvalidHandle; + } + if (module < 0 || module >= 255) { + return HAL_kInvalidHandle; + } + return createPortHandle(channel, module); +} + +const char* HAL_GetErrorMessage(int32_t code) { + switch (code) { + case 0: + return ""; + case SAMPLE_RATE_TOO_HIGH: + return SAMPLE_RATE_TOO_HIGH_MESSAGE; + case VOLTAGE_OUT_OF_RANGE: + return VOLTAGE_OUT_OF_RANGE_MESSAGE; + case LOOP_TIMING_ERROR: + return LOOP_TIMING_ERROR_MESSAGE; + case SPI_WRITE_NO_MOSI: + return SPI_WRITE_NO_MOSI_MESSAGE; + case SPI_READ_NO_MISO: + return SPI_READ_NO_MISO_MESSAGE; + case SPI_READ_NO_DATA: + return SPI_READ_NO_DATA_MESSAGE; + case INCOMPATIBLE_STATE: + return INCOMPATIBLE_STATE_MESSAGE; + case NO_AVAILABLE_RESOURCES: + return NO_AVAILABLE_RESOURCES_MESSAGE; + case RESOURCE_IS_ALLOCATED: + return RESOURCE_IS_ALLOCATED_MESSAGE; + case RESOURCE_OUT_OF_RANGE: + return RESOURCE_OUT_OF_RANGE_MESSAGE; + case HAL_INVALID_ACCUMULATOR_CHANNEL: + return HAL_INVALID_ACCUMULATOR_CHANNEL_MESSAGE; + case HAL_HANDLE_ERROR: + return HAL_HANDLE_ERROR_MESSAGE; + case NULL_PARAMETER: + return NULL_PARAMETER_MESSAGE; + case ANALOG_TRIGGER_LIMIT_ORDER_ERROR: + return ANALOG_TRIGGER_LIMIT_ORDER_ERROR_MESSAGE; + case ANALOG_TRIGGER_PULSE_OUTPUT_ERROR: + return ANALOG_TRIGGER_PULSE_OUTPUT_ERROR_MESSAGE; + case PARAMETER_OUT_OF_RANGE: + return PARAMETER_OUT_OF_RANGE_MESSAGE; + case HAL_COUNTER_NOT_SUPPORTED: + return HAL_COUNTER_NOT_SUPPORTED_MESSAGE; + case HAL_ERR_CANSessionMux_InvalidBuffer: + return ERR_CANSessionMux_InvalidBuffer_MESSAGE; + case HAL_ERR_CANSessionMux_MessageNotFound: + return ERR_CANSessionMux_MessageNotFound_MESSAGE; + case HAL_WARN_CANSessionMux_NoToken: + return WARN_CANSessionMux_NoToken_MESSAGE; + case HAL_ERR_CANSessionMux_NotAllowed: + return ERR_CANSessionMux_NotAllowed_MESSAGE; + case HAL_ERR_CANSessionMux_NotInitialized: + return ERR_CANSessionMux_NotInitialized_MESSAGE; + case HAL_PWM_SCALE_ERROR: + return HAL_PWM_SCALE_ERROR_MESSAGE; + case HAL_SERIAL_PORT_NOT_FOUND: + return HAL_SERIAL_PORT_NOT_FOUND_MESSAGE; + case HAL_THREAD_PRIORITY_ERROR: + return HAL_THREAD_PRIORITY_ERROR_MESSAGE; + case HAL_THREAD_PRIORITY_RANGE_ERROR: + return HAL_THREAD_PRIORITY_RANGE_ERROR_MESSAGE; + case HAL_SERIAL_PORT_OPEN_ERROR: + return HAL_SERIAL_PORT_OPEN_ERROR_MESSAGE; + case HAL_SERIAL_PORT_ERROR: + return HAL_SERIAL_PORT_ERROR_MESSAGE; + case HAL_CAN_TIMEOUT: + return HAL_CAN_TIMEOUT_MESSAGE; + case HAL_CAN_BUFFER_OVERRUN: + return HAL_CAN_BUFFER_OVERRUN_MESSAGE; + case HAL_LED_CHANNEL_ERROR: + return HAL_LED_CHANNEL_ERROR_MESSAGE; + case HAL_INVALID_DMA_STATE: + return HAL_INVALID_DMA_STATE_MESSAGE; + case HAL_INVALID_DMA_ADDITION: + return HAL_INVALID_DMA_ADDITION_MESSAGE; + case HAL_USE_LAST_ERROR: + return HAL_USE_LAST_ERROR_MESSAGE; + case HAL_CONSOLE_OUT_ENABLED_ERROR: + return HAL_CONSOLE_OUT_ENABLED_ERROR_MESSAGE; + default: + return "Unknown error status"; + } +} + +static HAL_RuntimeType runtimeType = HAL_Runtime_SystemCore; + +HAL_RuntimeType HAL_GetRuntimeType(void) { + return runtimeType; +} + +int32_t HAL_GetFPGAVersion(int32_t* status) { + hal::init::CheckInit(); + *status = HAL_HANDLE_ERROR; + return 0; +} + +int64_t HAL_GetFPGARevision(int32_t* status) { + hal::init::CheckInit(); + *status = HAL_HANDLE_ERROR; + return 0; +} + +void HAL_GetSerialNumber(struct WPI_String* serialNumber) { + const char* serialNum = std::getenv("serialnum"); + if (!serialNum) { + serialNum = ""; + } + size_t len = std::strlen(serialNum); + auto write = WPI_AllocateString(serialNumber, len); + std::memcpy(write, serialNum, len); +} + +void HAL_GetComments(struct WPI_String* comments) { + comments->len = 0; + comments->str = nullptr; +} + +void InitializeTeamNumber(void) { + char hostnameBuf[25]; + auto status = gethostname(hostnameBuf, sizeof(hostnameBuf)); + if (status != 0) { + teamNumber = 0; + return; + } + + std::string_view hostname{hostnameBuf, sizeof(hostnameBuf)}; + + // hostname is frc-{TEAM}-roborio + // Split string around '-' (max of 2 splits), take the second element of the + // resulting array. + wpi::SmallVector elements; + wpi::split(hostname, elements, "-", 2); + if (elements.size() < 3) { + teamNumber = 0; + return; + } + + teamNumber = wpi::parse_integer(elements[1], 10).value_or(0); +} + +int32_t HAL_GetTeamNumber(void) { + if (teamNumber == -1) { + InitializeTeamNumber(); + } + return teamNumber; +} + +uint64_t HAL_GetFPGATime(int32_t* status) { + hal::init::CheckInit(); + return wpi::NowDefault(); +} + +uint64_t HAL_ExpandFPGATime(uint32_t unexpandedLower, int32_t* status) { + // Capture the current FPGA time. This will give us the upper half of the + // clock. + uint64_t fpgaTime = HAL_GetFPGATime(status); + if (*status != 0) { + return 0; + } + + // Now, we need to detect the case where the lower bits rolled over after we + // sampled. In that case, the upper bits will be 1 bigger than they should + // be. + + // Break it into lower and upper portions. + uint32_t lower = fpgaTime & 0xffffffffull; + uint64_t upper = (fpgaTime >> 32) & 0xffffffff; + + // The time was sampled *before* the current time, so roll it back. + if (lower < unexpandedLower) { + --upper; + } + + return (upper << 32) + static_cast(unexpandedLower); +} + +HAL_Bool HAL_GetFPGAButton(int32_t* status) { + hal::init::CheckInit(); + *status = HAL_HANDLE_ERROR; + return false; +} + +HAL_Bool HAL_GetSystemActive(int32_t* status) { + hal::init::CheckInit(); + *status = HAL_HANDLE_ERROR; + return false; +} + +HAL_Bool HAL_GetBrownedOut(int32_t* status) { + hal::init::CheckInit(); + *status = HAL_HANDLE_ERROR; + return false; +} + +int32_t HAL_GetCommsDisableCount(int32_t* status) { + hal::init::CheckInit(); + *status = HAL_HANDLE_ERROR; + return 0; +} + +HAL_Bool HAL_GetRSLState(int32_t* status) { + hal::init::CheckInit(); + *status = HAL_HANDLE_ERROR; + return false; +} + +HAL_Bool HAL_GetSystemTimeValid(int32_t* status) { + *status = HAL_HANDLE_ERROR; + return false; +} + +HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) { + static std::atomic_bool initialized{false}; + static wpi::mutex initializeMutex; + // Initial check, as if it's true initialization has finished + if (initialized) { + return true; + } + + std::scoped_lock lock(initializeMutex); + // Second check in case another thread was waiting + if (initialized) { + return true; + } + + hal::init::InitializeHAL(); + + hal::init::HAL_IsInitialized.store(true); + + setlinebuf(stdin); + setlinebuf(stdout); + + prctl(PR_SET_PDEATHSIG, SIGTERM); + + // // Return false if program failed to kill an existing program + // if (!killExistingProgram(timeout, mode)) { + // return false; + // } + + // FRC_NetworkCommunication_Reserve(nullptr); + + int32_t status = 0; + + hal::InitializeDriverStation(); + + dsStartTime = HAL_GetFPGATime(&status); + if (status != 0) { + return false; + } + + hal::WaitForInitialPacket(); + + initialized = true; + return true; +} + +void HAL_Shutdown(void) {} + +void HAL_SimPeriodicBefore(void) {} + +void HAL_SimPeriodicAfter(void) {} + +int64_t HAL_Report(int32_t resource, int32_t instanceNumber, int32_t context, + const char* feature) { + if (feature == nullptr) { + feature = ""; + } + + return 0; + + // return FRC_NetworkCommunication_nUsageReporting_report( + // resource, instanceNumber, context, feature); +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/HALInitializer.cpp b/hal/src/main/native/systemcore/HALInitializer.cpp new file mode 100644 index 00000000000..50cc9ab42de --- /dev/null +++ b/hal/src/main/native/systemcore/HALInitializer.cpp @@ -0,0 +1,14 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "HALInitializer.h" + +#include "hal/HALBase.h" + +namespace hal::init { +std::atomic_bool HAL_IsInitialized{false}; +void RunInitialize() { + HAL_Initialize(500, 0); +} +} // namespace hal::init diff --git a/hal/src/main/native/systemcore/HALInitializer.h b/hal/src/main/native/systemcore/HALInitializer.h new file mode 100644 index 00000000000..20b9a8bff08 --- /dev/null +++ b/hal/src/main/native/systemcore/HALInitializer.h @@ -0,0 +1,55 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +namespace hal::init { +extern std::atomic_bool HAL_IsInitialized; +extern void RunInitialize(); +inline void CheckInit() { + if (HAL_IsInitialized.load(std::memory_order_relaxed)) { + return; + } + RunInitialize(); +} + +extern void InitializeCTREPCM(); +extern void InitializeREVPH(); +extern void InitializeAccelerometer(); +extern void InitializeAddressableLED(); +extern void InitializeAnalogAccumulator(); +extern void InitializeAnalogGyro(); +extern void InitializeAnalogInput(); +extern void InitializeAnalogInternal(); +extern void InitializeAnalogOutput(); +extern void InitializeAnalogTrigger(); +extern void InitializeCAN(); +extern void InitializeCANAPI(); +extern void InitializeConstants(); +extern void InitializeCounter(); +extern void InitializeDigitalInternal(); +extern void InitializeDIO(); +extern void InitializeDMA(); +extern void InitializeDutyCycle(); +extern void InitializeEncoder(); +extern void InitializeFPGAEncoder(); +extern void InitializeFRCDriverStation(); +extern void InitializeHAL(); +extern void InitializeI2C(); +extern void InitializeInterrupts(); +extern void InitializeLEDs(); +extern void InitializeMain(); +extern void InitializeNotifier(); +extern void InitializeCTREPDP(); +extern void InitializeREVPDH(); +extern void InitializePorts(); +extern void InitializePower(); +extern void InitializePWM(); +extern void InitializeRelay(); +extern void InitializeSerialPort(); +extern void InitializeSPI(); +extern void InitializeThreads(); +} // namespace hal::init diff --git a/hal/src/main/native/systemcore/HALInternal.h b/hal/src/main/native/systemcore/HALInternal.h new file mode 100644 index 00000000000..b384b51bfa0 --- /dev/null +++ b/hal/src/main/native/systemcore/HALInternal.h @@ -0,0 +1,20 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include + +namespace hal { +void SetLastError(int32_t* status, std::string_view value); +void SetLastErrorIndexOutOfRange(int32_t* status, std::string_view message, + int32_t minimum, int32_t maximum, + int32_t channel); +void SetLastErrorPreviouslyAllocated(int32_t* status, std::string_view message, + int32_t channel, + std::string_view previousAllocation); +uint64_t GetDSInitializeTime(); +} // namespace hal diff --git a/hal/src/main/native/systemcore/I2C.cpp b/hal/src/main/native/systemcore/I2C.cpp new file mode 100644 index 00000000000..6e6c3f833cf --- /dev/null +++ b/hal/src/main/native/systemcore/I2C.cpp @@ -0,0 +1,54 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/I2C.h" + +#include +#include +#include +#include +#include + +#include + +#include + +#include "HALInitializer.h" +#include "HALInternal.h" +#include "hal/DIO.h" +#include "hal/HAL.h" + +using namespace hal; + +namespace hal::init { +void InitializeI2C() {} +} // namespace hal::init + +extern "C" { + +void HAL_InitializeI2C(HAL_I2CPort port, int32_t* status) { + hal::init::CheckInit(); + *status = HAL_HANDLE_ERROR; + return; +} + +int32_t HAL_TransactionI2C(HAL_I2CPort port, int32_t deviceAddress, + const uint8_t* dataToSend, int32_t sendSize, + uint8_t* dataReceived, int32_t receiveSize) { + return -1; +} + +int32_t HAL_WriteI2C(HAL_I2CPort port, int32_t deviceAddress, + const uint8_t* dataToSend, int32_t sendSize) { + return -1; +} + +int32_t HAL_ReadI2C(HAL_I2CPort port, int32_t deviceAddress, uint8_t* buffer, + int32_t count) { + return -1; +} + +void HAL_CloseI2C(HAL_I2CPort port) {} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/Interrupts.cpp b/hal/src/main/native/systemcore/Interrupts.cpp new file mode 100644 index 00000000000..c7bff65f8fd --- /dev/null +++ b/hal/src/main/native/systemcore/Interrupts.cpp @@ -0,0 +1,83 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/Interrupts.h" + +#include + +#include + +#include "HALInitializer.h" +#include "HALInternal.h" +#include "PortsInternal.h" +#include "hal/Errors.h" +#include "hal/HALBase.h" +#include "hal/handles/HandlesInternal.h" +#include "hal/handles/LimitedHandleResource.h" + +using namespace hal; + +namespace hal::init { +void InitializeInterrupts() {} +} // namespace hal::init + +extern "C" { + +HAL_InterruptHandle HAL_InitializeInterrupts(int32_t* status) { + hal::init::CheckInit(); + *status = HAL_HANDLE_ERROR; + return HAL_kInvalidHandle; +} + +void HAL_CleanInterrupts(HAL_InterruptHandle interruptHandle) {} + +int64_t HAL_WaitForInterrupt(HAL_InterruptHandle interruptHandle, + double timeout, HAL_Bool ignorePrevious, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +int64_t HAL_WaitForMultipleInterrupts(HAL_InterruptHandle interruptHandle, + int64_t mask, double timeout, + HAL_Bool ignorePrevious, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +int64_t HAL_ReadInterruptRisingTimestamp(HAL_InterruptHandle interruptHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +int64_t HAL_ReadInterruptFallingTimestamp(HAL_InterruptHandle interruptHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +void HAL_RequestInterrupts(HAL_InterruptHandle interruptHandle, + HAL_Handle digitalSourceHandle, + HAL_AnalogTriggerType analogTriggerType, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetInterruptUpSourceEdge(HAL_InterruptHandle interruptHandle, + HAL_Bool risingEdge, HAL_Bool fallingEdge, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_ReleaseWaitingInterrupt(HAL_InterruptHandle interruptHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/LEDs.cpp b/hal/src/main/native/systemcore/LEDs.cpp new file mode 100644 index 00000000000..c7d7b5bea2f --- /dev/null +++ b/hal/src/main/native/systemcore/LEDs.cpp @@ -0,0 +1,34 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/LEDs.h" + +#include + +#include +#include + +#include +#include +#include + +#include "HALInternal.h" +#include "hal/Errors.h" + +namespace hal::init { + +void InitializeLEDs() {} +} // namespace hal::init + +extern "C" { +void HAL_SetRadioLEDState(HAL_RadioLEDState state, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +HAL_RadioLEDState HAL_GetRadioLEDState(int32_t* status) { + *status = HAL_HANDLE_ERROR; + return HAL_RadioLED_kOff; +} +} // extern "C" diff --git a/hal/src/main/native/systemcore/Notifier.cpp b/hal/src/main/native/systemcore/Notifier.cpp new file mode 100644 index 00000000000..3587a4cdd28 --- /dev/null +++ b/hal/src/main/native/systemcore/Notifier.cpp @@ -0,0 +1,195 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/Notifier.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "HALInitializer.h" +#include "hal/Errors.h" +#include "hal/HALBase.h" +#include "hal/cpp/fpga_clock.h" +#include "hal/handles/UnlimitedHandleResource.h" +#include "hal/simulation/NotifierData.h" + +namespace { +struct Notifier { + std::string name; + uint64_t waitTime = UINT64_MAX; + bool active = true; + bool waitTimeValid = false; // True if waitTime is set and in the future + bool waitingForAlarm = false; // True if in HAL_WaitForNotifierAlarm() + uint64_t waitCount = 0; // Counts calls to HAL_WaitForNotifierAlarm() + wpi::mutex mutex; + wpi::condition_variable cond; +}; +} // namespace + +using namespace hal; + +static wpi::mutex notifiersWaiterMutex; +static wpi::condition_variable notifiersWaiterCond; + +class NotifierHandleContainer + : public UnlimitedHandleResource { + public: + ~NotifierHandleContainer() { + ForEach([](HAL_NotifierHandle handle, Notifier* notifier) { + { + std::scoped_lock lock(notifier->mutex); + notifier->active = false; + notifier->waitTimeValid = false; + } + notifier->cond.notify_all(); // wake up any waiting threads + }); + notifiersWaiterCond.notify_all(); + } +}; + +static NotifierHandleContainer* notifierHandles; +static std::atomic notifiersPaused{false}; + +namespace hal::init { +void InitializeNotifier() { + static NotifierHandleContainer nH; + notifierHandles = &nH; +} +} // namespace hal::init + +extern "C" { + +HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status) { + hal::init::CheckInit(); + std::shared_ptr notifier = std::make_shared(); + HAL_NotifierHandle handle = notifierHandles->Allocate(notifier); + if (handle == HAL_kInvalidHandle) { + *status = HAL_HANDLE_ERROR; + return HAL_kInvalidHandle; + } + return handle; +} + +HAL_Bool HAL_SetNotifierThreadPriority(HAL_Bool realTime, int32_t priority, + int32_t* status) { + // TODO fix this + return true; +} + +void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name, + int32_t* status) { + auto notifier = notifierHandles->Get(notifierHandle); + if (!notifier) { + return; + } + std::scoped_lock lock(notifier->mutex); + notifier->name = name; +} + +void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) { + auto notifier = notifierHandles->Get(notifierHandle); + if (!notifier) { + return; + } + + { + std::scoped_lock lock(notifier->mutex); + notifier->active = false; + notifier->waitTimeValid = false; + } + notifier->cond.notify_all(); +} + +void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle) { + auto notifier = notifierHandles->Free(notifierHandle); + if (!notifier) { + return; + } + + // Just in case HAL_StopNotifier() wasn't called... + { + std::scoped_lock lock(notifier->mutex); + notifier->active = false; + notifier->waitTimeValid = false; + } + notifier->cond.notify_all(); +} + +void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle, + uint64_t triggerTime, int32_t* status) { + auto notifier = notifierHandles->Get(notifierHandle); + if (!notifier) { + return; + } + + { + std::scoped_lock lock(notifier->mutex); + notifier->waitTime = triggerTime; + notifier->waitTimeValid = (triggerTime != UINT64_MAX); + } + + // We wake up any waiters to change how long they're sleeping for + notifier->cond.notify_all(); +} + +void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle, + int32_t* status) { + auto notifier = notifierHandles->Get(notifierHandle); + if (!notifier) { + return; + } + + { + std::scoped_lock lock(notifier->mutex); + notifier->waitTimeValid = false; + } +} + +uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle, + int32_t* status) { + auto notifier = notifierHandles->Get(notifierHandle); + if (!notifier) { + return 0; + } + + std::unique_lock ulock(notifiersWaiterMutex); + std::unique_lock lock(notifier->mutex); + notifier->waitingForAlarm = true; + ++notifier->waitCount; + ulock.unlock(); + notifiersWaiterCond.notify_all(); + while (notifier->active) { + uint64_t curTime = HAL_GetFPGATime(status); + if (notifier->waitTimeValid && curTime >= notifier->waitTime) { + notifier->waitTimeValid = false; + notifier->waitingForAlarm = false; + return curTime; + } + + double waitDuration; + if (!notifier->waitTimeValid || notifiersPaused) { + // If not running, wait 1000 seconds + waitDuration = 1000.0; + } else { + waitDuration = (notifier->waitTime - curTime) * 1e-6; + } + + notifier->cond.wait_for(lock, std::chrono::duration(waitDuration)); + } + notifier->waitingForAlarm = false; + return 0; +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/PWM.cpp b/hal/src/main/native/systemcore/PWM.cpp new file mode 100644 index 00000000000..d50d4160c99 --- /dev/null +++ b/hal/src/main/native/systemcore/PWM.cpp @@ -0,0 +1,139 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/PWM.h" + +#include +#include +#include +#include + +#include +#include + +#include "HALInitializer.h" +#include "HALInternal.h" +#include "PortsInternal.h" +#include "hal/Errors.h" +#include "hal/cpp/fpga_clock.h" +#include "hal/handles/HandlesInternal.h" + +using namespace hal; + +namespace hal::init { +void InitializePWM() {} +} // namespace hal::init + +extern "C" { + +HAL_DigitalHandle HAL_InitializePWMPort(HAL_PortHandle portHandle, + const char* allocationLocation, + int32_t* status) { + hal::init::CheckInit(); + *status = HAL_HANDLE_ERROR; + return HAL_kInvalidHandle; +} + +void HAL_FreePWMPort(HAL_DigitalHandle pwmPortHandle) {} + +HAL_Bool HAL_CheckPWMChannel(int32_t channel) { + return false; +} + +void HAL_SetPWMConfigMicroseconds(HAL_DigitalHandle pwmPortHandle, int32_t max, + int32_t deadbandMax, int32_t center, + int32_t deadbandMin, int32_t min, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_GetPWMConfigMicroseconds(HAL_DigitalHandle pwmPortHandle, + int32_t* maxPwm, int32_t* deadbandMaxPwm, + int32_t* centerPwm, int32_t* deadbandMinPwm, + int32_t* minPwm, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetPWMEliminateDeadband(HAL_DigitalHandle pwmPortHandle, + HAL_Bool eliminateDeadband, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +HAL_Bool HAL_GetPWMEliminateDeadband(HAL_DigitalHandle pwmPortHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return false; +} + +void HAL_SetPWMPulseTimeMicroseconds(HAL_DigitalHandle pwmPortHandle, + int32_t microsecondPulseTime, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetPWMSpeed(HAL_DigitalHandle pwmPortHandle, double speed, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetPWMPosition(HAL_DigitalHandle pwmPortHandle, double pos, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetPWMDisabled(HAL_DigitalHandle pwmPortHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +int32_t HAL_GetPWMPulseTimeMicroseconds(HAL_DigitalHandle pwmPortHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +double HAL_GetPWMSpeed(HAL_DigitalHandle pwmPortHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +double HAL_GetPWMPosition(HAL_DigitalHandle pwmPortHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +void HAL_LatchPWMZero(HAL_DigitalHandle pwmPortHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetPWMPeriodScale(HAL_DigitalHandle pwmPortHandle, int32_t squelchMask, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetPWMAlwaysHighMode(HAL_DigitalHandle pwmPortHandle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +int32_t HAL_GetPWMLoopTiming(int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +uint64_t HAL_GetPWMCycleStartTime(int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/Ports.cpp b/hal/src/main/native/systemcore/Ports.cpp new file mode 100644 index 00000000000..d27ecbd75ca --- /dev/null +++ b/hal/src/main/native/systemcore/Ports.cpp @@ -0,0 +1,90 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/Ports.h" + +#include "PortsInternal.h" + +using namespace hal; + +namespace hal::init { +void InitializePorts() {} +} // namespace hal::init + +extern "C" { + +int32_t HAL_GetNumAccumulators(void) { + return kNumAccumulators; +} +int32_t HAL_GetNumAnalogTriggers(void) { + return kNumAnalogTriggers; +} +int32_t HAL_GetNumAnalogInputs(void) { + return kNumAnalogInputs; +} +int32_t HAL_GetNumAnalogOutputs(void) { + return kNumAnalogOutputs; +} +int32_t HAL_GetNumCounters(void) { + return kNumCounters; +} +int32_t HAL_GetNumDigitalHeaders(void) { + return kNumDigitalHeaders; +} +int32_t HAL_GetNumPWMHeaders(void) { + return kNumPWMHeaders; +} +int32_t HAL_GetNumDigitalChannels(void) { + return kNumDigitalChannels; +} +int32_t HAL_GetNumPWMChannels(void) { + return kNumPWMChannels; +} +int32_t HAL_GetNumDigitalPWMOutputs(void) { + return kNumDigitalPWMOutputs; +} +int32_t HAL_GetNumEncoders(void) { + return kNumEncoders; +} +int32_t HAL_GetNumInterrupts(void) { + return kNumInterrupts; +} +int32_t HAL_GetNumRelayChannels(void) { + return kNumRelayChannels; +} +int32_t HAL_GetNumRelayHeaders(void) { + return kNumRelayHeaders; +} +int32_t HAL_GetNumCTREPCMModules(void) { + return kNumCTREPCMModules; +} +int32_t HAL_GetNumCTRESolenoidChannels(void) { + return kNumCTRESolenoidChannels; +} +int32_t HAL_GetNumCTREPDPModules(void) { + return kNumCTREPDPModules; +} +int32_t HAL_GetNumCTREPDPChannels(void) { + return kNumCTREPDPChannels; +} +int32_t HAL_GetNumREVPDHModules(void) { + return kNumREVPDHModules; +} +int32_t HAL_GetNumREVPDHChannels(void) { + return kNumREVPDHChannels; +} +int32_t HAL_GetNumREVPHModules(void) { + return kNumREVPHModules; +} +int32_t HAL_GetNumREVPHChannels(void) { + return kNumREVPHChannels; +} +int32_t HAL_GetNumDutyCycles(void) { + return kNumDutyCycles; +} +int32_t HAL_GetNumAddressableLEDs(void) { + return kNumAddressableLEDs; +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/PortsInternal.h b/hal/src/main/native/systemcore/PortsInternal.h new file mode 100644 index 00000000000..db95f532f39 --- /dev/null +++ b/hal/src/main/native/systemcore/PortsInternal.h @@ -0,0 +1,39 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +namespace hal { + +constexpr int32_t kNumAccumulators = 0; +constexpr int32_t kNumAnalogTriggers = 0; +constexpr int32_t kNumAnalogInputs = 8; +constexpr int32_t kNumAnalogOutputs = 0; +constexpr int32_t kNumCounters = 0; +constexpr int32_t kNumDigitalHeaders = 10; +constexpr int32_t kNumDigitalMXPChannels = 16; +constexpr int32_t kNumDigitalSPIPortChannels = 5; +constexpr int32_t kNumPWMHeaders = 0; +constexpr int32_t kNumDigitalChannels = + kNumDigitalHeaders + kNumDigitalMXPChannels + kNumDigitalSPIPortChannels; +constexpr int32_t kNumPWMChannels = 0 + kNumPWMHeaders; +constexpr int32_t kNumDigitalPWMOutputs = 0; +constexpr int32_t kNumEncoders = 0; +constexpr int32_t kNumInterrupts = 0; +constexpr int32_t kNumRelayChannels = 8; +constexpr int32_t kNumRelayHeaders = kNumRelayChannels / 2; +constexpr int32_t kNumCTREPCMModules = 63; +constexpr int32_t kNumCTRESolenoidChannels = 8; +constexpr int32_t kNumCTREPDPModules = 63; +constexpr int32_t kNumCTREPDPChannels = 16; +constexpr int32_t kNumREVPDHModules = 63; +constexpr int32_t kNumREVPDHChannels = 24; +constexpr int32_t kNumDutyCycles = 0; +constexpr int32_t kNumAddressableLEDs = 0; +constexpr int32_t kNumREVPHModules = 63; +constexpr int32_t kNumREVPHChannels = 16; + +} // namespace hal diff --git a/hal/src/main/native/systemcore/Power.cpp b/hal/src/main/native/systemcore/Power.cpp new file mode 100644 index 00000000000..907167382e6 --- /dev/null +++ b/hal/src/main/native/systemcore/Power.cpp @@ -0,0 +1,154 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/Power.h" + +#include + +#include "HALInitializer.h" +#include "hal/Errors.h" + +using namespace hal; + +namespace hal { + +static void initializePower(int32_t* status) { + hal::init::CheckInit(); +} + +} // namespace hal + +namespace hal::init { +void InitializePower() {} +} // namespace hal::init + +extern "C" { + +double HAL_GetVinVoltage(int32_t* status) { + initializePower(status); + *status = HAL_HANDLE_ERROR; + return 0; +} + +double HAL_GetVinCurrent(int32_t* status) { + initializePower(status); + *status = HAL_HANDLE_ERROR; + return 0; +} + +double HAL_GetUserVoltage6V(int32_t* status) { + initializePower(status); + *status = HAL_HANDLE_ERROR; + return 0; +} + +double HAL_GetUserCurrent6V(int32_t* status) { + initializePower(status); + *status = HAL_HANDLE_ERROR; + return 0; +} + +HAL_Bool HAL_GetUserActive6V(int32_t* status) { + initializePower(status); + *status = HAL_HANDLE_ERROR; + return 0; +} + +int32_t HAL_GetUserCurrentFaults6V(int32_t* status) { + initializePower(status); + *status = HAL_HANDLE_ERROR; + return 0; +} + +void HAL_SetUserRailEnabled6V(HAL_Bool enabled, int32_t* status) { + initializePower(status); + *status = HAL_HANDLE_ERROR; + return; +} + +double HAL_GetUserVoltage5V(int32_t* status) { + initializePower(status); + *status = HAL_HANDLE_ERROR; + return 0; +} + +double HAL_GetUserCurrent5V(int32_t* status) { + initializePower(status); + *status = HAL_HANDLE_ERROR; + return 0; +} + +HAL_Bool HAL_GetUserActive5V(int32_t* status) { + initializePower(status); + *status = HAL_HANDLE_ERROR; + return 0; +} + +int32_t HAL_GetUserCurrentFaults5V(int32_t* status) { + initializePower(status); + *status = HAL_HANDLE_ERROR; + return 0; +} + +void HAL_SetUserRailEnabled5V(HAL_Bool enabled, int32_t* status) { + initializePower(status); + *status = HAL_HANDLE_ERROR; + return; +} + +double HAL_GetUserVoltage3V3(int32_t* status) { + initializePower(status); + *status = HAL_HANDLE_ERROR; + return 0; +} + +double HAL_GetUserCurrent3V3(int32_t* status) { + initializePower(status); + *status = HAL_HANDLE_ERROR; + return 0; +} + +HAL_Bool HAL_GetUserActive3V3(int32_t* status) { + initializePower(status); + *status = HAL_HANDLE_ERROR; + return 0; +} + +int32_t HAL_GetUserCurrentFaults3V3(int32_t* status) { + initializePower(status); + *status = HAL_HANDLE_ERROR; + return 0; +} + +void HAL_SetUserRailEnabled3V3(HAL_Bool enabled, int32_t* status) { + initializePower(status); + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_ResetUserCurrentFaults(int32_t* status) { + initializePower(status); + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetBrownoutVoltage(double voltage, int32_t* status) { + initializePower(status); + *status = HAL_HANDLE_ERROR; + return; +} + +double HAL_GetBrownoutVoltage(int32_t* status) { + initializePower(status); + *status = HAL_HANDLE_ERROR; + return 0; +} + +double HAL_GetCPUTemp(int32_t* status) { + initializePower(status); + *status = HAL_HANDLE_ERROR; + return 0; +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/PowerDistribution.cpp b/hal/src/main/native/systemcore/PowerDistribution.cpp new file mode 100644 index 00000000000..580aa9005a6 --- /dev/null +++ b/hal/src/main/native/systemcore/PowerDistribution.cpp @@ -0,0 +1,308 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/PowerDistribution.h" + +#include +#include + +#include "CTREPDP.h" +#include "HALInternal.h" +#include "PortsInternal.h" +#include "REVPDH.h" +#include "hal/Errors.h" +#include "hal/HALBase.h" +#include "hal/handles/HandlesInternal.h" + +using namespace hal; + +extern "C" { + +HAL_PowerDistributionHandle HAL_InitializePowerDistribution( + int32_t moduleNumber, HAL_PowerDistributionType type, + const char* allocationLocation, int32_t* status) { + if (type == HAL_PowerDistributionType::HAL_PowerDistributionType_kAutomatic) { + if (moduleNumber != HAL_DEFAULT_POWER_DISTRIBUTION_MODULE) { + *status = PARAMETER_OUT_OF_RANGE; + hal::SetLastError( + status, "Automatic PowerDistributionType must have default module"); + return HAL_kInvalidHandle; + } + + uint64_t waitTime = hal::GetDSInitializeTime() + 400000; + + // Ensure we have been alive for long enough to receive a few Power packets. + do { + uint64_t currentTime = HAL_GetFPGATime(status); + if (*status != 0) { + return HAL_kInvalidHandle; + } + if (currentTime >= waitTime) { + break; + } + std::this_thread::sleep_for( + std::chrono::microseconds(waitTime - currentTime)); + } while (true); + + // Try PDP first + auto pdpHandle = HAL_InitializePDP(0, allocationLocation, status); + if (pdpHandle != HAL_kInvalidHandle) { + *status = 0; + HAL_GetPDPVoltage(pdpHandle, status); + if (*status == 0 || *status == HAL_CAN_TIMEOUT) { + return static_cast(pdpHandle); + } + HAL_CleanPDP(pdpHandle); + } + *status = 0; + auto pdhHandle = HAL_InitializeREVPDH(1, allocationLocation, status); + return static_cast(pdhHandle); + } + + if (type == HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE) { + if (moduleNumber == HAL_DEFAULT_POWER_DISTRIBUTION_MODULE) { + moduleNumber = 0; + } + return static_cast( + HAL_InitializePDP(moduleNumber, allocationLocation, status)); + } else { + if (moduleNumber == HAL_DEFAULT_POWER_DISTRIBUTION_MODULE) { + moduleNumber = 1; + } + return static_cast( + HAL_InitializeREVPDH(moduleNumber, allocationLocation, status)); + } +} + +#define IsCtre(handle) ::hal::isHandleType(handle, HAL_HandleEnum::CTREPDP) + +void HAL_CleanPowerDistribution(HAL_PowerDistributionHandle handle) { + if (IsCtre(handle)) { + HAL_CleanPDP(handle); + } else { + HAL_FreeREVPDH(handle); + } +} + +int32_t HAL_GetPowerDistributionModuleNumber(HAL_PowerDistributionHandle handle, + int32_t* status) { + if (IsCtre(handle)) { + return HAL_GetPDPModuleNumber(handle, status); + } else { + return HAL_GetREVPDHModuleNumber(handle, status); + } +} + +HAL_Bool HAL_CheckPowerDistributionChannel(HAL_PowerDistributionHandle handle, + int32_t channel) { + if (IsCtre(handle)) { + return HAL_CheckPDPChannel(channel); + } else { + return HAL_CheckREVPDHChannelNumber(channel); + } +} + +HAL_Bool HAL_CheckPowerDistributionModule(int32_t module, + HAL_PowerDistributionType type) { + if (type == HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE) { + return HAL_CheckPDPModule(module); + } else { + return HAL_CheckREVPDHModuleNumber(module); + } +} + +HAL_PowerDistributionType HAL_GetPowerDistributionType( + HAL_PowerDistributionHandle handle, int32_t* status) { + return IsCtre(handle) + ? HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE + : HAL_PowerDistributionType::HAL_PowerDistributionType_kRev; +} + +int32_t HAL_GetPowerDistributionNumChannels(HAL_PowerDistributionHandle handle, + int32_t* status) { + if (IsCtre(handle)) { + return kNumCTREPDPChannels; + } else { + return kNumREVPDHChannels; + } +} + +double HAL_GetPowerDistributionTemperature(HAL_PowerDistributionHandle handle, + int32_t* status) { + if (IsCtre(handle)) { + return HAL_GetPDPTemperature(handle, status); + } else { + // Not supported + return 0; + } +} + +double HAL_GetPowerDistributionVoltage(HAL_PowerDistributionHandle handle, + int32_t* status) { + if (IsCtre(handle)) { + return HAL_GetPDPVoltage(handle, status); + } else { + return HAL_GetREVPDHVoltage(handle, status); + } +} + +double HAL_GetPowerDistributionChannelCurrent( + HAL_PowerDistributionHandle handle, int32_t channel, int32_t* status) { + if (IsCtre(handle)) { + return HAL_GetPDPChannelCurrent(handle, channel, status); + } else { + return HAL_GetREVPDHChannelCurrent(handle, channel, status); + } +} + +void HAL_GetPowerDistributionAllChannelCurrents( + HAL_PowerDistributionHandle handle, double* currents, + int32_t currentsLength, int32_t* status) { + if (IsCtre(handle)) { + if (currentsLength < kNumCTREPDPChannels) { + *status = PARAMETER_OUT_OF_RANGE; + SetLastError(status, "Output array not large enough"); + return; + } + return HAL_GetPDPAllChannelCurrents(handle, currents, status); + } else { + if (currentsLength < kNumREVPDHChannels) { + *status = PARAMETER_OUT_OF_RANGE; + SetLastError(status, "Output array not large enough"); + return; + } + return HAL_GetREVPDHAllChannelCurrents(handle, currents, status); + } +} + +double HAL_GetPowerDistributionTotalCurrent(HAL_PowerDistributionHandle handle, + int32_t* status) { + if (IsCtre(handle)) { + return HAL_GetPDPTotalCurrent(handle, status); + } else { + return HAL_GetREVPDHTotalCurrent(handle, status); + } +} + +double HAL_GetPowerDistributionTotalPower(HAL_PowerDistributionHandle handle, + int32_t* status) { + if (IsCtre(handle)) { + return HAL_GetPDPTotalPower(handle, status); + } else { + // Not currently supported + return 0; + } +} + +double HAL_GetPowerDistributionTotalEnergy(HAL_PowerDistributionHandle handle, + int32_t* status) { + if (IsCtre(handle)) { + return HAL_GetPDPTotalEnergy(handle, status); + } else { + // Not currently supported + return 0; + } +} + +void HAL_ResetPowerDistributionTotalEnergy(HAL_PowerDistributionHandle handle, + int32_t* status) { + if (IsCtre(handle)) { + HAL_ResetPDPTotalEnergy(handle, status); + } else { + // Not supported + } +} + +void HAL_ClearPowerDistributionStickyFaults(HAL_PowerDistributionHandle handle, + int32_t* status) { + if (IsCtre(handle)) { + HAL_ClearPDPStickyFaults(handle, status); + } else { + HAL_ClearREVPDHStickyFaults(handle, status); + } +} + +void HAL_SetPowerDistributionSwitchableChannel( + HAL_PowerDistributionHandle handle, HAL_Bool enabled, int32_t* status) { + if (IsCtre(handle)) { + // No-op on CTRE + return; + } else { + HAL_SetREVPDHSwitchableChannel(handle, enabled, status); + } +} + +HAL_Bool HAL_GetPowerDistributionSwitchableChannel( + HAL_PowerDistributionHandle handle, int32_t* status) { + if (IsCtre(handle)) { + // No-op on CTRE + return false; + } else { + return HAL_GetREVPDHSwitchableChannelState(handle, status); + } +} + +void HAL_GetPowerDistributionVersion(HAL_PowerDistributionHandle handle, + HAL_PowerDistributionVersion* version, + int32_t* status) { + if (IsCtre(handle)) { + std::memset(version, 0, sizeof(*version)); + } else { + HAL_GetREVPDHVersion(handle, version, status); + } +} + +void HAL_GetPowerDistributionFaults(HAL_PowerDistributionHandle handle, + HAL_PowerDistributionFaults* faults, + int32_t* status) { + if (IsCtre(handle)) { + std::memset(faults, 0, sizeof(*faults)); + } else { + HAL_GetREVPDHFaults(handle, faults, status); + } +} + +void HAL_GetPowerDistributionStickyFaults( + HAL_PowerDistributionHandle handle, + HAL_PowerDistributionStickyFaults* stickyFaults, int32_t* status) { + if (IsCtre(handle)) { + std::memset(stickyFaults, 0, sizeof(*stickyFaults)); + } else { + HAL_GetREVPDHStickyFaults(handle, stickyFaults, status); + } +} + +void HAL_StartPowerDistributionStream(HAL_PowerDistributionHandle handle, + int32_t* status) { + if (IsCtre(handle)) { + HAL_StartPDPStream(handle, status); + } else { + HAL_StartREVPDHStream(handle, status); + } +} + +HAL_PowerDistributionChannelData* HAL_GetPowerDistributionStreamData( + HAL_PowerDistributionHandle handle, int32_t* count, int32_t* status) { + if (IsCtre(handle)) { + return HAL_GetPDPStreamData(handle, count, status); + } else { + return HAL_GetREVPDHStreamData(handle, count, status); + } +} + +void HAL_FreePowerDistributionStreamData(HAL_PowerDistributionChannelData* data, + int32_t count) { + delete[] data; +} + +void HAL_StopPowerDistributionStream(HAL_PowerDistributionHandle handle, + int32_t* status) { + if (IsCtre(handle)) { + HAL_StopPDPStream(handle, status); + } else { + HAL_StopREVPDHStream(handle, status); + } +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/REVPDH.cpp b/hal/src/main/native/systemcore/REVPDH.cpp new file mode 100644 index 00000000000..fc9f7c1778d --- /dev/null +++ b/hal/src/main/native/systemcore/REVPDH.cpp @@ -0,0 +1,915 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "REVPDH.h" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include "HALInitializer.h" +#include "HALInternal.h" +#include "PortsInternal.h" +#include "rev/PDHFrames.h" + +using namespace hal; + +static constexpr HAL_CANManufacturer manufacturer = + HAL_CANManufacturer::HAL_CAN_Man_kREV; + +static constexpr HAL_CANDeviceType deviceType = + HAL_CANDeviceType::HAL_CAN_Dev_kPowerDistribution; + +static constexpr int32_t kDefaultControlPeriod = 50; + +namespace { + +struct REV_PDHObj { + int32_t controlPeriod; + HAL_CANHandle hcan; + std::string previousAllocation; + HAL_PowerDistributionVersion versionInfo; + bool streamHandleAllocated{false}; + uint32_t streamSessionHandles[4]; +}; + +} // namespace + +static constexpr uint32_t APIFromExtId(uint32_t extId) { + return (extId >> 6) & 0x3FF; +} + +static constexpr uint32_t PDH_SET_SWITCH_CHANNEL_FRAME_API = + APIFromExtId(PDH_SET_SWITCH_CHANNEL_FRAME_ID); + +static constexpr uint32_t PDH_STATUS_0_FRAME_API = + APIFromExtId(PDH_STATUS_0_FRAME_ID); +static constexpr uint32_t PDH_STATUS_1_FRAME_API = + APIFromExtId(PDH_STATUS_1_FRAME_ID); +static constexpr uint32_t PDH_STATUS_2_FRAME_API = + APIFromExtId(PDH_STATUS_2_FRAME_ID); +static constexpr uint32_t PDH_STATUS_3_FRAME_API = + APIFromExtId(PDH_STATUS_3_FRAME_ID); +static constexpr uint32_t PDH_STATUS_4_FRAME_API = + APIFromExtId(PDH_STATUS_4_FRAME_ID); + +static constexpr uint32_t PDH_CLEAR_FAULTS_FRAME_API = + APIFromExtId(PDH_CLEAR_FAULTS_FRAME_ID); + +static constexpr uint32_t PDH_VERSION_FRAME_API = + APIFromExtId(PDH_VERSION_FRAME_ID); + +static constexpr int32_t kPDHFrameStatus0Timeout = 20; +static constexpr int32_t kPDHFrameStatus1Timeout = 20; +static constexpr int32_t kPDHFrameStatus2Timeout = 20; +static constexpr int32_t kPDHFrameStatus3Timeout = 20; +static constexpr int32_t kPDHFrameStatus4Timeout = 20; + +static IndexedHandleResource* REVPDHHandles; + +namespace hal::init { +void InitializeREVPDH() { + static IndexedHandleResource + rH; + REVPDHHandles = &rH; +} +} // namespace hal::init + +extern "C" { + +static PDH_status_0_t HAL_ReadREVPDHStatus0(HAL_CANHandle hcan, + int32_t* status) { + uint8_t packedData[8] = {0}; + int32_t length = 0; + uint64_t timestamp = 0; + PDH_status_0_t result = {}; + + HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_0_FRAME_API, packedData, &length, + ×tamp, kPDHFrameStatus0Timeout * 2, status); + + if (*status != 0) { + return result; + } + + PDH_status_0_unpack(&result, packedData, PDH_STATUS_0_LENGTH); + + return result; +} + +static PDH_status_1_t HAL_ReadREVPDHStatus1(HAL_CANHandle hcan, + int32_t* status) { + uint8_t packedData[8] = {0}; + int32_t length = 0; + uint64_t timestamp = 0; + PDH_status_1_t result = {}; + + HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_1_FRAME_API, packedData, &length, + ×tamp, kPDHFrameStatus1Timeout * 2, status); + + if (*status != 0) { + return result; + } + + PDH_status_1_unpack(&result, packedData, PDH_STATUS_1_LENGTH); + + return result; +} + +static PDH_status_2_t HAL_ReadREVPDHStatus2(HAL_CANHandle hcan, + int32_t* status) { + uint8_t packedData[8] = {0}; + int32_t length = 0; + uint64_t timestamp = 0; + PDH_status_2_t result = {}; + + HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_2_FRAME_API, packedData, &length, + ×tamp, kPDHFrameStatus2Timeout * 2, status); + + if (*status != 0) { + return result; + } + + PDH_status_2_unpack(&result, packedData, PDH_STATUS_2_LENGTH); + + return result; +} + +static PDH_status_3_t HAL_ReadREVPDHStatus3(HAL_CANHandle hcan, + int32_t* status) { + uint8_t packedData[8] = {0}; + int32_t length = 0; + uint64_t timestamp = 0; + PDH_status_3_t result = {}; + + HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_3_FRAME_API, packedData, &length, + ×tamp, kPDHFrameStatus3Timeout * 2, status); + + if (*status != 0) { + return result; + } + + PDH_status_3_unpack(&result, packedData, PDH_STATUS_3_LENGTH); + + return result; +} + +static PDH_status_4_t HAL_ReadREVPDHStatus4(HAL_CANHandle hcan, + int32_t* status) { + uint8_t packedData[8] = {0}; + int32_t length = 0; + uint64_t timestamp = 0; + PDH_status_4_t result = {}; + + HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_4_FRAME_API, packedData, &length, + ×tamp, kPDHFrameStatus4Timeout * 2, status); + + if (*status != 0) { + return result; + } + + PDH_status_4_unpack(&result, packedData, PDH_STATUS_4_LENGTH); + + return result; +} + +/** + * Helper function for the individual getter functions for status 4 + */ +PDH_status_4_t HAL_GetREVPDHStatus4(HAL_REVPDHHandle handle, int32_t* status) { + PDH_status_4_t statusFrame = {}; + auto hpdh = REVPDHHandles->Get(handle); + if (hpdh == nullptr) { + *status = HAL_HANDLE_ERROR; + return statusFrame; + } + + statusFrame = HAL_ReadREVPDHStatus4(hpdh->hcan, status); + return statusFrame; +} + +HAL_REVPDHHandle HAL_InitializeREVPDH(int32_t module, + const char* allocationLocation, + int32_t* status) { + hal::init::CheckInit(); + if (!HAL_CheckREVPDHModuleNumber(module)) { + *status = RESOURCE_OUT_OF_RANGE; + hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PDH", 1, + kNumREVPDHModules, module); + return HAL_kInvalidHandle; + } + + HAL_REVPDHHandle handle; + // Module starts at 1 + auto hpdh = REVPDHHandles->Allocate(module - 1, &handle, status); + if (*status != 0) { + if (hpdh) { + hal::SetLastErrorPreviouslyAllocated(status, "REV PDH", module, + hpdh->previousAllocation); + } else { + hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PDH", 1, + kNumREVPDHModules, module); + } + return HAL_kInvalidHandle; // failed to allocate. Pass error back. + } + + HAL_CANHandle hcan = + HAL_InitializeCAN(manufacturer, module, deviceType, status); + + if (*status != 0) { + REVPDHHandles->Free(handle); + return HAL_kInvalidHandle; + } + + hpdh->previousAllocation = allocationLocation ? allocationLocation : ""; + hpdh->hcan = hcan; + hpdh->controlPeriod = kDefaultControlPeriod; + std::memset(&hpdh->versionInfo, 0, sizeof(hpdh->versionInfo)); + + return handle; +} + +void HAL_FreeREVPDH(HAL_REVPDHHandle handle) { + auto hpdh = REVPDHHandles->Get(handle); + if (hpdh == nullptr) { + return; + } + + HAL_CleanCAN(hpdh->hcan); + + REVPDHHandles->Free(handle); +} + +int32_t HAL_GetREVPDHModuleNumber(HAL_REVPDHHandle handle, int32_t* status) { + return hal::getHandleIndex(handle); +} + +HAL_Bool HAL_CheckREVPDHModuleNumber(int32_t module) { + return ((module >= 1) && (module <= kNumREVPDHModules)) ? 1 : 0; +} + +HAL_Bool HAL_CheckREVPDHChannelNumber(int32_t channel) { + return ((channel >= 0) && (channel < kNumREVPDHChannels)) ? 1 : 0; +} + +double HAL_GetREVPDHChannelCurrent(HAL_REVPDHHandle handle, int32_t channel, + int32_t* status) { + auto hpdh = REVPDHHandles->Get(handle); + if (hpdh == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + if (!HAL_CheckREVPDHChannelNumber(channel)) { + *status = RESOURCE_OUT_OF_RANGE; + return 0; + } + + // Determine what periodic status the channel is in + if (channel < 6) { + // Periodic status 0 + PDH_status_0_t statusFrame = HAL_ReadREVPDHStatus0(hpdh->hcan, status); + switch (channel) { + case 0: + return PDH_status_0_channel_0_current_decode( + statusFrame.channel_0_current); + case 1: + return PDH_status_0_channel_1_current_decode( + statusFrame.channel_1_current); + case 2: + return PDH_status_0_channel_2_current_decode( + statusFrame.channel_2_current); + case 3: + return PDH_status_0_channel_3_current_decode( + statusFrame.channel_3_current); + case 4: + return PDH_status_0_channel_4_current_decode( + statusFrame.channel_4_current); + case 5: + return PDH_status_0_channel_5_current_decode( + statusFrame.channel_5_current); + } + } else if (channel < 12) { + // Periodic status 1 + PDH_status_1_t statusFrame = HAL_ReadREVPDHStatus1(hpdh->hcan, status); + switch (channel) { + case 6: + return PDH_status_1_channel_6_current_decode( + statusFrame.channel_6_current); + case 7: + return PDH_status_1_channel_7_current_decode( + statusFrame.channel_7_current); + case 8: + return PDH_status_1_channel_8_current_decode( + statusFrame.channel_8_current); + case 9: + return PDH_status_1_channel_9_current_decode( + statusFrame.channel_9_current); + case 10: + return PDH_status_1_channel_10_current_decode( + statusFrame.channel_10_current); + case 11: + return PDH_status_1_channel_11_current_decode( + statusFrame.channel_11_current); + } + } else if (channel < 18) { + // Periodic status 2 + PDH_status_2_t statusFrame = HAL_ReadREVPDHStatus2(hpdh->hcan, status); + switch (channel) { + case 12: + return PDH_status_2_channel_12_current_decode( + statusFrame.channel_12_current); + case 13: + return PDH_status_2_channel_13_current_decode( + statusFrame.channel_13_current); + case 14: + return PDH_status_2_channel_14_current_decode( + statusFrame.channel_14_current); + case 15: + return PDH_status_2_channel_15_current_decode( + statusFrame.channel_15_current); + case 16: + return PDH_status_2_channel_16_current_decode( + statusFrame.channel_16_current); + case 17: + return PDH_status_2_channel_17_current_decode( + statusFrame.channel_17_current); + } + } else if (channel < 24) { + // Periodic status 3 + PDH_status_3_t statusFrame = HAL_ReadREVPDHStatus3(hpdh->hcan, status); + switch (channel) { + case 18: + return PDH_status_3_channel_18_current_decode( + statusFrame.channel_18_current); + case 19: + return PDH_status_3_channel_19_current_decode( + statusFrame.channel_19_current); + case 20: + return PDH_status_3_channel_20_current_decode( + statusFrame.channel_20_current); + case 21: + return PDH_status_3_channel_21_current_decode( + statusFrame.channel_21_current); + case 22: + return PDH_status_3_channel_22_current_decode( + statusFrame.channel_22_current); + case 23: + return PDH_status_3_channel_23_current_decode( + statusFrame.channel_23_current); + } + } + return 0; +} + +void HAL_GetREVPDHAllChannelCurrents(HAL_REVPDHHandle handle, double* currents, + int32_t* status) { + auto hpdh = REVPDHHandles->Get(handle); + if (hpdh == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + PDH_status_0_t statusFrame0 = HAL_ReadREVPDHStatus0(hpdh->hcan, status); + PDH_status_1_t statusFrame1 = HAL_ReadREVPDHStatus1(hpdh->hcan, status); + PDH_status_2_t statusFrame2 = HAL_ReadREVPDHStatus2(hpdh->hcan, status); + PDH_status_3_t statusFrame3 = HAL_ReadREVPDHStatus3(hpdh->hcan, status); + + currents[0] = + PDH_status_0_channel_0_current_decode(statusFrame0.channel_0_current); + currents[1] = + PDH_status_0_channel_1_current_decode(statusFrame0.channel_1_current); + currents[2] = + PDH_status_0_channel_2_current_decode(statusFrame0.channel_2_current); + currents[3] = + PDH_status_0_channel_3_current_decode(statusFrame0.channel_3_current); + currents[4] = + PDH_status_0_channel_4_current_decode(statusFrame0.channel_4_current); + currents[5] = + PDH_status_0_channel_5_current_decode(statusFrame0.channel_5_current); + currents[6] = + PDH_status_1_channel_6_current_decode(statusFrame1.channel_6_current); + currents[7] = + PDH_status_1_channel_7_current_decode(statusFrame1.channel_7_current); + currents[8] = + PDH_status_1_channel_8_current_decode(statusFrame1.channel_8_current); + currents[9] = + PDH_status_1_channel_9_current_decode(statusFrame1.channel_9_current); + currents[10] = + PDH_status_1_channel_10_current_decode(statusFrame1.channel_10_current); + currents[11] = + PDH_status_1_channel_11_current_decode(statusFrame1.channel_11_current); + currents[12] = + PDH_status_2_channel_12_current_decode(statusFrame2.channel_12_current); + currents[13] = + PDH_status_2_channel_13_current_decode(statusFrame2.channel_13_current); + currents[14] = + PDH_status_2_channel_14_current_decode(statusFrame2.channel_14_current); + currents[15] = + PDH_status_2_channel_15_current_decode(statusFrame2.channel_15_current); + currents[16] = + PDH_status_2_channel_16_current_decode(statusFrame2.channel_16_current); + currents[17] = + PDH_status_2_channel_17_current_decode(statusFrame2.channel_17_current); + currents[18] = + PDH_status_3_channel_18_current_decode(statusFrame3.channel_18_current); + currents[19] = + PDH_status_3_channel_19_current_decode(statusFrame3.channel_19_current); + currents[20] = + PDH_status_3_channel_20_current_decode(statusFrame3.channel_20_current); + currents[21] = + PDH_status_3_channel_21_current_decode(statusFrame3.channel_21_current); + currents[22] = + PDH_status_3_channel_22_current_decode(statusFrame3.channel_22_current); + currents[23] = + PDH_status_3_channel_23_current_decode(statusFrame3.channel_23_current); +} + +uint16_t HAL_GetREVPDHTotalCurrent(HAL_REVPDHHandle handle, int32_t* status) { + PDH_status_4_t statusFrame = HAL_GetREVPDHStatus4(handle, status); + + if (*status != 0) { + return 0; + } + + return PDH_status_4_total_current_decode(statusFrame.total_current); +} + +void HAL_SetREVPDHSwitchableChannel(HAL_REVPDHHandle handle, HAL_Bool enabled, + int32_t* status) { + auto hpdh = REVPDHHandles->Get(handle); + if (hpdh == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + uint8_t packedData[8] = {0}; + PDH_set_switch_channel_t frame; + frame.output_set_value = enabled; + PDH_set_switch_channel_pack(packedData, &frame, + PDH_SET_SWITCH_CHANNEL_LENGTH); + + HAL_WriteCANPacket(hpdh->hcan, packedData, PDH_SET_SWITCH_CHANNEL_LENGTH, + PDH_SET_SWITCH_CHANNEL_FRAME_API, status); +} + +HAL_Bool HAL_GetREVPDHSwitchableChannelState(HAL_REVPDHHandle handle, + int32_t* status) { + PDH_status_4_t statusFrame = HAL_GetREVPDHStatus4(handle, status); + + if (*status != 0) { + return 0.0; + } + + return PDH_status_4_switch_channel_state_decode( + statusFrame.switch_channel_state); +} + +double HAL_GetREVPDHVoltage(HAL_REVPDHHandle handle, int32_t* status) { + PDH_status_4_t statusFrame = HAL_GetREVPDHStatus4(handle, status); + + if (*status != 0) { + return 0.0; + } + + return PDH_status_4_v_bus_decode(statusFrame.v_bus); +} + +void HAL_GetREVPDHVersion(HAL_REVPDHHandle handle, + HAL_PowerDistributionVersion* version, + int32_t* status) { + std::memset(version, 0, sizeof(*version)); + uint8_t packedData[8] = {0}; + int32_t length = 0; + uint64_t timestamp = 0; + PDH_version_t result = {}; + auto hpdh = REVPDHHandles->Get(handle); + if (hpdh == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + if (hpdh->versionInfo.firmwareMajor > 0) { + version->firmwareMajor = hpdh->versionInfo.firmwareMajor; + version->firmwareMinor = hpdh->versionInfo.firmwareMinor; + version->firmwareFix = hpdh->versionInfo.firmwareFix; + version->hardwareMajor = hpdh->versionInfo.hardwareMajor; + version->hardwareMinor = hpdh->versionInfo.hardwareMinor; + version->uniqueId = hpdh->versionInfo.uniqueId; + + *status = 0; + return; + } + + HAL_WriteCANRTRFrame(hpdh->hcan, PDH_VERSION_LENGTH, PDH_VERSION_FRAME_API, + status); + + if (*status != 0) { + return; + } + + uint32_t timeoutMs = 100; + for (uint32_t i = 0; i <= timeoutMs; i++) { + HAL_ReadCANPacketNew(hpdh->hcan, PDH_VERSION_FRAME_API, packedData, &length, + ×tamp, status); + if (*status == 0) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + + if (*status != 0) { + return; + } + + PDH_version_unpack(&result, packedData, PDH_VERSION_LENGTH); + + version->firmwareMajor = result.firmware_year; + version->firmwareMinor = result.firmware_minor; + version->firmwareFix = result.firmware_fix; + version->hardwareMinor = result.hardware_minor; + version->hardwareMajor = result.hardware_major; + version->uniqueId = result.unique_id; + + hpdh->versionInfo = *version; +} + +void HAL_GetREVPDHFaults(HAL_REVPDHHandle handle, + HAL_PowerDistributionFaults* faults, int32_t* status) { + std::memset(faults, 0, sizeof(*faults)); + auto hpdh = REVPDHHandles->Get(handle); + if (hpdh == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + PDH_status_0_t status0 = HAL_ReadREVPDHStatus0(hpdh->hcan, status); + PDH_status_1_t status1 = HAL_ReadREVPDHStatus1(hpdh->hcan, status); + PDH_status_2_t status2 = HAL_ReadREVPDHStatus2(hpdh->hcan, status); + PDH_status_3_t status3 = HAL_ReadREVPDHStatus3(hpdh->hcan, status); + PDH_status_4_t status4 = HAL_ReadREVPDHStatus4(hpdh->hcan, status); + + faults->channel0BreakerFault = status0.channel_0_breaker_fault; + faults->channel1BreakerFault = status0.channel_1_breaker_fault; + faults->channel2BreakerFault = status0.channel_2_breaker_fault; + faults->channel3BreakerFault = status0.channel_3_breaker_fault; + faults->channel4BreakerFault = status1.channel_4_breaker_fault; + faults->channel5BreakerFault = status1.channel_5_breaker_fault; + faults->channel6BreakerFault = status1.channel_6_breaker_fault; + faults->channel7BreakerFault = status1.channel_7_breaker_fault; + faults->channel8BreakerFault = status2.channel_8_breaker_fault; + faults->channel9BreakerFault = status2.channel_9_breaker_fault; + faults->channel10BreakerFault = status2.channel_10_breaker_fault; + faults->channel11BreakerFault = status2.channel_11_breaker_fault; + faults->channel12BreakerFault = status3.channel_12_breaker_fault; + faults->channel13BreakerFault = status3.channel_13_breaker_fault; + faults->channel14BreakerFault = status3.channel_14_breaker_fault; + faults->channel15BreakerFault = status3.channel_15_breaker_fault; + faults->channel16BreakerFault = status3.channel_16_breaker_fault; + faults->channel17BreakerFault = status3.channel_17_breaker_fault; + faults->channel18BreakerFault = status3.channel_18_breaker_fault; + faults->channel19BreakerFault = status3.channel_19_breaker_fault; + faults->channel20BreakerFault = status3.channel_20_breaker_fault; + faults->channel21BreakerFault = status3.channel_21_breaker_fault; + faults->channel22BreakerFault = status3.channel_22_breaker_fault; + faults->channel23BreakerFault = status3.channel_23_breaker_fault; + faults->brownout = status4.brownout_fault; + faults->canWarning = status4.can_warning_fault; + faults->hardwareFault = status4.hardware_fault; +} + +void HAL_GetREVPDHStickyFaults(HAL_REVPDHHandle handle, + HAL_PowerDistributionStickyFaults* stickyFaults, + int32_t* status) { + std::memset(stickyFaults, 0, sizeof(*stickyFaults)); + auto hpdh = REVPDHHandles->Get(handle); + if (hpdh == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + PDH_status_4_t status4 = HAL_ReadREVPDHStatus4(hpdh->hcan, status); + + stickyFaults->channel0BreakerFault = status4.sticky_ch0_breaker_fault; + stickyFaults->channel1BreakerFault = status4.sticky_ch1_breaker_fault; + stickyFaults->channel2BreakerFault = status4.sticky_ch2_breaker_fault; + stickyFaults->channel3BreakerFault = status4.sticky_ch3_breaker_fault; + stickyFaults->channel4BreakerFault = status4.sticky_ch4_breaker_fault; + stickyFaults->channel5BreakerFault = status4.sticky_ch5_breaker_fault; + stickyFaults->channel6BreakerFault = status4.sticky_ch6_breaker_fault; + stickyFaults->channel7BreakerFault = status4.sticky_ch7_breaker_fault; + stickyFaults->channel8BreakerFault = status4.sticky_ch8_breaker_fault; + stickyFaults->channel9BreakerFault = status4.sticky_ch9_breaker_fault; + stickyFaults->channel10BreakerFault = status4.sticky_ch10_breaker_fault; + stickyFaults->channel11BreakerFault = status4.sticky_ch11_breaker_fault; + stickyFaults->channel12BreakerFault = status4.sticky_ch12_breaker_fault; + stickyFaults->channel13BreakerFault = status4.sticky_ch13_breaker_fault; + stickyFaults->channel14BreakerFault = status4.sticky_ch14_breaker_fault; + stickyFaults->channel15BreakerFault = status4.sticky_ch15_breaker_fault; + stickyFaults->channel16BreakerFault = status4.sticky_ch16_breaker_fault; + stickyFaults->channel17BreakerFault = status4.sticky_ch17_breaker_fault; + stickyFaults->channel18BreakerFault = status4.sticky_ch18_breaker_fault; + stickyFaults->channel19BreakerFault = status4.sticky_ch19_breaker_fault; + stickyFaults->channel20BreakerFault = status4.sticky_ch20_breaker_fault; + stickyFaults->channel21BreakerFault = status4.sticky_ch21_breaker_fault; + stickyFaults->channel22BreakerFault = status4.sticky_ch22_breaker_fault; + stickyFaults->channel23BreakerFault = status4.sticky_ch23_breaker_fault; + stickyFaults->brownout = status4.sticky_brownout_fault; + stickyFaults->canWarning = status4.sticky_can_warning_fault; + stickyFaults->canBusOff = status4.sticky_can_bus_off_fault; + stickyFaults->hardwareFault = status4.sticky_hardware_fault; + stickyFaults->firmwareFault = status4.sticky_firmware_fault; + stickyFaults->hasReset = status4.sticky_has_reset_fault; +} + +void HAL_ClearREVPDHStickyFaults(HAL_REVPDHHandle handle, int32_t* status) { + auto hpdh = REVPDHHandles->Get(handle); + if (hpdh == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + uint8_t packedData[8] = {0}; + HAL_WriteCANPacket(hpdh->hcan, packedData, PDH_CLEAR_FAULTS_LENGTH, + PDH_CLEAR_FAULTS_FRAME_API, status); +} + +uint32_t HAL_StartCANStream(HAL_CANHandle handle, int32_t apiId, int32_t depth, + int32_t* status); + +void HAL_StartREVPDHStream(HAL_REVPDHHandle handle, int32_t* status) { + auto hpdh = REVPDHHandles->Get(handle); + if (hpdh == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + if (hpdh->streamHandleAllocated) { + *status = RESOURCE_IS_ALLOCATED; + return; + } + + hpdh->streamSessionHandles[0] = + HAL_StartCANStream(hpdh->hcan, PDH_STATUS_0_FRAME_API, 50, status); + if (*status != 0) { + return; + } + hpdh->streamSessionHandles[1] = + HAL_StartCANStream(hpdh->hcan, PDH_STATUS_1_FRAME_API, 50, status); + if (*status != 0) { + HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[0]); + return; + } + hpdh->streamSessionHandles[2] = + HAL_StartCANStream(hpdh->hcan, PDH_STATUS_2_FRAME_API, 50, status); + if (*status != 0) { + HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[0]); + HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[1]); + return; + } + hpdh->streamSessionHandles[3] = + HAL_StartCANStream(hpdh->hcan, PDH_STATUS_3_FRAME_API, 50, status); + if (*status != 0) { + HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[0]); + HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[1]); + HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[3]); + return; + } + hpdh->streamHandleAllocated = true; +} + +HAL_PowerDistributionChannelData* HAL_GetREVPDHStreamData( + HAL_REVPDHHandle handle, int32_t* count, int32_t* status) { + auto hpdh = REVPDHHandles->Get(handle); + if (hpdh == nullptr) { + *status = HAL_HANDLE_ERROR; + return nullptr; + } + + if (!hpdh->streamHandleAllocated) { + *status = RESOURCE_OUT_OF_RANGE; + return nullptr; + } + + *count = 0; + // 4 streams, 6 channels per stream, 50 depth per stream + HAL_PowerDistributionChannelData* retData = + new HAL_PowerDistributionChannelData[4 * 6 * 50]; + + HAL_CANStreamMessage messages[50]; + uint32_t messagesRead = 0; + HAL_CAN_ReadStreamSession(hpdh->streamSessionHandles[0], messages, 50, + &messagesRead, status); + if (*status < 0) { + goto Exit; + } + + for (uint32_t i = 0; i < messagesRead; i++) { + PDH_status_0_t statusFrame0; + PDH_status_0_unpack(&statusFrame0, messages[i].data, PDH_STATUS_0_LENGTH); + uint32_t timestamp = messages[i].timeStamp; + + retData[*count].current = + PDH_status_0_channel_0_current_decode(statusFrame0.channel_0_current); + retData[*count].channel = 1; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + PDH_status_0_channel_1_current_decode(statusFrame0.channel_1_current); + retData[*count].channel = 2; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + PDH_status_0_channel_2_current_decode(statusFrame0.channel_2_current); + retData[*count].channel = 3; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + PDH_status_0_channel_3_current_decode(statusFrame0.channel_3_current); + retData[*count].channel = 4; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + PDH_status_0_channel_4_current_decode(statusFrame0.channel_4_current); + retData[*count].channel = 5; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + PDH_status_0_channel_5_current_decode(statusFrame0.channel_5_current); + retData[*count].channel = 6; + retData[*count].timestamp = timestamp; + (*count)++; + } + + messagesRead = 0; + HAL_CAN_ReadStreamSession(hpdh->streamSessionHandles[1], messages, 50, + &messagesRead, status); + if (*status < 0) { + goto Exit; + } + + for (uint32_t i = 0; i < messagesRead; i++) { + PDH_status_1_t statusFrame1; + PDH_status_1_unpack(&statusFrame1, messages[i].data, PDH_STATUS_1_LENGTH); + uint32_t timestamp = messages[i].timeStamp; + + retData[*count].current = + PDH_status_1_channel_6_current_decode(statusFrame1.channel_6_current); + retData[*count].channel = 7; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + PDH_status_1_channel_7_current_decode(statusFrame1.channel_7_current); + retData[*count].channel = 8; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + PDH_status_1_channel_8_current_decode(statusFrame1.channel_8_current); + retData[*count].channel = 9; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + PDH_status_1_channel_9_current_decode(statusFrame1.channel_9_current); + retData[*count].channel = 10; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + PDH_status_1_channel_10_current_decode(statusFrame1.channel_10_current); + retData[*count].channel = 11; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + PDH_status_1_channel_11_current_decode(statusFrame1.channel_11_current); + retData[*count].channel = 12; + retData[*count].timestamp = timestamp; + (*count)++; + } + + messagesRead = 0; + HAL_CAN_ReadStreamSession(hpdh->streamSessionHandles[2], messages, 50, + &messagesRead, status); + if (*status < 0) { + goto Exit; + } + + for (uint32_t i = 0; i < messagesRead; i++) { + PDH_status_2_t statusFrame2; + PDH_status_2_unpack(&statusFrame2, messages[i].data, PDH_STATUS_2_LENGTH); + uint32_t timestamp = messages[i].timeStamp; + + retData[*count].current = + PDH_status_2_channel_12_current_decode(statusFrame2.channel_12_current); + retData[*count].channel = 13; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + PDH_status_2_channel_13_current_decode(statusFrame2.channel_13_current); + retData[*count].channel = 14; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + PDH_status_2_channel_14_current_decode(statusFrame2.channel_14_current); + retData[*count].channel = 15; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + PDH_status_2_channel_15_current_decode(statusFrame2.channel_15_current); + retData[*count].channel = 16; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + PDH_status_2_channel_16_current_decode(statusFrame2.channel_16_current); + retData[*count].channel = 17; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + PDH_status_2_channel_17_current_decode(statusFrame2.channel_17_current); + retData[*count].channel = 18; + retData[*count].timestamp = timestamp; + (*count)++; + } + + messagesRead = 0; + HAL_CAN_ReadStreamSession(hpdh->streamSessionHandles[3], messages, 50, + &messagesRead, status); + if (*status < 0) { + goto Exit; + } + + for (uint32_t i = 0; i < messagesRead; i++) { + PDH_status_3_t statusFrame3; + PDH_status_3_unpack(&statusFrame3, messages[i].data, PDH_STATUS_3_LENGTH); + uint32_t timestamp = messages[i].timeStamp; + + retData[*count].current = + PDH_status_3_channel_18_current_decode(statusFrame3.channel_18_current); + retData[*count].channel = 19; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + PDH_status_3_channel_19_current_decode(statusFrame3.channel_19_current); + retData[*count].channel = 20; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + PDH_status_3_channel_20_current_decode(statusFrame3.channel_20_current); + retData[*count].channel = 21; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + PDH_status_3_channel_21_current_decode(statusFrame3.channel_21_current); + retData[*count].channel = 22; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + PDH_status_3_channel_22_current_decode(statusFrame3.channel_22_current); + retData[*count].channel = 23; + retData[*count].timestamp = timestamp; + (*count)++; + retData[*count].current = + PDH_status_3_channel_23_current_decode(statusFrame3.channel_23_current); + retData[*count].channel = 24; + retData[*count].timestamp = timestamp; + (*count)++; + } + +Exit: + if (*status < 0) { + delete[] retData; + retData = nullptr; + } + return retData; +} + +void HAL_StopREVPDHStream(HAL_REVPDHHandle handle, int32_t* status) { + auto hpdh = REVPDHHandles->Get(handle); + if (hpdh == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + if (!hpdh->streamHandleAllocated) { + *status = RESOURCE_OUT_OF_RANGE; + return; + } + + HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[0]); + HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[1]); + HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[2]); + HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[3]); + + hpdh->streamHandleAllocated = false; +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/REVPDH.h b/hal/src/main/native/systemcore/REVPDH.h new file mode 100644 index 00000000000..0046455339d --- /dev/null +++ b/hal/src/main/native/systemcore/REVPDH.h @@ -0,0 +1,170 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "hal/PowerDistribution.h" +#include "hal/Types.h" + +/** + * @defgroup hal_rev_pdh REV Power Distribution Hub API Functions + * @ingroup hal_capi + * @{ + */ + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Initializes a REV Power Distribution Hub (PDH) device. + * + * @param module the device CAN ID (1 .. 63) + * @return the created PDH handle + */ +HAL_REVPDHHandle HAL_InitializeREVPDH(int32_t module, + const char* allocationLocation, + int32_t* status); + +/** + * Frees a PDH device handle. + * + * @param handle the previously created PDH handle + */ +void HAL_FreeREVPDH(HAL_REVPDHHandle handle); + +/** + * Gets the module number for a pdh. + */ +int32_t HAL_GetREVPDHModuleNumber(HAL_REVPDHHandle handle, int32_t* status); + +/** + * Checks if a PDH module number is valid. + * + * Does not check if a PDH device with this module has been initialized. + * + * @param module module number (1 .. 63) + * @return 1 if the module number is valid; 0 otherwise + */ +HAL_Bool HAL_CheckREVPDHModuleNumber(int32_t module); + +/** + * Checks if a PDH channel number is valid. + * + * @param module channel number (0 .. kNumREVPDHChannels) + * @return 1 if the channel number is valid; 0 otherwise + */ +HAL_Bool HAL_CheckREVPDHChannelNumber(int32_t channel); + +/** + * Gets the current of a PDH channel in Amps. + * + * @param handle PDH handle + * @param channel the channel to retrieve the current of (0 .. + * kNumREVPDHChannels) + * + * @return the current of the PDH channel in Amps + */ +double HAL_GetREVPDHChannelCurrent(HAL_REVPDHHandle handle, int32_t channel, + int32_t* status); + +/** + * @param handle PDH handle + * @param currents array of currents + */ +void HAL_GetREVPDHAllChannelCurrents(HAL_REVPDHHandle handle, double* currents, + int32_t* status); + +/** + * Gets the total current of the PDH in Amps, measured to the nearest even + * integer. + * + * @param handle PDH handle + * + * @return the total current of the PDH in Amps + */ +uint16_t HAL_GetREVPDHTotalCurrent(HAL_REVPDHHandle handle, int32_t* status); + +/** + * Sets the state of the switchable channel on a PDH device. + * + * @param handle PDH handle + * @param enabled 1 if the switchable channel should be enabled; 0 + * otherwise + */ +void HAL_SetREVPDHSwitchableChannel(HAL_REVPDHHandle handle, HAL_Bool enabled, + int32_t* status); + +/** + * Gets the current state of the switchable channel on a PDH device. + * + * This call relies on a periodic status sent by the PDH device and will be as + * fresh as the last packet received. + * + * @param handle PDH handle + * @return 1 if the switchable channel is enabled; 0 otherwise + */ +HAL_Bool HAL_GetREVPDHSwitchableChannelState(HAL_REVPDHHandle handle, + int32_t* status); + +/** + * Gets the firmware and hardware versions of a PDH device. + * + * @param handle PDH handle + * + * @return version information + */ +void HAL_GetREVPDHVersion(HAL_REVPDHHandle handle, + HAL_PowerDistributionVersion* version, + int32_t* status); + +/** + * Gets the voltage being supplied to a PDH device. + * + * @param handle PDH handle + * + * @return the voltage at the input of the PDH in Volts + */ +double HAL_GetREVPDHVoltage(HAL_REVPDHHandle handle, int32_t* status); + +/** + * Gets the faults of a PDH device. + * + * @param handle PDH handle + * + * @return the faults of the PDH + */ +void HAL_GetREVPDHFaults(HAL_REVPDHHandle handle, + HAL_PowerDistributionFaults* faults, int32_t* status); + +/** + * Gets the sticky faults of a PDH device. + * + * @param handle PDH handle + * + * @return the sticky faults of the PDH + */ +void HAL_GetREVPDHStickyFaults(HAL_REVPDHHandle handle, + HAL_PowerDistributionStickyFaults* stickyFaults, + int32_t* status); + +/** + * Clears the sticky faults on a PDH device. + * + * @param handle PDH handle + */ +void HAL_ClearREVPDHStickyFaults(HAL_REVPDHHandle handle, int32_t* status); + +void HAL_StartREVPDHStream(HAL_REVPDHHandle handle, int32_t* status); + +HAL_PowerDistributionChannelData* HAL_GetREVPDHStreamData( + HAL_REVPDHHandle handle, int32_t* count, int32_t* status); + +void HAL_StopREVPDHStream(HAL_REVPDHHandle handle, int32_t* status); + +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/hal/src/main/native/systemcore/REVPH.cpp b/hal/src/main/native/systemcore/REVPH.cpp new file mode 100644 index 00000000000..37b38659884 --- /dev/null +++ b/hal/src/main/native/systemcore/REVPH.cpp @@ -0,0 +1,790 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/REVPH.h" + +#include +#include + +#include + +#include "HALInitializer.h" +#include "HALInternal.h" +#include "PortsInternal.h" +#include "hal/CANAPI.h" +#include "hal/Errors.h" +#include "hal/handles/IndexedHandleResource.h" +#include "rev/PHFrames.h" + +using namespace hal; + +static constexpr HAL_CANManufacturer manufacturer = + HAL_CANManufacturer::HAL_CAN_Man_kREV; + +static constexpr HAL_CANDeviceType deviceType = + HAL_CANDeviceType::HAL_CAN_Dev_kPneumatics; + +static constexpr int32_t kDefaultControlPeriod = 20; +static constexpr uint8_t kDefaultCompressorDuty = 255; +static constexpr uint8_t kDefaultPressureTarget = 120; +static constexpr uint8_t kDefaultPressureHysteresis = 60; + +#define HAL_REVPH_MAX_PULSE_TIME 65534 + +static constexpr uint32_t APIFromExtId(uint32_t extId) { + return (extId >> 6) & 0x3FF; +} + +static constexpr uint32_t PH_STATUS_0_FRAME_API = + APIFromExtId(PH_STATUS_0_FRAME_ID); +static constexpr uint32_t PH_STATUS_1_FRAME_API = + APIFromExtId(PH_STATUS_1_FRAME_ID); + +static constexpr uint32_t PH_SET_ALL_FRAME_API = + APIFromExtId(PH_SET_ALL_FRAME_ID); +static constexpr uint32_t PH_PULSE_ONCE_FRAME_API = + APIFromExtId(PH_PULSE_ONCE_FRAME_ID); + +static constexpr uint32_t PH_COMPRESSOR_CONFIG_API = + APIFromExtId(PH_COMPRESSOR_CONFIG_FRAME_ID); + +static constexpr uint32_t PH_CLEAR_FAULTS_FRAME_API = + APIFromExtId(PH_CLEAR_FAULTS_FRAME_ID); + +static constexpr uint32_t PH_VERSION_FRAME_API = + APIFromExtId(PH_VERSION_FRAME_ID); + +static constexpr int32_t kPHFrameStatus0Timeout = 50; +static constexpr int32_t kPHFrameStatus1Timeout = 50; + +namespace { + +struct REV_PHObj { + int32_t controlPeriod; + PH_set_all_t desiredSolenoidsState; + wpi::mutex solenoidLock; + HAL_CANHandle hcan; + std::string previousAllocation; + HAL_REVPHVersion versionInfo; +}; + +} // namespace + +static IndexedHandleResource* REVPHHandles; + +namespace hal::init { +void InitializeREVPH() { + static IndexedHandleResource + rH; + REVPHHandles = &rH; +} +} // namespace hal::init + +static PH_status_0_t HAL_ReadREVPHStatus0(HAL_CANHandle hcan, int32_t* status) { + uint8_t packedData[8] = {0}; + int32_t length = 0; + uint64_t timestamp = 0; + PH_status_0_t result = {}; + + HAL_ReadCANPacketTimeout(hcan, PH_STATUS_0_FRAME_API, packedData, &length, + ×tamp, kPHFrameStatus0Timeout * 2, status); + + if (*status != 0) { + return result; + } + + PH_status_0_unpack(&result, packedData, PH_STATUS_0_LENGTH); + + return result; +} + +static PH_status_1_t HAL_ReadREVPHStatus1(HAL_CANHandle hcan, int32_t* status) { + uint8_t packedData[8] = {0}; + int32_t length = 0; + uint64_t timestamp = 0; + PH_status_1_t result = {}; + + HAL_ReadCANPacketTimeout(hcan, PH_STATUS_1_FRAME_API, packedData, &length, + ×tamp, kPHFrameStatus1Timeout * 2, status); + + if (*status != 0) { + return result; + } + + PH_status_1_unpack(&result, packedData, PH_STATUS_1_LENGTH); + + return result; +} + +enum REV_SolenoidState { + kSolenoidDisabled = 0, + kSolenoidEnabled, + kSolenoidControlledViaPulse +}; + +static void HAL_UpdateDesiredREVPHSolenoidState(REV_PHObj* hph, + int32_t solenoid, + REV_SolenoidState state) { + switch (solenoid) { + case 0: + hph->desiredSolenoidsState.channel_0 = state; + break; + case 1: + hph->desiredSolenoidsState.channel_1 = state; + break; + case 2: + hph->desiredSolenoidsState.channel_2 = state; + break; + case 3: + hph->desiredSolenoidsState.channel_3 = state; + break; + case 4: + hph->desiredSolenoidsState.channel_4 = state; + break; + case 5: + hph->desiredSolenoidsState.channel_5 = state; + break; + case 6: + hph->desiredSolenoidsState.channel_6 = state; + break; + case 7: + hph->desiredSolenoidsState.channel_7 = state; + break; + case 8: + hph->desiredSolenoidsState.channel_8 = state; + break; + case 9: + hph->desiredSolenoidsState.channel_9 = state; + break; + case 10: + hph->desiredSolenoidsState.channel_10 = state; + break; + case 11: + hph->desiredSolenoidsState.channel_11 = state; + break; + case 12: + hph->desiredSolenoidsState.channel_12 = state; + break; + case 13: + hph->desiredSolenoidsState.channel_13 = state; + break; + case 14: + hph->desiredSolenoidsState.channel_14 = state; + break; + case 15: + hph->desiredSolenoidsState.channel_15 = state; + break; + } +} + +static void HAL_SendREVPHSolenoidsState(REV_PHObj* hph, int32_t* status) { + uint8_t packedData[PH_SET_ALL_LENGTH] = {0}; + PH_set_all_pack(packedData, &(hph->desiredSolenoidsState), PH_SET_ALL_LENGTH); + HAL_WriteCANPacketRepeating(hph->hcan, packedData, PH_SET_ALL_LENGTH, + PH_SET_ALL_FRAME_API, hph->controlPeriod, status); +} + +static HAL_Bool HAL_CheckREVPHPulseTime(int32_t time) { + return ((time > 0) && (time <= HAL_REVPH_MAX_PULSE_TIME)) ? 1 : 0; +} + +HAL_REVPHHandle HAL_InitializeREVPH(int32_t module, + const char* allocationLocation, + int32_t* status) { + hal::init::CheckInit(); + if (!HAL_CheckREVPHModuleNumber(module)) { + *status = RESOURCE_OUT_OF_RANGE; + hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PH", 1, + kNumREVPHModules, module); + return HAL_kInvalidHandle; + } + + HAL_REVPHHandle handle; + // Module starts at 1 + auto hph = REVPHHandles->Allocate(module - 1, &handle, status); + if (*status != 0) { + if (hph) { + hal::SetLastErrorPreviouslyAllocated(status, "REV PH", module, + hph->previousAllocation); + } else { + hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PH", 1, + kNumREVPHModules, module); + } + return HAL_kInvalidHandle; // failed to allocate. Pass error back. + } + + HAL_CANHandle hcan = + HAL_InitializeCAN(manufacturer, module, deviceType, status); + + if (*status != 0) { + REVPHHandles->Free(handle); + return HAL_kInvalidHandle; + } + + hph->previousAllocation = allocationLocation ? allocationLocation : ""; + hph->hcan = hcan; + hph->controlPeriod = kDefaultControlPeriod; + std::memset(&hph->desiredSolenoidsState, 0, + sizeof(hph->desiredSolenoidsState)); + std::memset(&hph->versionInfo, 0, sizeof(hph->versionInfo)); + + int32_t can_status = 0; + + // Start closed-loop compressor control by starting solenoid state updates + HAL_SendREVPHSolenoidsState(hph.get(), &can_status); + + return handle; +} + +void HAL_FreeREVPH(HAL_REVPHHandle handle) { + auto hph = REVPHHandles->Get(handle); + if (hph) { + HAL_CleanCAN(hph->hcan); + } + + REVPHHandles->Free(handle); +} + +HAL_Bool HAL_CheckREVPHModuleNumber(int32_t module) { + return module >= 1 && module <= kNumREVPHModules; +} + +HAL_Bool HAL_CheckREVPHSolenoidChannel(int32_t channel) { + return channel >= 0 && channel < kNumREVPHChannels; +} + +HAL_Bool HAL_GetREVPHCompressor(HAL_REVPHHandle handle, int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return false; + } + + PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status); + + if (*status != 0) { + return false; + } + + return status0.compressor_on; +} + +void HAL_SetREVPHCompressorConfig(HAL_REVPHHandle handle, + const HAL_REVPHCompressorConfig* config, + int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + PH_compressor_config_t frameData; + frameData.minimum_tank_pressure = + PH_compressor_config_minimum_tank_pressure_encode( + config->minAnalogVoltage); + frameData.maximum_tank_pressure = + PH_compressor_config_maximum_tank_pressure_encode( + config->maxAnalogVoltage); + frameData.force_disable = config->forceDisable; + frameData.use_digital = config->useDigital; + + uint8_t packedData[PH_COMPRESSOR_CONFIG_LENGTH] = {0}; + PH_compressor_config_pack(packedData, &frameData, + PH_COMPRESSOR_CONFIG_LENGTH); + HAL_WriteCANPacket(ph->hcan, packedData, PH_COMPRESSOR_CONFIG_LENGTH, + PH_COMPRESSOR_CONFIG_API, status); +} + +void HAL_SetREVPHClosedLoopControlDisabled(HAL_REVPHHandle handle, + int32_t* status) { + HAL_REVPHCompressorConfig config = {0, 0, 0, 0}; + config.forceDisable = true; + + HAL_SetREVPHCompressorConfig(handle, &config, status); +} + +void HAL_SetREVPHClosedLoopControlDigital(HAL_REVPHHandle handle, + int32_t* status) { + HAL_REVPHCompressorConfig config = {0, 0, 0, 0}; + config.useDigital = true; + + HAL_SetREVPHCompressorConfig(handle, &config, status); +} + +void HAL_SetREVPHClosedLoopControlAnalog(HAL_REVPHHandle handle, + double minAnalogVoltage, + double maxAnalogVoltage, + int32_t* status) { + HAL_REVPHCompressorConfig config = {0, 0, 0, 0}; + config.minAnalogVoltage = minAnalogVoltage; + config.maxAnalogVoltage = maxAnalogVoltage; + + HAL_SetREVPHCompressorConfig(handle, &config, status); +} + +void HAL_SetREVPHClosedLoopControlHybrid(HAL_REVPHHandle handle, + double minAnalogVoltage, + double maxAnalogVoltage, + int32_t* status) { + HAL_REVPHCompressorConfig config = {0, 0, 0, 0}; + config.minAnalogVoltage = minAnalogVoltage; + config.maxAnalogVoltage = maxAnalogVoltage; + config.useDigital = true; + + HAL_SetREVPHCompressorConfig(handle, &config, status); +} + +HAL_REVPHCompressorConfigType HAL_GetREVPHCompressorConfig( + HAL_REVPHHandle handle, int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return HAL_REVPHCompressorConfigType_kDisabled; + } + + PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status); + + if (*status != 0) { + return HAL_REVPHCompressorConfigType_kDisabled; + } + + return static_cast(status0.compressor_config); +} + +HAL_Bool HAL_GetREVPHPressureSwitch(HAL_REVPHHandle handle, int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return false; + } + + PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status); + + if (*status != 0) { + return false; + } + + return status0.digital_sensor; +} + +double HAL_GetREVPHCompressorCurrent(HAL_REVPHHandle handle, int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status); + + if (*status != 0) { + return 0; + } + + return PH_status_1_compressor_current_decode(status1.compressor_current); +} + +double HAL_GetREVPHAnalogVoltage(HAL_REVPHHandle handle, int32_t channel, + int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + if (channel < 0 || channel > 1) { + *status = PARAMETER_OUT_OF_RANGE; + hal::SetLastErrorIndexOutOfRange(status, "Invalid REV Analog Index", 0, 2, + channel); + return 0; + } + + PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status); + + if (*status != 0) { + return 0; + } + + if (channel == 0) { + return PH_status_0_analog_0_decode(status0.analog_0); + } + return PH_status_0_analog_1_decode(status0.analog_1); +} + +double HAL_GetREVPHVoltage(HAL_REVPHHandle handle, int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status); + + if (*status != 0) { + return 0; + } + + return PH_status_1_v_bus_decode(status1.v_bus); +} + +double HAL_GetREVPH5VVoltage(HAL_REVPHHandle handle, int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status); + + if (*status != 0) { + return 0; + } + + return PH_status_1_supply_voltage_5_v_decode(status1.supply_voltage_5_v); +} + +double HAL_GetREVPHSolenoidCurrent(HAL_REVPHHandle handle, int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status); + + if (*status != 0) { + return 0; + } + + return PH_status_1_solenoid_current_decode(status1.solenoid_current); +} + +double HAL_GetREVPHSolenoidVoltage(HAL_REVPHHandle handle, int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status); + + if (*status != 0) { + return 0; + } + + return PH_status_1_solenoid_voltage_decode(status1.solenoid_voltage); +} + +void HAL_GetREVPHVersion(HAL_REVPHHandle handle, HAL_REVPHVersion* version, + int32_t* status) { + std::memset(version, 0, sizeof(*version)); + uint8_t packedData[8] = {0}; + int32_t length = 0; + uint64_t timestamp = 0; + PH_version_t result = {}; + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + if (ph->versionInfo.firmwareMajor > 0) { + version->firmwareMajor = ph->versionInfo.firmwareMajor; + version->firmwareMinor = ph->versionInfo.firmwareMinor; + version->firmwareFix = ph->versionInfo.firmwareFix; + version->hardwareMajor = ph->versionInfo.hardwareMajor; + version->hardwareMinor = ph->versionInfo.hardwareMinor; + version->uniqueId = ph->versionInfo.uniqueId; + + *status = 0; + return; + } + + HAL_WriteCANRTRFrame(ph->hcan, PH_VERSION_LENGTH, PH_VERSION_FRAME_API, + status); + + if (*status != 0) { + return; + } + + uint32_t timeoutMs = 100; + for (uint32_t i = 0; i <= timeoutMs; i++) { + HAL_ReadCANPacketNew(ph->hcan, PH_VERSION_FRAME_API, packedData, &length, + ×tamp, status); + if (*status == 0) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + + if (*status != 0) { + return; + } + + PH_version_unpack(&result, packedData, PH_VERSION_LENGTH); + + version->firmwareMajor = result.firmware_year; + version->firmwareMinor = result.firmware_minor; + version->firmwareFix = result.firmware_fix; + version->hardwareMinor = result.hardware_minor; + version->hardwareMajor = result.hardware_major; + version->uniqueId = result.unique_id; + + ph->versionInfo = *version; +} + +int32_t HAL_GetREVPHSolenoids(HAL_REVPHHandle handle, int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status); + + if (*status != 0) { + return 0; + } + + uint32_t result = status0.channel_0; + result |= status0.channel_1 << 1; + result |= status0.channel_2 << 2; + result |= status0.channel_3 << 3; + result |= status0.channel_4 << 4; + result |= status0.channel_5 << 5; + result |= status0.channel_6 << 6; + result |= status0.channel_7 << 7; + result |= status0.channel_8 << 8; + result |= status0.channel_9 << 9; + result |= status0.channel_10 << 10; + result |= status0.channel_11 << 11; + result |= status0.channel_12 << 12; + result |= status0.channel_13 << 13; + result |= status0.channel_14 << 14; + result |= status0.channel_15 << 15; + + return result; +} + +void HAL_SetREVPHSolenoids(HAL_REVPHHandle handle, int32_t mask, int32_t values, + int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + std::scoped_lock lock{ph->solenoidLock}; + for (int solenoid = 0; solenoid < kNumREVPHChannels; solenoid++) { + if (mask & (1 << solenoid)) { + // The mask bit for the solenoid is set, so we update the solenoid state + REV_SolenoidState desiredSolenoidState = + values & (1 << solenoid) ? kSolenoidEnabled : kSolenoidDisabled; + HAL_UpdateDesiredREVPHSolenoidState(ph.get(), solenoid, + desiredSolenoidState); + } + } + HAL_SendREVPHSolenoidsState(ph.get(), status); +} + +void HAL_FireREVPHOneShot(HAL_REVPHHandle handle, int32_t index, int32_t durMs, + int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + if (index >= kNumREVPHChannels || index < 0) { + *status = PARAMETER_OUT_OF_RANGE; + hal::SetLastError( + status, + fmt::format("Only [0-15] are valid index values. Requested {}", index)); + return; + } + + if (!HAL_CheckREVPHPulseTime(durMs)) { + *status = PARAMETER_OUT_OF_RANGE; + hal::SetLastError( + status, + fmt::format("Time not within expected range [0-65534]. Requested {}", + durMs)); + return; + } + + { + std::scoped_lock lock{ph->solenoidLock}; + HAL_UpdateDesiredREVPHSolenoidState(ph.get(), index, + kSolenoidControlledViaPulse); + HAL_SendREVPHSolenoidsState(ph.get(), status); + } + + if (*status != 0) { + return; + } + + PH_pulse_once_t pulse = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + pulse.pulse_length_ms = durMs; + + // Specify which solenoid should be pulsed + // The protocol supports specifying any number of solenoids to be pulsed at + // the same time, should that functionality be exposed to users in the future. + switch (index) { + case 0: + pulse.channel_0 = true; + break; + case 1: + pulse.channel_1 = true; + break; + case 2: + pulse.channel_2 = true; + break; + case 3: + pulse.channel_3 = true; + break; + case 4: + pulse.channel_4 = true; + break; + case 5: + pulse.channel_5 = true; + break; + case 6: + pulse.channel_6 = true; + break; + case 7: + pulse.channel_7 = true; + break; + case 8: + pulse.channel_8 = true; + break; + case 9: + pulse.channel_9 = true; + break; + case 10: + pulse.channel_10 = true; + break; + case 11: + pulse.channel_11 = true; + break; + case 12: + pulse.channel_12 = true; + break; + case 13: + pulse.channel_13 = true; + break; + case 14: + pulse.channel_14 = true; + break; + case 15: + pulse.channel_15 = true; + break; + } + + // Send pulse command + uint8_t packedData[PH_PULSE_ONCE_LENGTH] = {0}; + PH_pulse_once_pack(packedData, &pulse, PH_PULSE_ONCE_LENGTH); + HAL_WriteCANPacket(ph->hcan, packedData, PH_PULSE_ONCE_LENGTH, + PH_PULSE_ONCE_FRAME_API, status); +} + +void HAL_GetREVPHFaults(HAL_REVPHHandle handle, HAL_REVPHFaults* faults, + int32_t* status) { + std::memset(faults, 0, sizeof(*faults)); + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status); + faults->channel0Fault = status0.channel_0_fault; + faults->channel1Fault = status0.channel_1_fault; + faults->channel2Fault = status0.channel_2_fault; + faults->channel3Fault = status0.channel_3_fault; + faults->channel4Fault = status0.channel_4_fault; + faults->channel5Fault = status0.channel_5_fault; + faults->channel6Fault = status0.channel_6_fault; + faults->channel7Fault = status0.channel_7_fault; + faults->channel8Fault = status0.channel_8_fault; + faults->channel9Fault = status0.channel_9_fault; + faults->channel10Fault = status0.channel_10_fault; + faults->channel11Fault = status0.channel_11_fault; + faults->channel12Fault = status0.channel_12_fault; + faults->channel13Fault = status0.channel_13_fault; + faults->channel14Fault = status0.channel_14_fault; + faults->channel15Fault = status0.channel_15_fault; + faults->compressorOverCurrent = status0.compressor_oc_fault; + faults->compressorOpen = status0.compressor_open_fault; + faults->solenoidOverCurrent = status0.solenoid_oc_fault; + faults->brownout = status0.brownout_fault; + faults->canWarning = status0.can_warning_fault; + faults->hardwareFault = status0.hardware_fault; +} + +void HAL_GetREVPHStickyFaults(HAL_REVPHHandle handle, + HAL_REVPHStickyFaults* stickyFaults, + int32_t* status) { + std::memset(stickyFaults, 0, sizeof(*stickyFaults)); + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status); + stickyFaults->compressorOverCurrent = status1.sticky_compressor_oc_fault; + stickyFaults->compressorOpen = status1.sticky_compressor_open_fault; + stickyFaults->solenoidOverCurrent = status1.sticky_solenoid_oc_fault; + stickyFaults->brownout = status1.sticky_brownout_fault; + stickyFaults->canWarning = status1.sticky_can_warning_fault; + stickyFaults->canBusOff = status1.sticky_can_bus_off_fault; + stickyFaults->hardwareFault = status1.sticky_hardware_fault; + stickyFaults->firmwareFault = status1.sticky_firmware_fault; + stickyFaults->hasReset = status1.sticky_has_reset_fault; +} + +void HAL_ClearREVPHStickyFaults(HAL_REVPHHandle handle, int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + uint8_t packedData[8] = {0}; + HAL_WriteCANPacket(ph->hcan, packedData, PH_CLEAR_FAULTS_LENGTH, + PH_CLEAR_FAULTS_FRAME_API, status); +} + +int32_t HAL_GetREVPHSolenoidDisabledList(HAL_REVPHHandle handle, + int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return false; + } + + PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status); + if (*status != 0) { + return 0; + } + + uint32_t solenoidFaults = status0.channel_0_fault; + solenoidFaults |= status0.channel_1_fault << 1; + solenoidFaults |= status0.channel_2_fault << 2; + solenoidFaults |= status0.channel_3_fault << 3; + solenoidFaults |= status0.channel_4_fault << 4; + solenoidFaults |= status0.channel_5_fault << 5; + solenoidFaults |= status0.channel_6_fault << 6; + solenoidFaults |= status0.channel_7_fault << 7; + solenoidFaults |= status0.channel_8_fault << 8; + solenoidFaults |= status0.channel_9_fault << 9; + solenoidFaults |= status0.channel_10_fault << 10; + solenoidFaults |= status0.channel_11_fault << 11; + solenoidFaults |= status0.channel_12_fault << 12; + solenoidFaults |= status0.channel_13_fault << 13; + solenoidFaults |= status0.channel_14_fault << 14; + solenoidFaults |= status0.channel_15_fault << 15; + return solenoidFaults; +} diff --git a/hal/src/main/native/systemcore/Relay.cpp b/hal/src/main/native/systemcore/Relay.cpp new file mode 100644 index 00000000000..cdc7a11049e --- /dev/null +++ b/hal/src/main/native/systemcore/Relay.cpp @@ -0,0 +1,47 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/Relay.h" + +#include + +#include "HALInitializer.h" +#include "HALInternal.h" +#include "PortsInternal.h" +#include "hal/handles/IndexedHandleResource.h" + +using namespace hal; + +namespace hal::init { +void InitializeRelay() {} +} // namespace hal::init + +extern "C" { + +HAL_RelayHandle HAL_InitializeRelayPort(HAL_PortHandle portHandle, HAL_Bool fwd, + const char* allocationLocation, + int32_t* status) { + hal::init::CheckInit(); + *status = HAL_HANDLE_ERROR; + return HAL_kInvalidHandle; +} + +void HAL_FreeRelayPort(HAL_RelayHandle relayPortHandle) {} + +HAL_Bool HAL_CheckRelayChannel(int32_t channel) { + return false; +} + +void HAL_SetRelay(HAL_RelayHandle relayPortHandle, HAL_Bool on, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +HAL_Bool HAL_GetRelay(HAL_RelayHandle relayPortHandle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return false; +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/SPI.cpp b/hal/src/main/native/systemcore/SPI.cpp new file mode 100644 index 00000000000..bde55b8f1d4 --- /dev/null +++ b/hal/src/main/native/systemcore/SPI.cpp @@ -0,0 +1,141 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/SPI.h" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "HALInitializer.h" +#include "HALInternal.h" +#include "hal/DIO.h" +#include "hal/HAL.h" +#include "hal/handles/HandlesInternal.h" + +using namespace hal; + +namespace hal::init { +void InitializeSPI() {} +} // namespace hal::init + +extern "C" { + +void HAL_InitializeSPI(HAL_SPIPort port, int32_t* status) { + hal::init::CheckInit(); + *status = HAL_HANDLE_ERROR; + return; +} + +int32_t HAL_TransactionSPI(HAL_SPIPort port, const uint8_t* dataToSend, + uint8_t* dataReceived, int32_t size) { + return -1; +} + +int32_t HAL_WriteSPI(HAL_SPIPort port, const uint8_t* dataToSend, + int32_t sendSize) { + return -1; +} + +int32_t HAL_ReadSPI(HAL_SPIPort port, uint8_t* buffer, int32_t count) { + return -1; +} + +void HAL_CloseSPI(HAL_SPIPort port) {} + +void HAL_SetSPISpeed(HAL_SPIPort port, int32_t speed) {} + +void HAL_SetSPIMode(HAL_SPIPort port, HAL_SPIMode mode) {} + +HAL_SPIMode HAL_GetSPIMode(HAL_SPIPort port) { + return HAL_SPI_kMode0; +} + +void HAL_SetSPIChipSelectActiveHigh(HAL_SPIPort port, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetSPIChipSelectActiveLow(HAL_SPIPort port, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +int32_t HAL_GetSPIHandle(HAL_SPIPort port) { + return 0; +} + +void HAL_SetSPIHandle(HAL_SPIPort port, int32_t handle) {} + +void HAL_InitSPIAuto(HAL_SPIPort port, int32_t bufferSize, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_FreeSPIAuto(HAL_SPIPort port, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_StartSPIAutoRate(HAL_SPIPort port, double period, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_StartSPIAutoTrigger(HAL_SPIPort port, HAL_Handle digitalSourceHandle, + HAL_AnalogTriggerType analogTriggerType, + HAL_Bool triggerRising, HAL_Bool triggerFalling, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_StopSPIAuto(HAL_SPIPort port, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetSPIAutoTransmitData(HAL_SPIPort port, const uint8_t* dataToSend, + int32_t dataSize, int32_t zeroSize, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_ForceSPIAutoRead(HAL_SPIPort port, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +int32_t HAL_ReadSPIAutoReceivedData(HAL_SPIPort port, uint32_t* buffer, + int32_t numToRead, double timeout, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +int32_t HAL_GetSPIAutoDroppedCount(HAL_SPIPort port, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +void HAL_ConfigureSPIAutoStall(HAL_SPIPort port, int32_t csToSclkTicks, + int32_t stallTicks, int32_t pow2BytesPerRead, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/SerialPort.cpp b/hal/src/main/native/systemcore/SerialPort.cpp new file mode 100644 index 00000000000..70825a1690f --- /dev/null +++ b/hal/src/main/native/systemcore/SerialPort.cpp @@ -0,0 +1,145 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/SerialPort.h" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "HALInternal.h" +#include "hal/cpp/SerialHelper.h" +#include "hal/handles/HandlesInternal.h" +#include "hal/handles/IndexedHandleResource.h" + +namespace hal::init { +void InitializeSerialPort() {} +} // namespace hal::init + +using namespace hal; + +extern "C" { +HAL_SerialPortHandle HAL_InitializeSerialPort(HAL_SerialPort port, + int32_t* status) { + // hal::init::CheckInit(); + + *status = HAL_HANDLE_ERROR; + return HAL_kInvalidHandle; +} +HAL_SerialPortHandle HAL_InitializeSerialPortDirect(HAL_SerialPort port, + const char* portName, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return HAL_kInvalidHandle; +} + +void HAL_CloseSerial(HAL_SerialPortHandle handle) {} + +int HAL_GetSerialFD(HAL_SerialPortHandle handle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return 0; +} + +void HAL_SetSerialBaudRate(HAL_SerialPortHandle handle, int32_t baud, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetSerialDataBits(HAL_SerialPortHandle handle, int32_t bits, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetSerialParity(HAL_SerialPortHandle handle, int32_t parity, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetSerialStopBits(HAL_SerialPortHandle handle, int32_t stopBits, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetSerialWriteMode(HAL_SerialPortHandle handle, int32_t mode, + int32_t* status) { + // This seems to be a no op on the NI serial port driver +} + +void HAL_SetSerialFlowControl(HAL_SerialPortHandle handle, int32_t flow, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetSerialTimeout(HAL_SerialPortHandle handle, double timeout, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_EnableSerialTermination(HAL_SerialPortHandle handle, char terminator, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_DisableSerialTermination(HAL_SerialPortHandle handle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} + +void HAL_SetSerialReadBufferSize(HAL_SerialPortHandle handle, int32_t size, + int32_t* status) { + // NO OP currently +} + +void HAL_SetSerialWriteBufferSize(HAL_SerialPortHandle handle, int32_t size, + int32_t* status) { + // NO OP currently +} + +int32_t HAL_GetSerialBytesReceived(HAL_SerialPortHandle handle, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return -1; +} + +int32_t HAL_ReadSerial(HAL_SerialPortHandle handle, char* buffer, int32_t count, + int32_t* status) { + *status = HAL_HANDLE_ERROR; + return -1; +} + +int32_t HAL_WriteSerial(HAL_SerialPortHandle handle, const char* buffer, + int32_t count, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return -1; +} + +void HAL_FlushSerial(HAL_SerialPortHandle handle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} +void HAL_ClearSerial(HAL_SerialPortHandle handle, int32_t* status) { + *status = HAL_HANDLE_ERROR; + return; +} +} // extern "C" diff --git a/hal/src/main/native/systemcore/SimDevice.cpp b/hal/src/main/native/systemcore/SimDevice.cpp new file mode 100644 index 00000000000..03b35f7b464 --- /dev/null +++ b/hal/src/main/native/systemcore/SimDevice.cpp @@ -0,0 +1,53 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/SimDevice.h" + +extern "C" { + +HAL_SimDeviceHandle HAL_CreateSimDevice(const char* name) { + return 0; +} + +void HAL_FreeSimDevice(HAL_SimDeviceHandle handle) {} + +const char* HAL_GetSimDeviceName(HAL_SimDeviceHandle handle) { + return ""; +} + +HAL_SimValueHandle HAL_CreateSimValue(HAL_SimDeviceHandle device, + const char* name, int32_t direction, + const struct HAL_Value* initialValue) { + return 0; +} + +HAL_SimValueHandle HAL_CreateSimValueEnum(HAL_SimDeviceHandle device, + const char* name, int32_t direction, + int32_t numOptions, + const char** options, + int32_t initialValue) { + return 0; +} + +HAL_SimValueHandle HAL_CreateSimValueEnumDouble( + HAL_SimDeviceHandle device, const char* name, int32_t direction, + int32_t numOptions, const char** options, const double* optionValues, + int32_t initialValue) { + return 0; +} + +void HAL_GetSimValue(HAL_SimValueHandle handle, struct HAL_Value* value) { + value->type = HAL_UNASSIGNED; +} + +void HAL_SetSimValue(HAL_SimValueHandle handle, const struct HAL_Value* value) { +} + +void HAL_ResetSimValue(HAL_SimValueHandle handle) {} + +hal::SimDevice::SimDevice(const char* name, int index) {} + +hal::SimDevice::SimDevice(const char* name, int index, int channel) {} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/Threads.cpp b/hal/src/main/native/systemcore/Threads.cpp new file mode 100644 index 00000000000..aa55b56945b --- /dev/null +++ b/hal/src/main/native/systemcore/Threads.cpp @@ -0,0 +1,90 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/Threads.h" + +#include +#include + +#include "hal/Errors.h" + +namespace hal::init { +void InitializeThreads() {} +} // namespace hal::init + +extern "C" { + +int32_t HAL_GetThreadPriority(NativeThreadHandle handle, HAL_Bool* isRealTime, + int32_t* status) { + sched_param sch; + int policy; + int success = pthread_getschedparam( + *reinterpret_cast(handle), &policy, &sch); + if (success == 0) { + *status = 0; + } else { + *status = HAL_THREAD_PRIORITY_ERROR; + return -1; + } + if (policy == SCHED_FIFO || policy == SCHED_RR) { + *isRealTime = true; + return sch.sched_priority; + } else { + *isRealTime = false; + // 0 is the only supported priority for non-real-time + return 0; + } +} + +int32_t HAL_GetCurrentThreadPriority(HAL_Bool* isRealTime, int32_t* status) { + auto thread = pthread_self(); + return HAL_GetThreadPriority(&thread, isRealTime, status); +} + +HAL_Bool HAL_SetThreadPriority(NativeThreadHandle handle, HAL_Bool realTime, + int32_t priority, int32_t* status) { + if (handle == nullptr) { + *status = NULL_PARAMETER; + return false; + } + + int scheduler = realTime ? SCHED_FIFO : SCHED_OTHER; + if (realTime) { + // We don't support setting priorities for non RT threads + // so we don't need to check for proper range + if (priority < sched_get_priority_min(scheduler) || + priority > sched_get_priority_max(scheduler)) { + *status = HAL_THREAD_PRIORITY_RANGE_ERROR; + return false; + } + } + + sched_param sch; + int policy; + pthread_getschedparam(*reinterpret_cast(handle), &policy, + &sch); + if (scheduler == SCHED_FIFO || scheduler == SCHED_RR) { + sch.sched_priority = priority; + } else { + // Only need to set 0 priority for non RT thread + sch.sched_priority = 0; + } + + if (pthread_setschedparam(*reinterpret_cast(handle), + scheduler, &sch)) { + *status = HAL_THREAD_PRIORITY_ERROR; + return false; + } else { + *status = 0; + return true; + } +} + +HAL_Bool HAL_SetCurrentThreadPriority(HAL_Bool realTime, int32_t priority, + int32_t* status) { + auto thread = pthread_self(); + return HAL_SetThreadPriority(&thread, realTime, priority, status); +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/AccelerometerData.cpp b/hal/src/main/native/systemcore/mockdata/AccelerometerData.cpp new file mode 100644 index 00000000000..e097a24b61a --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/AccelerometerData.cpp @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/AccelerometerData.h" + +#include "hal/simulation/SimDataValue.h" + +extern "C" { +void HALSIM_ResetAccelerometerData(int32_t index) {} + +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, Accelerometer##CAPINAME, RETURN) + +DEFINE_CAPI(HAL_Bool, Active, false) +DEFINE_CAPI(HAL_AccelerometerRange, Range, HAL_AccelerometerRange_k2G) +DEFINE_CAPI(double, X, 0) +DEFINE_CAPI(double, Y, 0) +DEFINE_CAPI(double, Z, 0) + +void HALSIM_RegisterAccelerometerAllCallbacks(int32_t index, + HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify) {} +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/AddressableLEDData.cpp b/hal/src/main/native/systemcore/mockdata/AddressableLEDData.cpp new file mode 100644 index 00000000000..a1d7011945b --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/AddressableLEDData.cpp @@ -0,0 +1,44 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/AddressableLEDData.h" + +#include "hal/simulation/SimDataValue.h" + +extern "C" { + +int32_t HALSIM_FindAddressableLEDForChannel(int32_t channel) { + return 0; +} + +void HALSIM_ResetAddressableLEDData(int32_t index) {} + +int32_t HALSIM_GetAddressableLEDData(int32_t index, + struct HAL_AddressableLEDData* data) { + return 0; +} + +void HALSIM_SetAddressableLEDData(int32_t index, + const struct HAL_AddressableLEDData* data, + int32_t length) {} + +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, AddressableLED##CAPINAME, RETURN) + +DEFINE_CAPI(HAL_Bool, Initialized, false) +DEFINE_CAPI(int32_t, OutputPort, 0) +DEFINE_CAPI(int32_t, Length, 0) +DEFINE_CAPI(HAL_Bool, Running, false) + +#undef DEFINE_CAPI +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMCALLBACKREGISTRY_STUB_CAPI(TYPE, HALSIM, AddressableLED##CAPINAME) + +DEFINE_CAPI(HAL_ConstBufferCallback, Data, data) + +void HALSIM_RegisterAddressableLEDAllCallbacks(int32_t index, + HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify) {} +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/AnalogGyroData.cpp b/hal/src/main/native/systemcore/mockdata/AnalogGyroData.cpp new file mode 100644 index 00000000000..b4ff695deaf --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/AnalogGyroData.cpp @@ -0,0 +1,23 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/AnalogGyroData.h" + +#include "hal/simulation/SimDataValue.h" + +extern "C" { +void HALSIM_ResetAnalogGyroData(int32_t index) {} + +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, AnalogGyro##CAPINAME, RETURN) + +DEFINE_CAPI(double, Angle, 0) +DEFINE_CAPI(double, Rate, 0) +DEFINE_CAPI(HAL_Bool, Initialized, false) + +void HALSIM_RegisterAnalogGyroAllCallbacks(int32_t index, + HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify) {} +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/AnalogInData.cpp b/hal/src/main/native/systemcore/mockdata/AnalogInData.cpp new file mode 100644 index 00000000000..30ac79561f2 --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/AnalogInData.cpp @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/AnalogInData.h" + +#include "hal/simulation/SimDataValue.h" + +extern "C" { +void HALSIM_ResetAnalogInData(int32_t index) {} + +HAL_SimDeviceHandle HALSIM_GetAnalogInSimDevice(int32_t index) { + return 0; +} + +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, AnalogIn##CAPINAME, RETURN) + +DEFINE_CAPI(HAL_Bool, Initialized, false) +DEFINE_CAPI(int32_t, AverageBits, 0) +DEFINE_CAPI(int32_t, OversampleBits, 0) +DEFINE_CAPI(double, Voltage, 0) +DEFINE_CAPI(HAL_Bool, AccumulatorInitialized, false) +DEFINE_CAPI(int64_t, AccumulatorValue, 0) +DEFINE_CAPI(int64_t, AccumulatorCount, 0) +DEFINE_CAPI(int32_t, AccumulatorCenter, 0) +DEFINE_CAPI(int32_t, AccumulatorDeadband, 0) + +void HALSIM_RegisterAnalogInAllCallbacks(int32_t index, + HAL_NotifyCallback callback, + void* param, HAL_Bool initialNotify) {} +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/AnalogOutData.cpp b/hal/src/main/native/systemcore/mockdata/AnalogOutData.cpp new file mode 100644 index 00000000000..e6da2aaf991 --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/AnalogOutData.cpp @@ -0,0 +1,22 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/AnalogOutData.h" + +#include "hal/simulation/SimDataValue.h" + +extern "C" { +void HALSIM_ResetAnalogOutData(int32_t index) {} + +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, AnalogOut##CAPINAME, RETURN) + +DEFINE_CAPI(double, Voltage, 0) +DEFINE_CAPI(HAL_Bool, Initialized, false) + +void HALSIM_RegisterAnalogOutAllCallbacks(int32_t index, + HAL_NotifyCallback callback, + void* param, HAL_Bool initialNotify) { +} +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/AnalogTriggerData.cpp b/hal/src/main/native/systemcore/mockdata/AnalogTriggerData.cpp new file mode 100644 index 00000000000..af8d7cc53f4 --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/AnalogTriggerData.cpp @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/AnalogTriggerData.h" + +#include "hal/simulation/SimDataValue.h" + +extern "C" { + +int32_t HALSIM_FindAnalogTriggerForChannel(int32_t channel) { + return 0; +} + +void HALSIM_ResetAnalogTriggerData(int32_t index) {} + +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, AnalogTrigger##CAPINAME, RETURN) + +DEFINE_CAPI(HAL_Bool, Initialized, false) +DEFINE_CAPI(double, TriggerLowerBound, 0) +DEFINE_CAPI(double, TriggerUpperBound, 0) +DEFINE_CAPI(HALSIM_AnalogTriggerMode, TriggerMode, + HALSIM_AnalogTriggerUnassigned) + +void HALSIM_RegisterAnalogTriggerAllCallbacks(int32_t index, + HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify) {} +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/CTREPCMData.cpp b/hal/src/main/native/systemcore/mockdata/CTREPCMData.cpp new file mode 100644 index 00000000000..fc9edfea748 --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/CTREPCMData.cpp @@ -0,0 +1,38 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/CTREPCMData.h" + +#include "hal/simulation/SimDataValue.h" + +extern "C" { +void HALSIM_ResetCTREPCMData(int32_t index) {} + +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, CTREPCM##CAPINAME, RETURN) + +HAL_SIMDATAVALUE_STUB_CAPI_CHANNEL(HAL_Bool, HALSIM, CTREPCMSolenoidOutput, + false) +DEFINE_CAPI(HAL_Bool, Initialized, false) +DEFINE_CAPI(HAL_Bool, CompressorOn, false) +DEFINE_CAPI(HAL_Bool, ClosedLoopEnabled, false) +DEFINE_CAPI(HAL_Bool, PressureSwitch, false) +DEFINE_CAPI(double, CompressorCurrent, 0) + +void HALSIM_GetCTREPCMAllSolenoids(int32_t index, uint8_t* values) { + *values = 0; +} + +void HALSIM_SetCTREPCMAllSolenoids(int32_t index, uint8_t values) {} + +void HALSIM_RegisterCTREPCMAllNonSolenoidCallbacks(int32_t index, + HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify) {} + +void HALSIM_RegisterCTREPCMAllSolenoidCallbacks(int32_t index, int32_t channel, + HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify) {} +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/CanDataInternal.cpp b/hal/src/main/native/systemcore/mockdata/CanDataInternal.cpp new file mode 100644 index 00000000000..be2fbc05263 --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/CanDataInternal.cpp @@ -0,0 +1,22 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/CanData.h" +#include "hal/simulation/SimDataValue.h" + +extern "C" { + +void HALSIM_ResetCanData(void) {} + +#define DEFINE_CAPI(TYPE, CAPINAME) \ + HAL_SIMCALLBACKREGISTRY_STUB_CAPI_NOINDEX(TYPE, HALSIM, Can##CAPINAME) + +DEFINE_CAPI(HAL_CAN_SendMessageCallback, SendMessage) +DEFINE_CAPI(HAL_CAN_ReceiveMessageCallback, ReceiveMessage) +DEFINE_CAPI(HAL_CAN_OpenStreamSessionCallback, OpenStream) +DEFINE_CAPI(HAL_CAN_CloseStreamSessionCallback, CloseStream) +DEFINE_CAPI(HAL_CAN_ReadStreamSessionCallback, ReadStream) +DEFINE_CAPI(HAL_CAN_GetCANStatusCallback, GetCANStatus) + +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/DIOData.cpp b/hal/src/main/native/systemcore/mockdata/DIOData.cpp new file mode 100644 index 00000000000..392c31b6d4f --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/DIOData.cpp @@ -0,0 +1,27 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/DIOData.h" + +#include "hal/simulation/SimDataValue.h" + +extern "C" { +void HALSIM_ResetDIOData(int32_t index) {} + +HAL_SimDeviceHandle HALSIM_GetDIOSimDevice(int32_t index) { + return 0; +} + +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, DIO##CAPINAME, RETURN) + +DEFINE_CAPI(HAL_Bool, Initialized, false) +DEFINE_CAPI(HAL_Bool, Value, false) +DEFINE_CAPI(double, PulseLength, 0) +DEFINE_CAPI(HAL_Bool, IsInput, false) +DEFINE_CAPI(int32_t, FilterIndex, 0) + +void HALSIM_RegisterDIOAllCallbacks(int32_t index, HAL_NotifyCallback callback, + void* param, HAL_Bool initialNotify) {} +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/DigitalPWMData.cpp b/hal/src/main/native/systemcore/mockdata/DigitalPWMData.cpp new file mode 100644 index 00000000000..c3f5d31e30d --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/DigitalPWMData.cpp @@ -0,0 +1,27 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/DigitalPWMData.h" + +#include "hal/simulation/SimDataValue.h" + +extern "C" { +int32_t HALSIM_FindDigitalPWMForChannel(int32_t channel) { + return 0; +} + +void HALSIM_ResetDigitalPWMData(int32_t index) {} + +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, DigitalPWM##CAPINAME, RETURN) + +DEFINE_CAPI(HAL_Bool, Initialized, false) +DEFINE_CAPI(double, DutyCycle, 0) +DEFINE_CAPI(int32_t, Pin, 0) + +void HALSIM_RegisterDigitalPWMAllCallbacks(int32_t index, + HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify) {} +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/DriverStationData.cpp b/hal/src/main/native/systemcore/mockdata/DriverStationData.cpp new file mode 100644 index 00000000000..90fa34fc566 --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/DriverStationData.cpp @@ -0,0 +1,123 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/DriverStationData.h" + +#include "hal/simulation/SimDataValue.h" + +extern "C" { +void HALSIM_ResetDriverStationData(void) {} + +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI_NOINDEX(TYPE, HALSIM, DriverStation##CAPINAME, \ + RETURN) + +DEFINE_CAPI(HAL_Bool, Enabled, false) +DEFINE_CAPI(HAL_Bool, Autonomous, false) +DEFINE_CAPI(HAL_Bool, Test, false) +DEFINE_CAPI(HAL_Bool, EStop, false) +DEFINE_CAPI(HAL_Bool, FmsAttached, false) +DEFINE_CAPI(HAL_Bool, DsAttached, false) +DEFINE_CAPI(HAL_AllianceStationID, AllianceStationId, + HAL_AllianceStationID_kRed1) +DEFINE_CAPI(double, MatchTime, 0) + +#undef DEFINE_CAPI +#define DEFINE_CAPI(name, data) \ + int32_t HALSIM_RegisterJoystick##name##Callback( \ + int32_t joystickNum, HAL_Joystick##name##Callback callback, void* param, \ + HAL_Bool initialNotify) { \ + return 0; \ + } \ + \ + void HALSIM_CancelJoystick##name##Callback(int32_t uid) {} \ + \ + void HALSIM_GetJoystick##name(int32_t joystickNum, HAL_Joystick##name* d) {} \ + \ + void HALSIM_SetJoystick##name(int32_t joystickNum, \ + const HAL_Joystick##name* d) {} + +DEFINE_CAPI(Axes, axes) +DEFINE_CAPI(POVs, povs) +DEFINE_CAPI(Buttons, buttons) +DEFINE_CAPI(Descriptor, descriptor) + +int32_t HALSIM_RegisterJoystickOutputsCallback( + int32_t joystickNum, HAL_JoystickOutputsCallback callback, void* param, + HAL_Bool initialNotify) { + return 0; +} + +void HALSIM_CancelJoystickOutputsCallback(int32_t uid) {} + +void HALSIM_GetJoystickOutputs(int32_t joystickNum, int64_t* outputs, + int32_t* leftRumble, int32_t* rightRumble) {} + +void HALSIM_SetJoystickOutputs(int32_t joystickNum, int64_t outputs, + int32_t leftRumble, int32_t rightRumble) {} + +int32_t HALSIM_RegisterMatchInfoCallback(HAL_MatchInfoCallback callback, + void* param, HAL_Bool initialNotify) { + return 0; +} + +void HALSIM_CancelMatchInfoCallback(int32_t uid) {} + +void HALSIM_GetMatchInfo(HAL_MatchInfo* info) {} + +void HALSIM_SetMatchInfo(const HAL_MatchInfo* info) {} + +int32_t HALSIM_RegisterDriverStationNewDataCallback(HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify) { + return 0; +} + +void HALSIM_CancelDriverStationNewDataCallback(int32_t uid) {} + +void HALSIM_NotifyDriverStationNewData(void) {} + +void HALSIM_SetJoystickButton(int32_t stick, int32_t button, HAL_Bool state) {} + +void HALSIM_SetJoystickAxis(int32_t stick, int32_t axis, double value) {} + +void HALSIM_SetJoystickPOV(int32_t stick, int32_t pov, int32_t value) {} + +void HALSIM_SetJoystickButtonsValue(int32_t stick, uint32_t buttons) {} + +void HALSIM_SetJoystickAxisCount(int32_t stick, int32_t count) {} + +void HALSIM_SetJoystickPOVCount(int32_t stick, int32_t count) {} + +void HALSIM_SetJoystickButtonCount(int32_t stick, int32_t count) {} + +void HALSIM_GetJoystickCounts(int32_t stick, int32_t* axisCount, + int32_t* buttonCount, int32_t* povCount) { + *axisCount = 0; + *buttonCount = 0; + *povCount = 0; +} + +void HALSIM_SetJoystickIsXbox(int32_t stick, HAL_Bool isXbox) {} + +void HALSIM_SetJoystickType(int32_t stick, int32_t type) {} + +void HALSIM_SetJoystickName(int32_t stick, const struct WPI_String* name) {} + +void HALSIM_SetJoystickAxisType(int32_t stick, int32_t axis, int32_t type) {} + +void HALSIM_SetGameSpecificMessage(const struct WPI_String* message) {} + +void HALSIM_SetEventName(const struct WPI_String* name) {} + +void HALSIM_SetMatchType(HAL_MatchType type) {} + +void HALSIM_SetMatchNumber(int32_t matchNumber) {} + +void HALSIM_SetReplayNumber(int32_t replayNumber) {} + +void HALSIM_RegisterDriverStationAllCallbacks(HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify) {} +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/DutyCycleData.cpp b/hal/src/main/native/systemcore/mockdata/DutyCycleData.cpp new file mode 100644 index 00000000000..d63e27e273a --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/DutyCycleData.cpp @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/DutyCycleData.h" + +#include "hal/simulation/SimDataValue.h" + +extern "C" { +int32_t HALSIM_FindDutyCycleForChannel(int32_t channel) { + return 0; +} + +void HALSIM_ResetDutyCycleData(int32_t index) {} + +int32_t HALSIM_GetDutyCycleDigitalChannel(int32_t index) { + return 0; +} + +HAL_SimDeviceHandle HALSIM_GetDutyCycleSimDevice(int32_t index) { + return 0; +} + +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, DutyCycle##CAPINAME, RETURN) + +DEFINE_CAPI(HAL_Bool, Initialized, false) +DEFINE_CAPI(int32_t, Frequency, 0) +DEFINE_CAPI(double, Output, 0) + +void HALSIM_RegisterDutyCycleAllCallbacks(int32_t index, + HAL_NotifyCallback callback, + void* param, HAL_Bool initialNotify) { +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/EncoderData.cpp b/hal/src/main/native/systemcore/mockdata/EncoderData.cpp new file mode 100644 index 00000000000..4b07b311db7 --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/EncoderData.cpp @@ -0,0 +1,56 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/EncoderData.h" + +#include "hal/simulation/SimDataValue.h" + +extern "C" { +int32_t HALSIM_FindEncoderForChannel(int32_t channel) { + return 0; +} + +void HALSIM_ResetEncoderData(int32_t index) {} + +int32_t HALSIM_GetEncoderDigitalChannelA(int32_t index) { + return 0; +} + +int32_t HALSIM_GetEncoderDigitalChannelB(int32_t index) { + return 0; +} + +HAL_SimDeviceHandle HALSIM_GetEncoderSimDevice(int32_t index) { + return 0; +} + +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, Encoder##CAPINAME, RETURN) + +DEFINE_CAPI(HAL_Bool, Initialized, false) +DEFINE_CAPI(int32_t, Count, 0) +DEFINE_CAPI(double, Period, 0) +DEFINE_CAPI(HAL_Bool, Reset, false) +DEFINE_CAPI(double, MaxPeriod, 0) +DEFINE_CAPI(HAL_Bool, Direction, false) +DEFINE_CAPI(HAL_Bool, ReverseDirection, false) +DEFINE_CAPI(int32_t, SamplesToAverage, 0) +DEFINE_CAPI(double, DistancePerPulse, 0) + +void HALSIM_SetEncoderDistance(int32_t index, double distance) {} + +double HALSIM_GetEncoderDistance(int32_t index) { + return 0; +} + +void HALSIM_SetEncoderRate(int32_t index, double rate) {} + +double HALSIM_GetEncoderRate(int32_t index) { + return 0; +} + +void HALSIM_RegisterEncoderAllCallbacks(int32_t index, + HAL_NotifyCallback callback, + void* param, HAL_Bool initialNotify) {} +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/I2CData.cpp b/hal/src/main/native/systemcore/mockdata/I2CData.cpp new file mode 100644 index 00000000000..2f91061f95a --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/I2CData.cpp @@ -0,0 +1,24 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/I2CData.h" + +#include "hal/simulation/SimDataValue.h" + +extern "C" { +void HALSIM_ResetI2CData(int32_t index) {} + +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, I2C##CAPINAME, RETURN) + +DEFINE_CAPI(HAL_Bool, Initialized, false) + +#undef DEFINE_CAPI +#define DEFINE_CAPI(TYPE, CAPINAME) \ + HAL_SIMCALLBACKREGISTRY_STUB_CAPI(TYPE, HALSIM, I2C##CAPINAME) + +DEFINE_CAPI(HAL_BufferCallback, Read) +DEFINE_CAPI(HAL_ConstBufferCallback, Write) + +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/MockHooks.cpp b/hal/src/main/native/systemcore/mockdata/MockHooks.cpp new file mode 100644 index 00000000000..a6a680448ea --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/MockHooks.cpp @@ -0,0 +1,53 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/MockHooks.h" + +extern "C" { + +void HALSIM_SetRuntimeType(HAL_RuntimeType type) {} + +void HALSIM_WaitForProgramStart(void) {} + +void HALSIM_SetProgramStarted(void) {} + +HAL_Bool HALSIM_GetProgramStarted(void) { + return false; +} + +void HALSIM_RestartTiming(void) {} + +void HALSIM_PauseTiming(void) {} + +void HALSIM_ResumeTiming(void) {} + +HAL_Bool HALSIM_IsTimingPaused(void) { + return false; +} + +void HALSIM_StepTiming(uint64_t delta) {} + +void HALSIM_StepTimingAsync(uint64_t delta) {} + +void HALSIM_SetSendError(HALSIM_SendErrorHandler handler) {} + +void HALSIM_SetSendConsoleLine(HALSIM_SendConsoleLineHandler handler) {} + +int32_t HALSIM_RegisterSimPeriodicBeforeCallback( + HALSIM_SimPeriodicCallback callback, void* param) { + return 0; +} + +void HALSIM_CancelSimPeriodicBeforeCallback(int32_t uid) {} + +int32_t HALSIM_RegisterSimPeriodicAfterCallback( + HALSIM_SimPeriodicCallback callback, void* param) { + return 0; +} + +void HALSIM_CancelSimPeriodicAfterCallback(int32_t uid) {} + +void HALSIM_CancelAllSimPeriodicCallbacks(void) {} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/NotifierData.cpp b/hal/src/main/native/systemcore/mockdata/NotifierData.cpp new file mode 100644 index 00000000000..f75733e0fcd --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/NotifierData.cpp @@ -0,0 +1,21 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/NotifierData.h" + +extern "C" { + +uint64_t HALSIM_GetNextNotifierTimeout(void) { + return 0; +} + +int32_t HALSIM_GetNumNotifiers(void) { + return 0; +} + +int32_t HALSIM_GetNotifierInfo(struct HALSIM_NotifierInfo* arr, int32_t size) { + return 0; +} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/PWMData.cpp b/hal/src/main/native/systemcore/mockdata/PWMData.cpp new file mode 100644 index 00000000000..4fb6bcf13a0 --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/PWMData.cpp @@ -0,0 +1,24 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/PWMData.h" + +#include "hal/simulation/SimDataValue.h" + +extern "C" { +void HALSIM_ResetPWMData(int32_t index) {} + +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, PWM##CAPINAME, RETURN) + +DEFINE_CAPI(HAL_Bool, Initialized, false) +DEFINE_CAPI(int32_t, PulseMicrosecond, 0) +DEFINE_CAPI(double, Speed, 0) +DEFINE_CAPI(double, Position, 0) +DEFINE_CAPI(int32_t, PeriodScale, 0) +DEFINE_CAPI(HAL_Bool, ZeroLatch, false) + +void HALSIM_RegisterPWMAllCallbacks(int32_t index, HAL_NotifyCallback callback, + void* param, HAL_Bool initialNotify) {} +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/PowerDistributionData.cpp b/hal/src/main/native/systemcore/mockdata/PowerDistributionData.cpp new file mode 100644 index 00000000000..a44e2222145 --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/PowerDistributionData.cpp @@ -0,0 +1,35 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/PowerDistributionData.h" + +#include "../PortsInternal.h" +#include "hal/simulation/SimDataValue.h" + +extern "C" { +void HALSIM_ResetPowerDistributionData(int32_t index) {} + +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, PowerDistribution##CAPINAME, RETURN) + +DEFINE_CAPI(HAL_Bool, Initialized, false) +DEFINE_CAPI(double, Temperature, 0) +DEFINE_CAPI(double, Voltage, 0) +HAL_SIMDATAVALUE_STUB_CAPI_CHANNEL(double, HALSIM, PowerDistributionCurrent, 0) + +void HALSIM_GetPowerDistributionAllCurrents(int32_t index, double* currents, + int length) { + for (int i = 0; i < length; i++) { + currents[i] = 0; + } +} + +void HALSIM_SetPowerDistributionAllCurrents(int32_t index, + const double* currents, + int length) {} + +void HALSIM_RegisterPowerDistributionAllNonCurrentCallbacks( + int32_t index, int32_t channel, HAL_NotifyCallback callback, void* param, + HAL_Bool initialNotify) {} +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/REVPHData.cpp b/hal/src/main/native/systemcore/mockdata/REVPHData.cpp new file mode 100644 index 00000000000..f2c4e85ae34 --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/REVPHData.cpp @@ -0,0 +1,38 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/REVPHData.h" + +#include "hal/simulation/SimDataValue.h" + +extern "C" { +void HALSIM_ResetREVPHData(int32_t index) {} + +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, REVPH##CAPINAME, RETURN) + +HAL_SIMDATAVALUE_STUB_CAPI_CHANNEL(HAL_Bool, HALSIM, REVPHSolenoidOutput, false) +DEFINE_CAPI(HAL_Bool, Initialized, false) +DEFINE_CAPI(HAL_Bool, CompressorOn, false) +DEFINE_CAPI(HAL_REVPHCompressorConfigType, CompressorConfigType, + HAL_REVPHCompressorConfigType_kDisabled) +DEFINE_CAPI(HAL_Bool, PressureSwitch, false) +DEFINE_CAPI(double, CompressorCurrent, 0) + +void HALSIM_GetREVPHAllSolenoids(int32_t index, uint8_t* values) { + *values = 0; +} + +void HALSIM_SetREVPHAllSolenoids(int32_t index, uint8_t values) {} + +void HALSIM_RegisterREVPHAllNonSolenoidCallbacks(int32_t index, + HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify) {} + +void HALSIM_RegisterREVPHAllSolenoidCallbacks(int32_t index, int32_t channel, + HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify) {} +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/RelayData.cpp b/hal/src/main/native/systemcore/mockdata/RelayData.cpp new file mode 100644 index 00000000000..7d8d439be7a --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/RelayData.cpp @@ -0,0 +1,23 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/RelayData.h" + +#include "hal/simulation/SimDataValue.h" + +extern "C" { +void HALSIM_ResetRelayData(int32_t index) {} + +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, Relay##CAPINAME, RETURN) + +DEFINE_CAPI(HAL_Bool, InitializedForward, false) +DEFINE_CAPI(HAL_Bool, InitializedReverse, false) +DEFINE_CAPI(HAL_Bool, Forward, false) +DEFINE_CAPI(HAL_Bool, Reverse, false) + +void HALSIM_RegisterRelayAllCallbacks(int32_t index, + HAL_NotifyCallback callback, void* param, + HAL_Bool initialNotify) {} +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/Reset.cpp b/hal/src/main/native/systemcore/mockdata/Reset.cpp new file mode 100644 index 00000000000..7cbeea36d77 --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/Reset.cpp @@ -0,0 +1,5 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +extern "C" void HALSIM_ResetAllSimData(void) {} diff --git a/hal/src/main/native/systemcore/mockdata/RoboRioData.cpp b/hal/src/main/native/systemcore/mockdata/RoboRioData.cpp new file mode 100644 index 00000000000..b1ba26026da --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/RoboRioData.cpp @@ -0,0 +1,57 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/RoboRioData.h" + +#include "hal/simulation/SimDataValue.h" + +extern "C" { +void HALSIM_ResetRoboRioData(void) {} + +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI_NOINDEX(TYPE, HALSIM, RoboRio##CAPINAME, RETURN) + +DEFINE_CAPI(HAL_Bool, FPGAButton, false) +DEFINE_CAPI(double, VInVoltage, 0) +DEFINE_CAPI(double, VInCurrent, 0) +DEFINE_CAPI(double, UserVoltage6V, 0) +DEFINE_CAPI(double, UserCurrent6V, 0) +DEFINE_CAPI(HAL_Bool, UserActive6V, false) +DEFINE_CAPI(double, UserVoltage5V, 0) +DEFINE_CAPI(double, UserCurrent5V, 0) +DEFINE_CAPI(HAL_Bool, UserActive5V, false) +DEFINE_CAPI(double, UserVoltage3V3, 0) +DEFINE_CAPI(double, UserCurrent3V3, 0) +DEFINE_CAPI(HAL_Bool, UserActive3V3, false) +DEFINE_CAPI(int32_t, UserFaults6V, 0) +DEFINE_CAPI(int32_t, UserFaults5V, 0) +DEFINE_CAPI(int32_t, UserFaults3V3, 0) +DEFINE_CAPI(double, BrownoutVoltage, 6.75) +DEFINE_CAPI(double, CPUTemp, 45.0) +DEFINE_CAPI(int32_t, TeamNumber, 0) +DEFINE_CAPI(HAL_RadioLEDState, RadioLEDState, HAL_RadioLED_kOff); + +int32_t HALSIM_RegisterRoboRioSerialNumberCallback( + HAL_RoboRioStringCallback callback, void* param, HAL_Bool initialNotify) { + return 0; +} +void HALSIM_CancelRoboRioSerialNumberCallback(int32_t uid) {} +void HALSIM_GetRoboRioSerialNumber(struct WPI_String* serialNumber) { + WPI_AllocateString(serialNumber, 0); +} +void HALSIM_SetRoboRioSerialNumber(const struct WPI_String* serialNumber) {} + +int32_t HALSIM_RegisterRoboRioCommentsCallback( + HAL_RoboRioStringCallback callback, void* param, HAL_Bool initialNotify) { + return 0; +} +void HALSIM_CancelRoboRioCommentsCallback(int32_t uid) {} +void HALSIM_GetRoboRioComments(struct WPI_String* comments) { + WPI_AllocateString(comments, 0); +} +void HALSIM_SetRoboRioComments(const struct WPI_String* comments) {} + +void HALSIM_RegisterRoboRioAllCallbacks(HAL_NotifyCallback callback, + void* param, HAL_Bool initialNotify) {} +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/SPIAccelerometerData.cpp b/hal/src/main/native/systemcore/mockdata/SPIAccelerometerData.cpp new file mode 100644 index 00000000000..d0aa97f1de3 --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/SPIAccelerometerData.cpp @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/SPIAccelerometerData.h" + +#include "hal/simulation/SimDataValue.h" + +extern "C" { +void HALSIM_ResetSPIAccelerometerData(int32_t index) {} + +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, SPIAccelerometer##CAPINAME, RETURN) + +DEFINE_CAPI(HAL_Bool, Active, false) +DEFINE_CAPI(int32_t, Range, 0) +DEFINE_CAPI(double, X, 0) +DEFINE_CAPI(double, Y, 0) +DEFINE_CAPI(double, Z, 0) + +void HALSIM_RegisterSPIAccelerometerAllCallbacks(int32_t index, + HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify) {} +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/SPIData.cpp b/hal/src/main/native/systemcore/mockdata/SPIData.cpp new file mode 100644 index 00000000000..433ca100ead --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/SPIData.cpp @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/SPIData.h" + +#include "hal/simulation/SimDataValue.h" + +extern "C" { +void HALSIM_ResetSPIData(int32_t index) {} + +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, SPI##CAPINAME, RETURN) + +DEFINE_CAPI(HAL_Bool, Initialized, false) + +#undef DEFINE_CAPI +#define DEFINE_CAPI(TYPE, CAPINAME) \ + HAL_SIMCALLBACKREGISTRY_STUB_CAPI(TYPE, HALSIM, SPI##CAPINAME) + +DEFINE_CAPI(HAL_BufferCallback, Read) +DEFINE_CAPI(HAL_ConstBufferCallback, Write) +DEFINE_CAPI(HAL_SpiReadAutoReceiveBufferCallback, ReadAutoReceivedData) + +} // extern "C" diff --git a/hal/src/main/native/systemcore/mockdata/SimDeviceData.cpp b/hal/src/main/native/systemcore/mockdata/SimDeviceData.cpp new file mode 100644 index 00000000000..08bd8ce9b80 --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/SimDeviceData.cpp @@ -0,0 +1,97 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/simulation/SimDeviceData.h" + +#include "hal/simulation/SimDataValue.h" + +extern "C" { + +void HALSIM_SetSimDeviceEnabled(const char* prefix, HAL_Bool enabled) {} + +HAL_Bool HALSIM_IsSimDeviceEnabled(const char* name) { + return false; +} + +int32_t HALSIM_RegisterSimDeviceCreatedCallback( + const char* prefix, void* param, HALSIM_SimDeviceCallback callback, + HAL_Bool initialNotify) { + return 0; +} + +void HALSIM_CancelSimDeviceCreatedCallback(int32_t uid) {} + +int32_t HALSIM_RegisterSimDeviceFreedCallback(const char* prefix, void* param, + HALSIM_SimDeviceCallback callback, + HAL_Bool initialNotify) { + return 0; +} + +void HALSIM_CancelSimDeviceFreedCallback(int32_t uid) {} + +HAL_SimDeviceHandle HALSIM_GetSimDeviceHandle(const char* name) { + return 0; +} + +const char* HALSIM_GetSimDeviceName(HAL_SimDeviceHandle handle) { + return ""; +} + +HAL_SimDeviceHandle HALSIM_GetSimValueDeviceHandle(HAL_SimValueHandle handle) { + return 0; +} + +void HALSIM_EnumerateSimDevices(const char* prefix, void* param, + HALSIM_SimDeviceCallback callback) {} + +int32_t HALSIM_RegisterSimValueCreatedCallback(HAL_SimDeviceHandle device, + void* param, + HALSIM_SimValueCallback callback, + HAL_Bool initialNotify) { + return 0; +} + +void HALSIM_CancelSimValueCreatedCallback(int32_t uid) {} + +int32_t HALSIM_RegisterSimValueChangedCallback(HAL_SimValueHandle handle, + void* param, + HALSIM_SimValueCallback callback, + HAL_Bool initialNotify) { + return 0; +} + +void HALSIM_CancelSimValueChangedCallback(int32_t uid) {} + +int32_t HALSIM_RegisterSimValueResetCallback(HAL_SimValueHandle handle, + void* param, + HALSIM_SimValueCallback callback, + HAL_Bool initialNotify) { + return 0; +} + +void HALSIM_CancelSimValueResetCallback(int32_t uid) {} + +HAL_SimValueHandle HALSIM_GetSimValueHandle(HAL_SimDeviceHandle device, + const char* name) { + return 0; +} + +void HALSIM_EnumerateSimValues(HAL_SimDeviceHandle device, void* param, + HALSIM_SimValueCallback callback) {} + +const char** HALSIM_GetSimValueEnumOptions(HAL_SimValueHandle handle, + int32_t* numOptions) { + *numOptions = 0; + return nullptr; +} + +const double* HALSIM_GetSimValueEnumDoubleValues(HAL_SimValueHandle handle, + int32_t* numOptions) { + *numOptions = 0; + return nullptr; +} + +void HALSIM_ResetSimDeviceData(void) {} + +} // extern "C" diff --git a/hal/src/main/native/systemcore/rev/PDHFrames.cpp b/hal/src/main/native/systemcore/rev/PDHFrames.cpp new file mode 100644 index 00000000000..eaf33bdfcfd --- /dev/null +++ b/hal/src/main/native/systemcore/rev/PDHFrames.cpp @@ -0,0 +1,2014 @@ +/** + * The MIT License (MIT) + * + * Copyright (c) 2018-2019 Erik Moqvist + * + * 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. + */ + +/** + * This file was generated by cantools version + */ + +#include + +#include "PDHFrames.h" + +static inline uint8_t pack_left_shift_u8( + uint8_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint8_t)((uint8_t)(value << shift) & mask); +} + +static inline uint8_t pack_left_shift_u16( + uint16_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint8_t)((uint8_t)(value << shift) & mask); +} + +static inline uint8_t pack_left_shift_u32( + uint32_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint8_t)((uint8_t)(value << shift) & mask); +} + +static inline uint8_t pack_right_shift_u16( + uint16_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint8_t)((uint8_t)(value >> shift) & mask); +} + +static inline uint8_t pack_right_shift_u32( + uint32_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint8_t)((uint8_t)(value >> shift) & mask); +} + +static inline uint16_t unpack_left_shift_u16( + uint8_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint16_t)((uint16_t)(value & mask) << shift); +} + +static inline uint32_t unpack_left_shift_u32( + uint8_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint32_t)((uint32_t)(value & mask) << shift); +} + +static inline uint8_t unpack_right_shift_u8( + uint8_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint8_t)((uint8_t)(value & mask) >> shift); +} + +static inline uint16_t unpack_right_shift_u16( + uint8_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint16_t)((uint16_t)(value & mask) >> shift); +} + +static inline uint32_t unpack_right_shift_u32( + uint8_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint32_t)((uint32_t)(value & mask) >> shift); +} + +int PDH_set_switch_channel_pack( + uint8_t *dst_p, + const struct PDH_set_switch_channel_t *src_p, + size_t size) +{ + if (size < 1u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 1); + + dst_p[0] |= pack_left_shift_u8(src_p->output_set_value, 0u, 0x01u); + + return (1); +} + +int PDH_set_switch_channel_unpack( + struct PDH_set_switch_channel_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 1u) { + return (-EINVAL); + } + + dst_p->output_set_value = unpack_right_shift_u8(src_p[0], 0u, 0x01u); + + return (0); +} + +uint8_t PDH_set_switch_channel_output_set_value_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_set_switch_channel_output_set_value_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_set_switch_channel_output_set_value_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +int PDH_status_0_pack( + uint8_t *dst_p, + const struct PDH_status_0_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 8); + + dst_p[0] |= pack_left_shift_u16(src_p->channel_0_current, 0u, 0xffu); + dst_p[1] |= pack_right_shift_u16(src_p->channel_0_current, 8u, 0x03u); + dst_p[1] |= pack_left_shift_u16(src_p->channel_1_current, 2u, 0xfcu); + dst_p[2] |= pack_right_shift_u16(src_p->channel_1_current, 6u, 0x0fu); + dst_p[2] |= pack_left_shift_u16(src_p->channel_2_current, 4u, 0xf0u); + dst_p[3] |= pack_right_shift_u16(src_p->channel_2_current, 4u, 0x3fu); + dst_p[3] |= pack_left_shift_u8(src_p->channel_0_breaker_fault, 6u, 0x40u); + dst_p[3] |= pack_left_shift_u8(src_p->channel_1_breaker_fault, 7u, 0x80u); + dst_p[4] |= pack_left_shift_u16(src_p->channel_3_current, 0u, 0xffu); + dst_p[5] |= pack_right_shift_u16(src_p->channel_3_current, 8u, 0x03u); + dst_p[5] |= pack_left_shift_u16(src_p->channel_4_current, 2u, 0xfcu); + dst_p[6] |= pack_right_shift_u16(src_p->channel_4_current, 6u, 0x0fu); + dst_p[6] |= pack_left_shift_u16(src_p->channel_5_current, 4u, 0xf0u); + dst_p[7] |= pack_right_shift_u16(src_p->channel_5_current, 4u, 0x3fu); + dst_p[7] |= pack_left_shift_u8(src_p->channel_2_breaker_fault, 6u, 0x40u); + dst_p[7] |= pack_left_shift_u8(src_p->channel_3_breaker_fault, 7u, 0x80u); + + return (8); +} + +int PDH_status_0_unpack( + struct PDH_status_0_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + dst_p->channel_0_current = unpack_right_shift_u16(src_p[0], 0u, 0xffu); + dst_p->channel_0_current |= unpack_left_shift_u16(src_p[1], 8u, 0x03u); + dst_p->channel_1_current = unpack_right_shift_u16(src_p[1], 2u, 0xfcu); + dst_p->channel_1_current |= unpack_left_shift_u16(src_p[2], 6u, 0x0fu); + dst_p->channel_2_current = unpack_right_shift_u16(src_p[2], 4u, 0xf0u); + dst_p->channel_2_current |= unpack_left_shift_u16(src_p[3], 4u, 0x3fu); + dst_p->channel_0_breaker_fault = unpack_right_shift_u8(src_p[3], 6u, 0x40u); + dst_p->channel_1_breaker_fault = unpack_right_shift_u8(src_p[3], 7u, 0x80u); + dst_p->channel_3_current = unpack_right_shift_u16(src_p[4], 0u, 0xffu); + dst_p->channel_3_current |= unpack_left_shift_u16(src_p[5], 8u, 0x03u); + dst_p->channel_4_current = unpack_right_shift_u16(src_p[5], 2u, 0xfcu); + dst_p->channel_4_current |= unpack_left_shift_u16(src_p[6], 6u, 0x0fu); + dst_p->channel_5_current = unpack_right_shift_u16(src_p[6], 4u, 0xf0u); + dst_p->channel_5_current |= unpack_left_shift_u16(src_p[7], 4u, 0x3fu); + dst_p->channel_2_breaker_fault = unpack_right_shift_u8(src_p[7], 6u, 0x40u); + dst_p->channel_3_breaker_fault = unpack_right_shift_u8(src_p[7], 7u, 0x80u); + + return (0); +} + +uint16_t PDH_status_0_channel_0_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status_0_channel_0_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status_0_channel_0_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status_0_channel_1_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status_0_channel_1_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status_0_channel_1_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status_0_channel_2_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status_0_channel_2_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status_0_channel_2_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint8_t PDH_status_0_channel_0_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_0_channel_0_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_0_channel_0_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_0_channel_1_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_0_channel_1_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_0_channel_1_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint16_t PDH_status_0_channel_3_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status_0_channel_3_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status_0_channel_3_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status_0_channel_4_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status_0_channel_4_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status_0_channel_4_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status_0_channel_5_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status_0_channel_5_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status_0_channel_5_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint8_t PDH_status_0_channel_2_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_0_channel_2_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_0_channel_2_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_0_channel_3_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_0_channel_3_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_0_channel_3_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +int PDH_status_1_pack( + uint8_t *dst_p, + const struct PDH_status_1_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 8); + + dst_p[0] |= pack_left_shift_u16(src_p->channel_6_current, 0u, 0xffu); + dst_p[1] |= pack_right_shift_u16(src_p->channel_6_current, 8u, 0x03u); + dst_p[1] |= pack_left_shift_u16(src_p->channel_7_current, 2u, 0xfcu); + dst_p[2] |= pack_right_shift_u16(src_p->channel_7_current, 6u, 0x0fu); + dst_p[2] |= pack_left_shift_u16(src_p->channel_8_current, 4u, 0xf0u); + dst_p[3] |= pack_right_shift_u16(src_p->channel_8_current, 4u, 0x3fu); + dst_p[3] |= pack_left_shift_u8(src_p->channel_4_breaker_fault, 6u, 0x40u); + dst_p[3] |= pack_left_shift_u8(src_p->channel_5_breaker_fault, 7u, 0x80u); + dst_p[4] |= pack_left_shift_u16(src_p->channel_9_current, 0u, 0xffu); + dst_p[5] |= pack_right_shift_u16(src_p->channel_9_current, 8u, 0x03u); + dst_p[5] |= pack_left_shift_u16(src_p->channel_10_current, 2u, 0xfcu); + dst_p[6] |= pack_right_shift_u16(src_p->channel_10_current, 6u, 0x0fu); + dst_p[6] |= pack_left_shift_u16(src_p->channel_11_current, 4u, 0xf0u); + dst_p[7] |= pack_right_shift_u16(src_p->channel_11_current, 4u, 0x3fu); + dst_p[7] |= pack_left_shift_u8(src_p->channel_6_breaker_fault, 6u, 0x40u); + dst_p[7] |= pack_left_shift_u8(src_p->channel_7_breaker_fault, 7u, 0x80u); + + return (8); +} + +int PDH_status_1_unpack( + struct PDH_status_1_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + dst_p->channel_6_current = unpack_right_shift_u16(src_p[0], 0u, 0xffu); + dst_p->channel_6_current |= unpack_left_shift_u16(src_p[1], 8u, 0x03u); + dst_p->channel_7_current = unpack_right_shift_u16(src_p[1], 2u, 0xfcu); + dst_p->channel_7_current |= unpack_left_shift_u16(src_p[2], 6u, 0x0fu); + dst_p->channel_8_current = unpack_right_shift_u16(src_p[2], 4u, 0xf0u); + dst_p->channel_8_current |= unpack_left_shift_u16(src_p[3], 4u, 0x3fu); + dst_p->channel_4_breaker_fault = unpack_right_shift_u8(src_p[3], 6u, 0x40u); + dst_p->channel_5_breaker_fault = unpack_right_shift_u8(src_p[3], 7u, 0x80u); + dst_p->channel_9_current = unpack_right_shift_u16(src_p[4], 0u, 0xffu); + dst_p->channel_9_current |= unpack_left_shift_u16(src_p[5], 8u, 0x03u); + dst_p->channel_10_current = unpack_right_shift_u16(src_p[5], 2u, 0xfcu); + dst_p->channel_10_current |= unpack_left_shift_u16(src_p[6], 6u, 0x0fu); + dst_p->channel_11_current = unpack_right_shift_u16(src_p[6], 4u, 0xf0u); + dst_p->channel_11_current |= unpack_left_shift_u16(src_p[7], 4u, 0x3fu); + dst_p->channel_6_breaker_fault = unpack_right_shift_u8(src_p[7], 6u, 0x40u); + dst_p->channel_7_breaker_fault = unpack_right_shift_u8(src_p[7], 7u, 0x80u); + + return (0); +} + +uint16_t PDH_status_1_channel_6_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status_1_channel_6_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status_1_channel_6_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status_1_channel_7_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status_1_channel_7_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status_1_channel_7_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status_1_channel_8_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status_1_channel_8_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status_1_channel_8_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint8_t PDH_status_1_channel_4_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_1_channel_4_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_1_channel_4_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_1_channel_5_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_1_channel_5_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_1_channel_5_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint16_t PDH_status_1_channel_9_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status_1_channel_9_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status_1_channel_9_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status_1_channel_10_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status_1_channel_10_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status_1_channel_10_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status_1_channel_11_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status_1_channel_11_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status_1_channel_11_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint8_t PDH_status_1_channel_6_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_1_channel_6_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_1_channel_6_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_1_channel_7_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_1_channel_7_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_1_channel_7_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +int PDH_status_2_pack( + uint8_t *dst_p, + const struct PDH_status_2_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 8); + + dst_p[0] |= pack_left_shift_u16(src_p->channel_12_current, 0u, 0xffu); + dst_p[1] |= pack_right_shift_u16(src_p->channel_12_current, 8u, 0x03u); + dst_p[1] |= pack_left_shift_u16(src_p->channel_13_current, 2u, 0xfcu); + dst_p[2] |= pack_right_shift_u16(src_p->channel_13_current, 6u, 0x0fu); + dst_p[2] |= pack_left_shift_u16(src_p->channel_14_current, 4u, 0xf0u); + dst_p[3] |= pack_right_shift_u16(src_p->channel_14_current, 4u, 0x3fu); + dst_p[3] |= pack_left_shift_u8(src_p->channel_8_breaker_fault, 6u, 0x40u); + dst_p[3] |= pack_left_shift_u8(src_p->channel_9_breaker_fault, 7u, 0x80u); + dst_p[4] |= pack_left_shift_u16(src_p->channel_15_current, 0u, 0xffu); + dst_p[5] |= pack_right_shift_u16(src_p->channel_15_current, 8u, 0x03u); + dst_p[5] |= pack_left_shift_u16(src_p->channel_16_current, 2u, 0xfcu); + dst_p[6] |= pack_right_shift_u16(src_p->channel_16_current, 6u, 0x0fu); + dst_p[6] |= pack_left_shift_u16(src_p->channel_17_current, 4u, 0xf0u); + dst_p[7] |= pack_right_shift_u16(src_p->channel_17_current, 4u, 0x3fu); + dst_p[7] |= pack_left_shift_u8(src_p->channel_10_breaker_fault, 6u, 0x40u); + dst_p[7] |= pack_left_shift_u8(src_p->channel_11_breaker_fault, 7u, 0x80u); + + return (8); +} + +int PDH_status_2_unpack( + struct PDH_status_2_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + dst_p->channel_12_current = unpack_right_shift_u16(src_p[0], 0u, 0xffu); + dst_p->channel_12_current |= unpack_left_shift_u16(src_p[1], 8u, 0x03u); + dst_p->channel_13_current = unpack_right_shift_u16(src_p[1], 2u, 0xfcu); + dst_p->channel_13_current |= unpack_left_shift_u16(src_p[2], 6u, 0x0fu); + dst_p->channel_14_current = unpack_right_shift_u16(src_p[2], 4u, 0xf0u); + dst_p->channel_14_current |= unpack_left_shift_u16(src_p[3], 4u, 0x3fu); + dst_p->channel_8_breaker_fault = unpack_right_shift_u8(src_p[3], 6u, 0x40u); + dst_p->channel_9_breaker_fault = unpack_right_shift_u8(src_p[3], 7u, 0x80u); + dst_p->channel_15_current = unpack_right_shift_u16(src_p[4], 0u, 0xffu); + dst_p->channel_15_current |= unpack_left_shift_u16(src_p[5], 8u, 0x03u); + dst_p->channel_16_current = unpack_right_shift_u16(src_p[5], 2u, 0xfcu); + dst_p->channel_16_current |= unpack_left_shift_u16(src_p[6], 6u, 0x0fu); + dst_p->channel_17_current = unpack_right_shift_u16(src_p[6], 4u, 0xf0u); + dst_p->channel_17_current |= unpack_left_shift_u16(src_p[7], 4u, 0x3fu); + dst_p->channel_10_breaker_fault = unpack_right_shift_u8(src_p[7], 6u, 0x40u); + dst_p->channel_11_breaker_fault = unpack_right_shift_u8(src_p[7], 7u, 0x80u); + + return (0); +} + +uint16_t PDH_status_2_channel_12_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status_2_channel_12_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status_2_channel_12_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status_2_channel_13_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status_2_channel_13_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status_2_channel_13_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status_2_channel_14_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status_2_channel_14_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status_2_channel_14_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint8_t PDH_status_2_channel_8_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_2_channel_8_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_2_channel_8_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_2_channel_9_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_2_channel_9_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_2_channel_9_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint16_t PDH_status_2_channel_15_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status_2_channel_15_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status_2_channel_15_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status_2_channel_16_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status_2_channel_16_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status_2_channel_16_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status_2_channel_17_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status_2_channel_17_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status_2_channel_17_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint8_t PDH_status_2_channel_10_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_2_channel_10_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_2_channel_10_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_2_channel_11_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_2_channel_11_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_2_channel_11_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +int PDH_status_3_pack( + uint8_t *dst_p, + const struct PDH_status_3_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 8); + + dst_p[0] |= pack_left_shift_u16(src_p->channel_18_current, 0u, 0xffu); + dst_p[1] |= pack_right_shift_u16(src_p->channel_18_current, 8u, 0x03u); + dst_p[1] |= pack_left_shift_u16(src_p->channel_19_current, 2u, 0xfcu); + dst_p[2] |= pack_right_shift_u16(src_p->channel_19_current, 6u, 0x0fu); + dst_p[2] |= pack_left_shift_u8(src_p->channel_12_breaker_fault, 4u, 0x10u); + dst_p[2] |= pack_left_shift_u8(src_p->channel_13_breaker_fault, 5u, 0x20u); + dst_p[2] |= pack_left_shift_u8(src_p->channel_14_breaker_fault, 6u, 0x40u); + dst_p[2] |= pack_left_shift_u8(src_p->channel_15_breaker_fault, 7u, 0x80u); + dst_p[3] |= pack_left_shift_u8(src_p->channel_20_current, 0u, 0xffu); + dst_p[4] |= pack_left_shift_u8(src_p->channel_21_current, 0u, 0xffu); + dst_p[5] |= pack_left_shift_u8(src_p->channel_22_current, 0u, 0xffu); + dst_p[6] |= pack_left_shift_u8(src_p->channel_23_current, 0u, 0xffu); + dst_p[7] |= pack_left_shift_u8(src_p->channel_16_breaker_fault, 0u, 0x01u); + dst_p[7] |= pack_left_shift_u8(src_p->channel_17_breaker_fault, 1u, 0x02u); + dst_p[7] |= pack_left_shift_u8(src_p->channel_18_breaker_fault, 2u, 0x04u); + dst_p[7] |= pack_left_shift_u8(src_p->channel_19_breaker_fault, 3u, 0x08u); + dst_p[7] |= pack_left_shift_u8(src_p->channel_20_breaker_fault, 4u, 0x10u); + dst_p[7] |= pack_left_shift_u8(src_p->channel_21_breaker_fault, 5u, 0x20u); + dst_p[7] |= pack_left_shift_u8(src_p->channel_22_breaker_fault, 6u, 0x40u); + dst_p[7] |= pack_left_shift_u8(src_p->channel_23_breaker_fault, 7u, 0x80u); + + return (8); +} + +int PDH_status_3_unpack( + struct PDH_status_3_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + dst_p->channel_18_current = unpack_right_shift_u16(src_p[0], 0u, 0xffu); + dst_p->channel_18_current |= unpack_left_shift_u16(src_p[1], 8u, 0x03u); + dst_p->channel_19_current = unpack_right_shift_u16(src_p[1], 2u, 0xfcu); + dst_p->channel_19_current |= unpack_left_shift_u16(src_p[2], 6u, 0x0fu); + dst_p->channel_12_breaker_fault = unpack_right_shift_u8(src_p[2], 4u, 0x10u); + dst_p->channel_13_breaker_fault = unpack_right_shift_u8(src_p[2], 5u, 0x20u); + dst_p->channel_14_breaker_fault = unpack_right_shift_u8(src_p[2], 6u, 0x40u); + dst_p->channel_15_breaker_fault = unpack_right_shift_u8(src_p[2], 7u, 0x80u); + dst_p->channel_20_current = unpack_right_shift_u8(src_p[3], 0u, 0xffu); + dst_p->channel_21_current = unpack_right_shift_u8(src_p[4], 0u, 0xffu); + dst_p->channel_22_current = unpack_right_shift_u8(src_p[5], 0u, 0xffu); + dst_p->channel_23_current = unpack_right_shift_u8(src_p[6], 0u, 0xffu); + dst_p->channel_16_breaker_fault = unpack_right_shift_u8(src_p[7], 0u, 0x01u); + dst_p->channel_17_breaker_fault = unpack_right_shift_u8(src_p[7], 1u, 0x02u); + dst_p->channel_18_breaker_fault = unpack_right_shift_u8(src_p[7], 2u, 0x04u); + dst_p->channel_19_breaker_fault = unpack_right_shift_u8(src_p[7], 3u, 0x08u); + dst_p->channel_20_breaker_fault = unpack_right_shift_u8(src_p[7], 4u, 0x10u); + dst_p->channel_21_breaker_fault = unpack_right_shift_u8(src_p[7], 5u, 0x20u); + dst_p->channel_22_breaker_fault = unpack_right_shift_u8(src_p[7], 6u, 0x40u); + dst_p->channel_23_breaker_fault = unpack_right_shift_u8(src_p[7], 7u, 0x80u); + + return (0); +} + +uint16_t PDH_status_3_channel_18_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status_3_channel_18_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status_3_channel_18_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status_3_channel_19_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status_3_channel_19_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status_3_channel_19_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint8_t PDH_status_3_channel_12_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_3_channel_12_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_3_channel_12_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_3_channel_13_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_3_channel_13_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_3_channel_13_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_3_channel_14_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_3_channel_14_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_3_channel_14_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_3_channel_15_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_3_channel_15_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_3_channel_15_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_3_channel_20_current_encode(double value) +{ + return (uint8_t)(value / 0.0625); +} + +double PDH_status_3_channel_20_current_decode(uint8_t value) +{ + return ((double)value * 0.0625); +} + +bool PDH_status_3_channel_20_current_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PDH_status_3_channel_21_current_encode(double value) +{ + return (uint8_t)(value / 0.0625); +} + +double PDH_status_3_channel_21_current_decode(uint8_t value) +{ + return ((double)value * 0.0625); +} + +bool PDH_status_3_channel_21_current_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PDH_status_3_channel_22_current_encode(double value) +{ + return (uint8_t)(value / 0.0625); +} + +double PDH_status_3_channel_22_current_decode(uint8_t value) +{ + return ((double)value * 0.0625); +} + +bool PDH_status_3_channel_22_current_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PDH_status_3_channel_23_current_encode(double value) +{ + return (uint8_t)(value / 0.0625); +} + +double PDH_status_3_channel_23_current_decode(uint8_t value) +{ + return ((double)value * 0.0625); +} + +bool PDH_status_3_channel_23_current_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PDH_status_3_channel_16_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_3_channel_16_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_3_channel_16_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_3_channel_17_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_3_channel_17_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_3_channel_17_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_3_channel_18_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_3_channel_18_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_3_channel_18_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_3_channel_19_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_3_channel_19_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_3_channel_19_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_3_channel_20_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_3_channel_20_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_3_channel_20_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_3_channel_21_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_3_channel_21_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_3_channel_21_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_3_channel_22_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_3_channel_22_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_3_channel_22_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_3_channel_23_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_3_channel_23_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_3_channel_23_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +int PDH_status_4_pack( + uint8_t *dst_p, + const struct PDH_status_4_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 8); + + dst_p[0] |= pack_left_shift_u16(src_p->v_bus, 0u, 0xffu); + dst_p[1] |= pack_right_shift_u16(src_p->v_bus, 8u, 0x0fu); + dst_p[1] |= pack_left_shift_u8(src_p->system_enable, 4u, 0x10u); + dst_p[1] |= pack_left_shift_u8(src_p->rsvd0, 5u, 0xe0u); + dst_p[2] |= pack_left_shift_u8(src_p->brownout_fault, 0u, 0x01u); + dst_p[2] |= pack_left_shift_u8(src_p->rsvd1, 1u, 0x02u); + dst_p[2] |= pack_left_shift_u8(src_p->can_warning_fault, 2u, 0x04u); + dst_p[2] |= pack_left_shift_u8(src_p->hardware_fault, 3u, 0x08u); + dst_p[2] |= pack_left_shift_u8(src_p->switch_channel_state, 4u, 0x10u); + dst_p[2] |= pack_left_shift_u8(src_p->sticky_brownout_fault, 5u, 0x20u); + dst_p[2] |= pack_left_shift_u8(src_p->rsvd2, 6u, 0x40u); + dst_p[2] |= pack_left_shift_u8(src_p->sticky_can_warning_fault, 7u, 0x80u); + dst_p[3] |= pack_left_shift_u8(src_p->sticky_can_bus_off_fault, 0u, 0x01u); + dst_p[3] |= pack_left_shift_u8(src_p->sticky_hardware_fault, 1u, 0x02u); + dst_p[3] |= pack_left_shift_u8(src_p->sticky_firmware_fault, 2u, 0x04u); + dst_p[3] |= pack_left_shift_u8(src_p->sticky_ch20_breaker_fault, 3u, 0x08u); + dst_p[3] |= pack_left_shift_u8(src_p->sticky_ch21_breaker_fault, 4u, 0x10u); + dst_p[3] |= pack_left_shift_u8(src_p->sticky_ch22_breaker_fault, 5u, 0x20u); + dst_p[3] |= pack_left_shift_u8(src_p->sticky_ch23_breaker_fault, 6u, 0x40u); + dst_p[3] |= pack_left_shift_u8(src_p->sticky_has_reset_fault, 7u, 0x80u); + dst_p[4] |= pack_left_shift_u8(src_p->total_current, 0u, 0xffu); + dst_p[5] |= pack_left_shift_u8(src_p->sticky_ch0_breaker_fault, 0u, 0x01u); + dst_p[5] |= pack_left_shift_u8(src_p->sticky_ch1_breaker_fault, 1u, 0x02u); + dst_p[5] |= pack_left_shift_u8(src_p->sticky_ch2_breaker_fault, 2u, 0x04u); + dst_p[5] |= pack_left_shift_u8(src_p->sticky_ch3_breaker_fault, 3u, 0x08u); + dst_p[5] |= pack_left_shift_u8(src_p->sticky_ch4_breaker_fault, 4u, 0x10u); + dst_p[5] |= pack_left_shift_u8(src_p->sticky_ch5_breaker_fault, 5u, 0x20u); + dst_p[5] |= pack_left_shift_u8(src_p->sticky_ch6_breaker_fault, 6u, 0x40u); + dst_p[5] |= pack_left_shift_u8(src_p->sticky_ch7_breaker_fault, 7u, 0x80u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_ch8_breaker_fault, 0u, 0x01u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_ch9_breaker_fault, 1u, 0x02u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_ch10_breaker_fault, 2u, 0x04u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_ch11_breaker_fault, 3u, 0x08u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_ch12_breaker_fault, 4u, 0x10u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_ch13_breaker_fault, 5u, 0x20u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_ch14_breaker_fault, 6u, 0x40u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_ch15_breaker_fault, 7u, 0x80u); + dst_p[7] |= pack_left_shift_u8(src_p->sticky_ch16_breaker_fault, 0u, 0x01u); + dst_p[7] |= pack_left_shift_u8(src_p->sticky_ch17_breaker_fault, 1u, 0x02u); + dst_p[7] |= pack_left_shift_u8(src_p->sticky_ch18_breaker_fault, 2u, 0x04u); + dst_p[7] |= pack_left_shift_u8(src_p->sticky_ch19_breaker_fault, 3u, 0x08u); + + return (8); +} + +int PDH_status_4_unpack( + struct PDH_status_4_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + dst_p->v_bus = unpack_right_shift_u16(src_p[0], 0u, 0xffu); + dst_p->v_bus |= unpack_left_shift_u16(src_p[1], 8u, 0x0fu); + dst_p->system_enable = unpack_right_shift_u8(src_p[1], 4u, 0x10u); + dst_p->rsvd0 = unpack_right_shift_u8(src_p[1], 5u, 0xe0u); + dst_p->brownout_fault = unpack_right_shift_u8(src_p[2], 0u, 0x01u); + dst_p->rsvd1 = unpack_right_shift_u8(src_p[2], 1u, 0x02u); + dst_p->can_warning_fault = unpack_right_shift_u8(src_p[2], 2u, 0x04u); + dst_p->hardware_fault = unpack_right_shift_u8(src_p[2], 3u, 0x08u); + dst_p->switch_channel_state = unpack_right_shift_u8(src_p[2], 4u, 0x10u); + dst_p->sticky_brownout_fault = unpack_right_shift_u8(src_p[2], 5u, 0x20u); + dst_p->rsvd2 = unpack_right_shift_u8(src_p[2], 6u, 0x40u); + dst_p->sticky_can_warning_fault = unpack_right_shift_u8(src_p[2], 7u, 0x80u); + dst_p->sticky_can_bus_off_fault = unpack_right_shift_u8(src_p[3], 0u, 0x01u); + dst_p->sticky_hardware_fault = unpack_right_shift_u8(src_p[3], 1u, 0x02u); + dst_p->sticky_firmware_fault = unpack_right_shift_u8(src_p[3], 2u, 0x04u); + dst_p->sticky_ch20_breaker_fault = unpack_right_shift_u8(src_p[3], 3u, 0x08u); + dst_p->sticky_ch21_breaker_fault = unpack_right_shift_u8(src_p[3], 4u, 0x10u); + dst_p->sticky_ch22_breaker_fault = unpack_right_shift_u8(src_p[3], 5u, 0x20u); + dst_p->sticky_ch23_breaker_fault = unpack_right_shift_u8(src_p[3], 6u, 0x40u); + dst_p->sticky_has_reset_fault = unpack_right_shift_u8(src_p[3], 7u, 0x80u); + dst_p->total_current = unpack_right_shift_u8(src_p[4], 0u, 0xffu); + dst_p->sticky_ch0_breaker_fault = unpack_right_shift_u8(src_p[5], 0u, 0x01u); + dst_p->sticky_ch1_breaker_fault = unpack_right_shift_u8(src_p[5], 1u, 0x02u); + dst_p->sticky_ch2_breaker_fault = unpack_right_shift_u8(src_p[5], 2u, 0x04u); + dst_p->sticky_ch3_breaker_fault = unpack_right_shift_u8(src_p[5], 3u, 0x08u); + dst_p->sticky_ch4_breaker_fault = unpack_right_shift_u8(src_p[5], 4u, 0x10u); + dst_p->sticky_ch5_breaker_fault = unpack_right_shift_u8(src_p[5], 5u, 0x20u); + dst_p->sticky_ch6_breaker_fault = unpack_right_shift_u8(src_p[5], 6u, 0x40u); + dst_p->sticky_ch7_breaker_fault = unpack_right_shift_u8(src_p[5], 7u, 0x80u); + dst_p->sticky_ch8_breaker_fault = unpack_right_shift_u8(src_p[6], 0u, 0x01u); + dst_p->sticky_ch9_breaker_fault = unpack_right_shift_u8(src_p[6], 1u, 0x02u); + dst_p->sticky_ch10_breaker_fault = unpack_right_shift_u8(src_p[6], 2u, 0x04u); + dst_p->sticky_ch11_breaker_fault = unpack_right_shift_u8(src_p[6], 3u, 0x08u); + dst_p->sticky_ch12_breaker_fault = unpack_right_shift_u8(src_p[6], 4u, 0x10u); + dst_p->sticky_ch13_breaker_fault = unpack_right_shift_u8(src_p[6], 5u, 0x20u); + dst_p->sticky_ch14_breaker_fault = unpack_right_shift_u8(src_p[6], 6u, 0x40u); + dst_p->sticky_ch15_breaker_fault = unpack_right_shift_u8(src_p[6], 7u, 0x80u); + dst_p->sticky_ch16_breaker_fault = unpack_right_shift_u8(src_p[7], 0u, 0x01u); + dst_p->sticky_ch17_breaker_fault = unpack_right_shift_u8(src_p[7], 1u, 0x02u); + dst_p->sticky_ch18_breaker_fault = unpack_right_shift_u8(src_p[7], 2u, 0x04u); + dst_p->sticky_ch19_breaker_fault = unpack_right_shift_u8(src_p[7], 3u, 0x08u); + + return (0); +} + +uint16_t PDH_status_4_v_bus_encode(double value) +{ + return (uint16_t)(value / 0.0078125); +} + +double PDH_status_4_v_bus_decode(uint16_t value) +{ + return ((double)value * 0.0078125); +} + +bool PDH_status_4_v_bus_is_in_range(uint16_t value) +{ + return (value <= 4095u); +} + +uint8_t PDH_status_4_system_enable_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_system_enable_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_system_enable_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_rsvd0_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_rsvd0_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_rsvd0_is_in_range(uint8_t value) +{ + return (value <= 7u); +} + +uint8_t PDH_status_4_brownout_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_brownout_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_brownout_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_rsvd1_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_rsvd1_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_rsvd1_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_can_warning_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_can_warning_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_can_warning_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_hardware_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_hardware_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_hardware_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_switch_channel_state_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_switch_channel_state_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_switch_channel_state_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_brownout_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_brownout_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_brownout_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_rsvd2_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_rsvd2_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_rsvd2_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_can_warning_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_can_warning_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_can_warning_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_can_bus_off_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_can_bus_off_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_can_bus_off_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_hardware_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_hardware_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_hardware_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_firmware_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_firmware_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_firmware_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch20_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch20_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch20_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch21_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch21_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch21_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch22_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch22_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch22_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch23_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch23_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch23_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_has_reset_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_has_reset_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_has_reset_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_total_current_encode(double value) +{ + return (uint8_t)(value / 2.0); +} + +double PDH_status_4_total_current_decode(uint8_t value) +{ + return ((double)value * 2.0); +} + +bool PDH_status_4_total_current_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PDH_status_4_sticky_ch0_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch0_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch0_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch1_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch1_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch1_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch2_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch2_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch2_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch3_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch3_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch3_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch4_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch4_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch4_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch5_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch5_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch5_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch6_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch6_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch6_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch7_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch7_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch7_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch8_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch8_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch8_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch9_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch9_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch9_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch10_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch10_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch10_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch11_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch11_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch11_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch12_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch12_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch12_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch13_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch13_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch13_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch14_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch14_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch14_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch15_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch15_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch15_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch16_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch16_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch16_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch17_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch17_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch17_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch18_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch18_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch18_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status_4_sticky_ch19_breaker_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status_4_sticky_ch19_breaker_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status_4_sticky_ch19_breaker_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +int PDH_clear_faults_pack( + uint8_t *dst_p, + const struct PDH_clear_faults_t *src_p, + size_t size) +{ + (void)dst_p; + (void)src_p; + (void)size; + + return (0); +} + +int PDH_clear_faults_unpack( + struct PDH_clear_faults_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + (void)dst_p; + (void)src_p; + (void)size; + + return (0); +} + +int PDH_version_pack( + uint8_t *dst_p, + const struct PDH_version_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 8); + + dst_p[0] |= pack_left_shift_u8(src_p->firmware_fix, 0u, 0xffu); + dst_p[1] |= pack_left_shift_u8(src_p->firmware_minor, 0u, 0xffu); + dst_p[2] |= pack_left_shift_u8(src_p->firmware_year, 0u, 0xffu); + dst_p[3] |= pack_left_shift_u8(src_p->hardware_minor, 0u, 0xffu); + dst_p[4] |= pack_left_shift_u8(src_p->hardware_major, 0u, 0xffu); + dst_p[5] |= pack_left_shift_u32(src_p->unique_id, 0u, 0xffu); + dst_p[6] |= pack_right_shift_u32(src_p->unique_id, 8u, 0xffu); + dst_p[7] |= pack_right_shift_u32(src_p->unique_id, 16u, 0xffu); + + return (8); +} + +int PDH_version_unpack( + struct PDH_version_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + dst_p->firmware_fix = unpack_right_shift_u8(src_p[0], 0u, 0xffu); + dst_p->firmware_minor = unpack_right_shift_u8(src_p[1], 0u, 0xffu); + dst_p->firmware_year = unpack_right_shift_u8(src_p[2], 0u, 0xffu); + dst_p->hardware_minor = unpack_right_shift_u8(src_p[3], 0u, 0xffu); + dst_p->hardware_major = unpack_right_shift_u8(src_p[4], 0u, 0xffu); + dst_p->unique_id = unpack_right_shift_u32(src_p[5], 0u, 0xffu); + dst_p->unique_id |= unpack_left_shift_u32(src_p[6], 8u, 0xffu); + dst_p->unique_id |= unpack_left_shift_u32(src_p[7], 16u, 0xffu); + + return (0); +} + +uint8_t PDH_version_firmware_fix_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_version_firmware_fix_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_version_firmware_fix_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PDH_version_firmware_minor_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_version_firmware_minor_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_version_firmware_minor_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PDH_version_firmware_year_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_version_firmware_year_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_version_firmware_year_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PDH_version_hardware_minor_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_version_hardware_minor_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_version_hardware_minor_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PDH_version_hardware_major_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_version_hardware_major_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_version_hardware_major_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint32_t PDH_version_unique_id_encode(double value) +{ + return (uint32_t)(value); +} + +double PDH_version_unique_id_decode(uint32_t value) +{ + return ((double)value); +} + +bool PDH_version_unique_id_is_in_range(uint32_t value) +{ + return (value <= 16777215u); +} diff --git a/hal/src/main/native/systemcore/rev/PDHFrames.h b/hal/src/main/native/systemcore/rev/PDHFrames.h new file mode 100644 index 00000000000..eb498c9eb75 --- /dev/null +++ b/hal/src/main/native/systemcore/rev/PDHFrames.h @@ -0,0 +1,3618 @@ +/** + * The MIT License (MIT) + * + * Copyright (c) 2018-2019 Erik Moqvist + * + * 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. + */ + +/** + * This file was generated by cantools version + */ + +#ifndef PDH_H +#define PDH_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include + +#ifndef EINVAL +# define EINVAL 22 +#endif + +/* Frame ids. */ +#define PDH_SET_SWITCH_CHANNEL_FRAME_ID (0x8050840u) +#define PDH_STATUS_0_FRAME_ID (0x8051800u) +#define PDH_STATUS_1_FRAME_ID (0x8051840u) +#define PDH_STATUS_2_FRAME_ID (0x8051880u) +#define PDH_STATUS_3_FRAME_ID (0x80518c0u) +#define PDH_STATUS_4_FRAME_ID (0x8051900u) +#define PDH_CLEAR_FAULTS_FRAME_ID (0x8051b80u) +#define PDH_VERSION_FRAME_ID (0x8052600u) + +/* Frame lengths in bytes. */ +#define PDH_SET_SWITCH_CHANNEL_LENGTH (1u) +#define PDH_STATUS_0_LENGTH (8u) +#define PDH_STATUS_1_LENGTH (8u) +#define PDH_STATUS_2_LENGTH (8u) +#define PDH_STATUS_3_LENGTH (8u) +#define PDH_STATUS_4_LENGTH (8u) +#define PDH_CLEAR_FAULTS_LENGTH (0u) +#define PDH_VERSION_LENGTH (8u) + +/* Extended or standard frame types. */ +#define PDH_SET_SWITCH_CHANNEL_IS_EXTENDED (1) +#define PDH_STATUS_0_IS_EXTENDED (1) +#define PDH_STATUS_1_IS_EXTENDED (1) +#define PDH_STATUS_2_IS_EXTENDED (1) +#define PDH_STATUS_3_IS_EXTENDED (1) +#define PDH_STATUS_4_IS_EXTENDED (1) +#define PDH_CLEAR_FAULTS_IS_EXTENDED (1) +#define PDH_VERSION_IS_EXTENDED (1) + +/* Frame cycle times in milliseconds. */ + + +/* Signal choices. */ + + +/** + * Signals in message Set_Switch_Channel. + * + * Set the switch channel output + * + * All signal values are as on the CAN bus. + */ +struct PDH_set_switch_channel_t { + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t output_set_value : 1; +}; + +/** + * Signals in message Status_0. + * + * Periodic status frame 0 + * + * All signal values are as on the CAN bus. + */ +struct PDH_status_0_t { + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_0_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_1_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_2_current : 10; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_0_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_1_breaker_fault : 1; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_3_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_4_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_5_current : 10; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_2_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_3_breaker_fault : 1; +}; + +/** + * Signals in message Status_1. + * + * Periodic status frame 1 + * + * All signal values are as on the CAN bus. + */ +struct PDH_status_1_t { + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_6_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_7_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_8_current : 10; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_4_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_5_breaker_fault : 1; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_9_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_10_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_11_current : 10; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_6_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_7_breaker_fault : 1; +}; + +/** + * Signals in message Status_2. + * + * Periodic status frame 2 + * + * All signal values are as on the CAN bus. + */ +struct PDH_status_2_t { + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_12_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_13_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_14_current : 10; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_8_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_9_breaker_fault : 1; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_15_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_16_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_17_current : 10; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_10_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_11_breaker_fault : 1; +}; + +/** + * Signals in message Status_3. + * + * Periodic status frame 3 + * + * All signal values are as on the CAN bus. + */ +struct PDH_status_3_t { + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_18_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_19_current : 10; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_12_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_13_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_14_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_15_breaker_fault : 1; + + /** + * Range: 0..511 (0..31.9375 A) + * Scale: 0.0625 + * Offset: 0 + */ + uint8_t channel_20_current : 8; + + /** + * Range: 0..511 (0..31.9375 A) + * Scale: 0.0625 + * Offset: 0 + */ + uint8_t channel_21_current : 8; + + /** + * Range: 0..511 (0..31.9375 A) + * Scale: 0.0625 + * Offset: 0 + */ + uint8_t channel_22_current : 8; + + /** + * Range: 0..511 (0..31.9375 A) + * Scale: 0.0625 + * Offset: 0 + */ + uint8_t channel_23_current : 8; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_16_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_17_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_18_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_19_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_20_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_21_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_22_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_23_breaker_fault : 1; +}; + +/** + * Signals in message Status_4. + * + * Periodic status frame 4 + * + * All signal values are as on the CAN bus. + */ +struct PDH_status_4_t { + /** + * Range: 0..4095 (0..31.9921875 V) + * Scale: 0.0078125 + * Offset: 0 + */ + uint16_t v_bus : 12; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t system_enable : 1; + + /** + * Range: 0..7 (0..7 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t rsvd0 : 3; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t brownout_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t rsvd1 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t can_warning_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t hardware_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t switch_channel_state : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_brownout_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t rsvd2 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_can_warning_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_can_bus_off_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_hardware_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_firmware_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch20_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch21_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch22_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch23_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_has_reset_fault : 1; + + /** + * Range: 0..255 (0..510 A) + * Scale: 2 + * Offset: 0 + */ + uint8_t total_current : 8; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch0_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch1_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch2_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch3_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch4_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch5_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch6_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch7_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch8_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch9_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch10_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch11_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch12_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch13_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch14_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch15_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch16_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch17_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch18_breaker_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch19_breaker_fault : 1; +}; + +/** + * Signals in message Clear_Faults. + * + * Clear sticky faults on the device + * + * All signal values are as on the CAN bus. + */ +struct PDH_clear_faults_t { + /** + * Dummy signal in empty message. + */ + uint8_t dummy; +}; + +/** + * Signals in message Version. + * + * Get the version of the PDH device + * + * All signal values are as on the CAN bus. + */ +struct PDH_version_t { + /** + * Range: 0..255 (0..255 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t firmware_fix : 8; + + /** + * Range: 0..255 (0..255 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t firmware_minor : 8; + + /** + * Range: 0..255 (0..255 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t firmware_year : 8; + + /** + * Range: 0..255 (0..255 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t hardware_minor : 8; + + /** + * Range: 0..255 (0..255 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t hardware_major : 8; + + /** + * Range: 0..16777215 (0..16777215 -) + * Scale: 1 + * Offset: 0 + */ + uint32_t unique_id : 24; +}; + +/** + * Pack message Set_Switch_Channel. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PDH_set_switch_channel_pack( + uint8_t *dst_p, + const struct PDH_set_switch_channel_t *src_p, + size_t size); + +/** + * Unpack message Set_Switch_Channel. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PDH_set_switch_channel_unpack( + struct PDH_set_switch_channel_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_set_switch_channel_output_set_value_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_set_switch_channel_output_set_value_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_set_switch_channel_output_set_value_is_in_range(uint8_t value); + +/** + * Pack message Status_0. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PDH_status_0_pack( + uint8_t *dst_p, + const struct PDH_status_0_t *src_p, + size_t size); + +/** + * Unpack message Status_0. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PDH_status_0_unpack( + struct PDH_status_0_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status_0_channel_0_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_0_channel_0_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_0_channel_0_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status_0_channel_1_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_0_channel_1_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_0_channel_1_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status_0_channel_2_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_0_channel_2_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_0_channel_2_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_0_channel_0_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_0_channel_0_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_0_channel_0_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_0_channel_1_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_0_channel_1_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_0_channel_1_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status_0_channel_3_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_0_channel_3_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_0_channel_3_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status_0_channel_4_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_0_channel_4_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_0_channel_4_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status_0_channel_5_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_0_channel_5_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_0_channel_5_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_0_channel_2_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_0_channel_2_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_0_channel_2_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_0_channel_3_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_0_channel_3_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_0_channel_3_breaker_fault_is_in_range(uint8_t value); + +/** + * Pack message Status_1. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PDH_status_1_pack( + uint8_t *dst_p, + const struct PDH_status_1_t *src_p, + size_t size); + +/** + * Unpack message Status_1. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PDH_status_1_unpack( + struct PDH_status_1_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status_1_channel_6_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_1_channel_6_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_1_channel_6_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status_1_channel_7_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_1_channel_7_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_1_channel_7_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status_1_channel_8_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_1_channel_8_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_1_channel_8_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_1_channel_4_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_1_channel_4_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_1_channel_4_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_1_channel_5_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_1_channel_5_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_1_channel_5_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status_1_channel_9_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_1_channel_9_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_1_channel_9_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status_1_channel_10_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_1_channel_10_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_1_channel_10_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status_1_channel_11_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_1_channel_11_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_1_channel_11_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_1_channel_6_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_1_channel_6_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_1_channel_6_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_1_channel_7_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_1_channel_7_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_1_channel_7_breaker_fault_is_in_range(uint8_t value); + +/** + * Pack message Status_2. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PDH_status_2_pack( + uint8_t *dst_p, + const struct PDH_status_2_t *src_p, + size_t size); + +/** + * Unpack message Status_2. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PDH_status_2_unpack( + struct PDH_status_2_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status_2_channel_12_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_2_channel_12_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_2_channel_12_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status_2_channel_13_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_2_channel_13_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_2_channel_13_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status_2_channel_14_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_2_channel_14_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_2_channel_14_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_2_channel_8_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_2_channel_8_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_2_channel_8_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_2_channel_9_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_2_channel_9_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_2_channel_9_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status_2_channel_15_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_2_channel_15_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_2_channel_15_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status_2_channel_16_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_2_channel_16_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_2_channel_16_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status_2_channel_17_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_2_channel_17_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_2_channel_17_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_2_channel_10_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_2_channel_10_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_2_channel_10_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_2_channel_11_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_2_channel_11_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_2_channel_11_breaker_fault_is_in_range(uint8_t value); + +/** + * Pack message Status_3. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PDH_status_3_pack( + uint8_t *dst_p, + const struct PDH_status_3_t *src_p, + size_t size); + +/** + * Unpack message Status_3. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PDH_status_3_unpack( + struct PDH_status_3_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status_3_channel_18_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_3_channel_18_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_3_channel_18_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status_3_channel_19_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_3_channel_19_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_3_channel_19_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_3_channel_12_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_3_channel_12_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_3_channel_12_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_3_channel_13_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_3_channel_13_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_3_channel_13_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_3_channel_14_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_3_channel_14_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_3_channel_14_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_3_channel_15_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_3_channel_15_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_3_channel_15_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_3_channel_20_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_3_channel_20_current_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_3_channel_20_current_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_3_channel_21_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_3_channel_21_current_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_3_channel_21_current_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_3_channel_22_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_3_channel_22_current_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_3_channel_22_current_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_3_channel_23_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_3_channel_23_current_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_3_channel_23_current_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_3_channel_16_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_3_channel_16_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_3_channel_16_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_3_channel_17_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_3_channel_17_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_3_channel_17_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_3_channel_18_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_3_channel_18_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_3_channel_18_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_3_channel_19_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_3_channel_19_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_3_channel_19_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_3_channel_20_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_3_channel_20_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_3_channel_20_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_3_channel_21_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_3_channel_21_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_3_channel_21_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_3_channel_22_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_3_channel_22_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_3_channel_22_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_3_channel_23_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_3_channel_23_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_3_channel_23_breaker_fault_is_in_range(uint8_t value); + +/** + * Pack message Status_4. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PDH_status_4_pack( + uint8_t *dst_p, + const struct PDH_status_4_t *src_p, + size_t size); + +/** + * Unpack message Status_4. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PDH_status_4_unpack( + struct PDH_status_4_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status_4_v_bus_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_v_bus_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_v_bus_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_system_enable_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_system_enable_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_system_enable_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_rsvd0_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_rsvd0_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_rsvd0_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_brownout_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_brownout_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_brownout_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_rsvd1_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_rsvd1_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_rsvd1_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_can_warning_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_can_warning_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_can_warning_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_hardware_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_hardware_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_hardware_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_switch_channel_state_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_switch_channel_state_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_switch_channel_state_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_brownout_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_brownout_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_brownout_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_rsvd2_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_rsvd2_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_rsvd2_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_can_warning_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_can_warning_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_can_warning_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_can_bus_off_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_can_bus_off_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_can_bus_off_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_hardware_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_hardware_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_hardware_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_firmware_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_firmware_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_firmware_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch20_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch20_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch20_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch21_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch21_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch21_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch22_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch22_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch22_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch23_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch23_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch23_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_has_reset_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_has_reset_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_has_reset_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_total_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_total_current_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_total_current_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch0_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch0_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch0_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch1_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch1_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch1_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch2_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch2_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch2_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch3_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch3_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch3_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch4_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch4_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch4_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch5_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch5_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch5_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch6_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch6_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch6_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch7_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch7_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch7_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch8_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch8_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch8_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch9_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch9_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch9_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch10_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch10_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch10_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch11_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch11_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch11_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch12_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch12_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch12_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch13_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch13_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch13_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch14_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch14_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch14_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch15_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch15_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch15_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch16_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch16_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch16_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch17_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch17_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch17_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch18_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch18_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch18_breaker_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status_4_sticky_ch19_breaker_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status_4_sticky_ch19_breaker_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status_4_sticky_ch19_breaker_fault_is_in_range(uint8_t value); + +/** + * Pack message Clear_Faults. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PDH_clear_faults_pack( + uint8_t *dst_p, + const struct PDH_clear_faults_t *src_p, + size_t size); + +/** + * Unpack message Clear_Faults. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PDH_clear_faults_unpack( + struct PDH_clear_faults_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Pack message Version. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PDH_version_pack( + uint8_t *dst_p, + const struct PDH_version_t *src_p, + size_t size); + +/** + * Unpack message Version. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PDH_version_unpack( + struct PDH_version_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_version_firmware_fix_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_version_firmware_fix_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_version_firmware_fix_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_version_firmware_minor_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_version_firmware_minor_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_version_firmware_minor_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_version_firmware_year_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_version_firmware_year_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_version_firmware_year_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_version_hardware_minor_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_version_hardware_minor_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_version_hardware_minor_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_version_hardware_major_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_version_hardware_major_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_version_hardware_major_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint32_t PDH_version_unique_id_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_version_unique_id_decode(uint32_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_version_unique_id_is_in_range(uint32_t value); + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/hal/src/main/native/systemcore/rev/PHFrames.cpp b/hal/src/main/native/systemcore/rev/PHFrames.cpp new file mode 100644 index 00000000000..0d108c25e66 --- /dev/null +++ b/hal/src/main/native/systemcore/rev/PHFrames.cpp @@ -0,0 +1,2071 @@ +/** + * The MIT License (MIT) + * + * Copyright (c) 2018-2019 Erik Moqvist + * + * 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. + */ + +/** + * This file was generated by cantools version + */ + +#include + +#include "PHFrames.h" + +static inline uint8_t pack_left_shift_u8( + uint8_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint8_t)((uint8_t)(value << shift) & mask); +} + +static inline uint8_t pack_left_shift_u16( + uint16_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint8_t)((uint8_t)(value << shift) & mask); +} + +static inline uint8_t pack_left_shift_u32( + uint32_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint8_t)((uint8_t)(value << shift) & mask); +} + +static inline uint8_t pack_right_shift_u16( + uint16_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint8_t)((uint8_t)(value >> shift) & mask); +} + +static inline uint8_t pack_right_shift_u32( + uint32_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint8_t)((uint8_t)(value >> shift) & mask); +} + +static inline uint16_t unpack_left_shift_u16( + uint8_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint16_t)((uint16_t)(value & mask) << shift); +} + +static inline uint32_t unpack_left_shift_u32( + uint8_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint32_t)((uint32_t)(value & mask) << shift); +} + +static inline uint8_t unpack_right_shift_u8( + uint8_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint8_t)((uint8_t)(value & mask) >> shift); +} + +static inline uint16_t unpack_right_shift_u16( + uint8_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint16_t)((uint16_t)(value & mask) >> shift); +} + +static inline uint32_t unpack_right_shift_u32( + uint8_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint32_t)((uint32_t)(value & mask) >> shift); +} + +int PH_compressor_config_pack( + uint8_t *dst_p, + const struct PH_compressor_config_t *src_p, + size_t size) +{ + if (size < 5u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 5); + + dst_p[0] |= pack_left_shift_u16(src_p->minimum_tank_pressure, 0u, 0xffu); + dst_p[1] |= pack_right_shift_u16(src_p->minimum_tank_pressure, 8u, 0xffu); + dst_p[2] |= pack_left_shift_u16(src_p->maximum_tank_pressure, 0u, 0xffu); + dst_p[3] |= pack_right_shift_u16(src_p->maximum_tank_pressure, 8u, 0xffu); + dst_p[4] |= pack_left_shift_u8(src_p->force_disable, 0u, 0x01u); + dst_p[4] |= pack_left_shift_u8(src_p->use_digital, 1u, 0x02u); + + return (5); +} + +int PH_compressor_config_unpack( + struct PH_compressor_config_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 5u) { + return (-EINVAL); + } + + dst_p->minimum_tank_pressure = unpack_right_shift_u16(src_p[0], 0u, 0xffu); + dst_p->minimum_tank_pressure |= unpack_left_shift_u16(src_p[1], 8u, 0xffu); + dst_p->maximum_tank_pressure = unpack_right_shift_u16(src_p[2], 0u, 0xffu); + dst_p->maximum_tank_pressure |= unpack_left_shift_u16(src_p[3], 8u, 0xffu); + dst_p->force_disable = unpack_right_shift_u8(src_p[4], 0u, 0x01u); + dst_p->use_digital = unpack_right_shift_u8(src_p[4], 1u, 0x02u); + + return (0); +} + +uint16_t PH_compressor_config_minimum_tank_pressure_encode(double value) +{ + return (uint16_t)(value / 0.001); +} + +double PH_compressor_config_minimum_tank_pressure_decode(uint16_t value) +{ + return ((double)value * 0.001); +} + +bool PH_compressor_config_minimum_tank_pressure_is_in_range(uint16_t value) +{ + return (value <= 5000u); +} + +uint16_t PH_compressor_config_maximum_tank_pressure_encode(double value) +{ + return (uint16_t)(value / 0.001); +} + +double PH_compressor_config_maximum_tank_pressure_decode(uint16_t value) +{ + return ((double)value * 0.001); +} + +bool PH_compressor_config_maximum_tank_pressure_is_in_range(uint16_t value) +{ + return (value <= 5000u); +} + +uint8_t PH_compressor_config_force_disable_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_compressor_config_force_disable_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_compressor_config_force_disable_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_compressor_config_use_digital_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_compressor_config_use_digital_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_compressor_config_use_digital_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +int PH_set_all_pack( + uint8_t *dst_p, + const struct PH_set_all_t *src_p, + size_t size) +{ + if (size < 4u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 4); + + dst_p[0] |= pack_left_shift_u8(src_p->channel_0, 0u, 0x03u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_1, 2u, 0x0cu); + dst_p[0] |= pack_left_shift_u8(src_p->channel_2, 4u, 0x30u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_3, 6u, 0xc0u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_4, 0u, 0x03u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_5, 2u, 0x0cu); + dst_p[1] |= pack_left_shift_u8(src_p->channel_6, 4u, 0x30u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_7, 6u, 0xc0u); + dst_p[2] |= pack_left_shift_u8(src_p->channel_8, 0u, 0x03u); + dst_p[2] |= pack_left_shift_u8(src_p->channel_9, 2u, 0x0cu); + dst_p[2] |= pack_left_shift_u8(src_p->channel_10, 4u, 0x30u); + dst_p[2] |= pack_left_shift_u8(src_p->channel_11, 6u, 0xc0u); + dst_p[3] |= pack_left_shift_u8(src_p->channel_12, 0u, 0x03u); + dst_p[3] |= pack_left_shift_u8(src_p->channel_13, 2u, 0x0cu); + dst_p[3] |= pack_left_shift_u8(src_p->channel_14, 4u, 0x30u); + dst_p[3] |= pack_left_shift_u8(src_p->channel_15, 6u, 0xc0u); + + return (4); +} + +int PH_set_all_unpack( + struct PH_set_all_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 4u) { + return (-EINVAL); + } + + dst_p->channel_0 = unpack_right_shift_u8(src_p[0], 0u, 0x03u); + dst_p->channel_1 = unpack_right_shift_u8(src_p[0], 2u, 0x0cu); + dst_p->channel_2 = unpack_right_shift_u8(src_p[0], 4u, 0x30u); + dst_p->channel_3 = unpack_right_shift_u8(src_p[0], 6u, 0xc0u); + dst_p->channel_4 = unpack_right_shift_u8(src_p[1], 0u, 0x03u); + dst_p->channel_5 = unpack_right_shift_u8(src_p[1], 2u, 0x0cu); + dst_p->channel_6 = unpack_right_shift_u8(src_p[1], 4u, 0x30u); + dst_p->channel_7 = unpack_right_shift_u8(src_p[1], 6u, 0xc0u); + dst_p->channel_8 = unpack_right_shift_u8(src_p[2], 0u, 0x03u); + dst_p->channel_9 = unpack_right_shift_u8(src_p[2], 2u, 0x0cu); + dst_p->channel_10 = unpack_right_shift_u8(src_p[2], 4u, 0x30u); + dst_p->channel_11 = unpack_right_shift_u8(src_p[2], 6u, 0xc0u); + dst_p->channel_12 = unpack_right_shift_u8(src_p[3], 0u, 0x03u); + dst_p->channel_13 = unpack_right_shift_u8(src_p[3], 2u, 0x0cu); + dst_p->channel_14 = unpack_right_shift_u8(src_p[3], 4u, 0x30u); + dst_p->channel_15 = unpack_right_shift_u8(src_p[3], 6u, 0xc0u); + + return (0); +} + +uint8_t PH_set_all_channel_0_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_0_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_0_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_1_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_1_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_1_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_2_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_2_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_2_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_3_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_3_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_3_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_4_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_4_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_4_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_5_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_5_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_5_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_6_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_6_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_6_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_7_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_7_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_7_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_8_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_8_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_8_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_9_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_9_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_9_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_10_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_10_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_10_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_11_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_11_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_11_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_12_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_12_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_12_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_13_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_13_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_13_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_14_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_14_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_14_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_15_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_15_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_15_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +int PH_pulse_once_pack( + uint8_t *dst_p, + const struct PH_pulse_once_t *src_p, + size_t size) +{ + if (size < 4u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 4); + + dst_p[0] |= pack_left_shift_u8(src_p->channel_0, 0u, 0x01u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_1, 1u, 0x02u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_2, 2u, 0x04u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_3, 3u, 0x08u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_4, 4u, 0x10u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_5, 5u, 0x20u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_6, 6u, 0x40u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_7, 7u, 0x80u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_8, 0u, 0x01u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_9, 1u, 0x02u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_10, 2u, 0x04u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_11, 3u, 0x08u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_12, 4u, 0x10u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_13, 5u, 0x20u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_14, 6u, 0x40u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_15, 7u, 0x80u); + dst_p[2] |= pack_left_shift_u16(src_p->pulse_length_ms, 0u, 0xffu); + dst_p[3] |= pack_right_shift_u16(src_p->pulse_length_ms, 8u, 0xffu); + + return (4); +} + +int PH_pulse_once_unpack( + struct PH_pulse_once_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 4u) { + return (-EINVAL); + } + + dst_p->channel_0 = unpack_right_shift_u8(src_p[0], 0u, 0x01u); + dst_p->channel_1 = unpack_right_shift_u8(src_p[0], 1u, 0x02u); + dst_p->channel_2 = unpack_right_shift_u8(src_p[0], 2u, 0x04u); + dst_p->channel_3 = unpack_right_shift_u8(src_p[0], 3u, 0x08u); + dst_p->channel_4 = unpack_right_shift_u8(src_p[0], 4u, 0x10u); + dst_p->channel_5 = unpack_right_shift_u8(src_p[0], 5u, 0x20u); + dst_p->channel_6 = unpack_right_shift_u8(src_p[0], 6u, 0x40u); + dst_p->channel_7 = unpack_right_shift_u8(src_p[0], 7u, 0x80u); + dst_p->channel_8 = unpack_right_shift_u8(src_p[1], 0u, 0x01u); + dst_p->channel_9 = unpack_right_shift_u8(src_p[1], 1u, 0x02u); + dst_p->channel_10 = unpack_right_shift_u8(src_p[1], 2u, 0x04u); + dst_p->channel_11 = unpack_right_shift_u8(src_p[1], 3u, 0x08u); + dst_p->channel_12 = unpack_right_shift_u8(src_p[1], 4u, 0x10u); + dst_p->channel_13 = unpack_right_shift_u8(src_p[1], 5u, 0x20u); + dst_p->channel_14 = unpack_right_shift_u8(src_p[1], 6u, 0x40u); + dst_p->channel_15 = unpack_right_shift_u8(src_p[1], 7u, 0x80u); + dst_p->pulse_length_ms = unpack_right_shift_u16(src_p[2], 0u, 0xffu); + dst_p->pulse_length_ms |= unpack_left_shift_u16(src_p[3], 8u, 0xffu); + + return (0); +} + +uint8_t PH_pulse_once_channel_0_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_0_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_0_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_1_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_1_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_1_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_2_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_2_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_2_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_3_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_3_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_3_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_4_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_4_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_4_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_5_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_5_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_5_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_6_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_6_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_6_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_7_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_7_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_7_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_8_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_8_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_8_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_9_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_9_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_9_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_10_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_10_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_10_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_11_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_11_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_11_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_12_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_12_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_12_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_13_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_13_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_13_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_14_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_14_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_14_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_15_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_15_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_15_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint16_t PH_pulse_once_pulse_length_ms_encode(double value) +{ + return (uint16_t)(value); +} + +double PH_pulse_once_pulse_length_ms_decode(uint16_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_pulse_length_ms_is_in_range(uint16_t value) +{ + (void)value; + + return (true); +} + +int PH_status_0_pack( + uint8_t *dst_p, + const struct PH_status_0_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 8); + + dst_p[0] |= pack_left_shift_u8(src_p->channel_0, 0u, 0x01u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_1, 1u, 0x02u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_2, 2u, 0x04u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_3, 3u, 0x08u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_4, 4u, 0x10u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_5, 5u, 0x20u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_6, 6u, 0x40u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_7, 7u, 0x80u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_8, 0u, 0x01u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_9, 1u, 0x02u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_10, 2u, 0x04u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_11, 3u, 0x08u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_12, 4u, 0x10u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_13, 5u, 0x20u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_14, 6u, 0x40u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_15, 7u, 0x80u); + dst_p[2] |= pack_left_shift_u8(src_p->analog_0, 0u, 0xffu); + dst_p[3] |= pack_left_shift_u8(src_p->analog_1, 0u, 0xffu); + dst_p[4] |= pack_left_shift_u8(src_p->digital_sensor, 0u, 0x01u); + dst_p[4] |= pack_left_shift_u8(src_p->brownout_fault, 1u, 0x02u); + dst_p[4] |= pack_left_shift_u8(src_p->compressor_oc_fault, 2u, 0x04u); + dst_p[4] |= pack_left_shift_u8(src_p->compressor_open_fault, 3u, 0x08u); + dst_p[4] |= pack_left_shift_u8(src_p->solenoid_oc_fault, 4u, 0x10u); + dst_p[4] |= pack_left_shift_u8(src_p->can_warning_fault, 5u, 0x20u); + dst_p[4] |= pack_left_shift_u8(src_p->hardware_fault, 6u, 0x40u); + dst_p[5] |= pack_left_shift_u8(src_p->channel_0_fault, 0u, 0x01u); + dst_p[5] |= pack_left_shift_u8(src_p->channel_1_fault, 1u, 0x02u); + dst_p[5] |= pack_left_shift_u8(src_p->channel_2_fault, 2u, 0x04u); + dst_p[5] |= pack_left_shift_u8(src_p->channel_3_fault, 3u, 0x08u); + dst_p[5] |= pack_left_shift_u8(src_p->channel_4_fault, 4u, 0x10u); + dst_p[5] |= pack_left_shift_u8(src_p->channel_5_fault, 5u, 0x20u); + dst_p[5] |= pack_left_shift_u8(src_p->channel_6_fault, 6u, 0x40u); + dst_p[5] |= pack_left_shift_u8(src_p->channel_7_fault, 7u, 0x80u); + dst_p[6] |= pack_left_shift_u8(src_p->channel_8_fault, 0u, 0x01u); + dst_p[6] |= pack_left_shift_u8(src_p->channel_9_fault, 1u, 0x02u); + dst_p[6] |= pack_left_shift_u8(src_p->channel_10_fault, 2u, 0x04u); + dst_p[6] |= pack_left_shift_u8(src_p->channel_11_fault, 3u, 0x08u); + dst_p[6] |= pack_left_shift_u8(src_p->channel_12_fault, 4u, 0x10u); + dst_p[6] |= pack_left_shift_u8(src_p->channel_13_fault, 5u, 0x20u); + dst_p[6] |= pack_left_shift_u8(src_p->channel_14_fault, 6u, 0x40u); + dst_p[6] |= pack_left_shift_u8(src_p->channel_15_fault, 7u, 0x80u); + dst_p[7] |= pack_left_shift_u8(src_p->compressor_on, 0u, 0x01u); + dst_p[7] |= pack_left_shift_u8(src_p->system_enabled, 1u, 0x02u); + dst_p[7] |= pack_left_shift_u8(src_p->robo_rio_present, 2u, 0x04u); + dst_p[7] |= pack_left_shift_u8(src_p->compressor_config, 3u, 0x18u); + + return (8); +} + +int PH_status_0_unpack( + struct PH_status_0_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + dst_p->channel_0 = unpack_right_shift_u8(src_p[0], 0u, 0x01u); + dst_p->channel_1 = unpack_right_shift_u8(src_p[0], 1u, 0x02u); + dst_p->channel_2 = unpack_right_shift_u8(src_p[0], 2u, 0x04u); + dst_p->channel_3 = unpack_right_shift_u8(src_p[0], 3u, 0x08u); + dst_p->channel_4 = unpack_right_shift_u8(src_p[0], 4u, 0x10u); + dst_p->channel_5 = unpack_right_shift_u8(src_p[0], 5u, 0x20u); + dst_p->channel_6 = unpack_right_shift_u8(src_p[0], 6u, 0x40u); + dst_p->channel_7 = unpack_right_shift_u8(src_p[0], 7u, 0x80u); + dst_p->channel_8 = unpack_right_shift_u8(src_p[1], 0u, 0x01u); + dst_p->channel_9 = unpack_right_shift_u8(src_p[1], 1u, 0x02u); + dst_p->channel_10 = unpack_right_shift_u8(src_p[1], 2u, 0x04u); + dst_p->channel_11 = unpack_right_shift_u8(src_p[1], 3u, 0x08u); + dst_p->channel_12 = unpack_right_shift_u8(src_p[1], 4u, 0x10u); + dst_p->channel_13 = unpack_right_shift_u8(src_p[1], 5u, 0x20u); + dst_p->channel_14 = unpack_right_shift_u8(src_p[1], 6u, 0x40u); + dst_p->channel_15 = unpack_right_shift_u8(src_p[1], 7u, 0x80u); + dst_p->analog_0 = unpack_right_shift_u8(src_p[2], 0u, 0xffu); + dst_p->analog_1 = unpack_right_shift_u8(src_p[3], 0u, 0xffu); + dst_p->digital_sensor = unpack_right_shift_u8(src_p[4], 0u, 0x01u); + dst_p->brownout_fault = unpack_right_shift_u8(src_p[4], 1u, 0x02u); + dst_p->compressor_oc_fault = unpack_right_shift_u8(src_p[4], 2u, 0x04u); + dst_p->compressor_open_fault = unpack_right_shift_u8(src_p[4], 3u, 0x08u); + dst_p->solenoid_oc_fault = unpack_right_shift_u8(src_p[4], 4u, 0x10u); + dst_p->can_warning_fault = unpack_right_shift_u8(src_p[4], 5u, 0x20u); + dst_p->hardware_fault = unpack_right_shift_u8(src_p[4], 6u, 0x40u); + dst_p->channel_0_fault = unpack_right_shift_u8(src_p[5], 0u, 0x01u); + dst_p->channel_1_fault = unpack_right_shift_u8(src_p[5], 1u, 0x02u); + dst_p->channel_2_fault = unpack_right_shift_u8(src_p[5], 2u, 0x04u); + dst_p->channel_3_fault = unpack_right_shift_u8(src_p[5], 3u, 0x08u); + dst_p->channel_4_fault = unpack_right_shift_u8(src_p[5], 4u, 0x10u); + dst_p->channel_5_fault = unpack_right_shift_u8(src_p[5], 5u, 0x20u); + dst_p->channel_6_fault = unpack_right_shift_u8(src_p[5], 6u, 0x40u); + dst_p->channel_7_fault = unpack_right_shift_u8(src_p[5], 7u, 0x80u); + dst_p->channel_8_fault = unpack_right_shift_u8(src_p[6], 0u, 0x01u); + dst_p->channel_9_fault = unpack_right_shift_u8(src_p[6], 1u, 0x02u); + dst_p->channel_10_fault = unpack_right_shift_u8(src_p[6], 2u, 0x04u); + dst_p->channel_11_fault = unpack_right_shift_u8(src_p[6], 3u, 0x08u); + dst_p->channel_12_fault = unpack_right_shift_u8(src_p[6], 4u, 0x10u); + dst_p->channel_13_fault = unpack_right_shift_u8(src_p[6], 5u, 0x20u); + dst_p->channel_14_fault = unpack_right_shift_u8(src_p[6], 6u, 0x40u); + dst_p->channel_15_fault = unpack_right_shift_u8(src_p[6], 7u, 0x80u); + dst_p->compressor_on = unpack_right_shift_u8(src_p[7], 0u, 0x01u); + dst_p->system_enabled = unpack_right_shift_u8(src_p[7], 1u, 0x02u); + dst_p->robo_rio_present = unpack_right_shift_u8(src_p[7], 2u, 0x04u); + dst_p->compressor_config = unpack_right_shift_u8(src_p[7], 3u, 0x18u); + + return (0); +} + +uint8_t PH_status_0_channel_0_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_0_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_0_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_1_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_1_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_1_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_2_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_2_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_2_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_3_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_3_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_3_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_4_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_4_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_4_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_5_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_5_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_5_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_6_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_6_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_6_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_7_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_7_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_7_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_8_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_8_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_8_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_9_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_9_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_9_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_10_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_10_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_10_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_11_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_11_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_11_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_12_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_12_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_12_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_13_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_13_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_13_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_14_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_14_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_14_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_15_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_15_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_15_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_analog_0_encode(double value) +{ + return (uint8_t)(value / 0.01961); +} + +double PH_status_0_analog_0_decode(uint8_t value) +{ + return ((double)value * 0.01961); +} + +bool PH_status_0_analog_0_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PH_status_0_analog_1_encode(double value) +{ + return (uint8_t)(value / 0.01961); +} + +double PH_status_0_analog_1_decode(uint8_t value) +{ + return ((double)value * 0.01961); +} + +bool PH_status_0_analog_1_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PH_status_0_digital_sensor_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_digital_sensor_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_digital_sensor_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_brownout_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_brownout_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_brownout_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_compressor_oc_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_compressor_oc_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_compressor_oc_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_compressor_open_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_compressor_open_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_compressor_open_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_solenoid_oc_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_solenoid_oc_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_solenoid_oc_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_can_warning_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_can_warning_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_can_warning_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_hardware_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_hardware_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_hardware_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_0_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_0_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_0_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_1_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_1_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_1_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_2_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_2_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_2_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_3_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_3_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_3_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_4_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_4_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_4_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_5_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_5_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_5_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_6_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_6_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_6_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_7_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_7_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_7_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_8_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_8_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_8_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_9_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_9_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_9_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_10_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_10_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_10_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_11_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_11_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_11_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_12_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_12_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_12_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_13_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_13_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_13_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_14_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_14_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_14_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_channel_15_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_channel_15_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_channel_15_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_compressor_on_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_compressor_on_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_compressor_on_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_system_enabled_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_system_enabled_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_system_enabled_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_robo_rio_present_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_robo_rio_present_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_robo_rio_present_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_0_compressor_config_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_0_compressor_config_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_0_compressor_config_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +int PH_status_1_pack( + uint8_t *dst_p, + const struct PH_status_1_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 8); + + dst_p[0] |= pack_left_shift_u8(src_p->v_bus, 0u, 0xffu); + dst_p[1] |= pack_left_shift_u16(src_p->solenoid_voltage, 0u, 0xffu); + dst_p[2] |= pack_right_shift_u16(src_p->solenoid_voltage, 8u, 0x0fu); + dst_p[4] |= pack_left_shift_u8(src_p->compressor_current, 0u, 0xffu); + dst_p[5] |= pack_left_shift_u8(src_p->solenoid_current, 0u, 0xffu); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_brownout_fault, 0u, 0x01u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_compressor_oc_fault, 1u, 0x02u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_compressor_open_fault, 2u, 0x04u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_solenoid_oc_fault, 3u, 0x08u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_can_warning_fault, 4u, 0x10u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_can_bus_off_fault, 5u, 0x20u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_hardware_fault, 6u, 0x40u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_firmware_fault, 7u, 0x80u); + dst_p[7] |= pack_left_shift_u8(src_p->sticky_has_reset_fault, 0u, 0x01u); + dst_p[7] |= pack_left_shift_u8(src_p->supply_voltage_5_v, 1u, 0xfeu); + + return (8); +} + +int PH_status_1_unpack( + struct PH_status_1_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + dst_p->v_bus = unpack_right_shift_u8(src_p[0], 0u, 0xffu); + dst_p->solenoid_voltage = unpack_right_shift_u16(src_p[1], 0u, 0xffu); + dst_p->solenoid_voltage |= unpack_left_shift_u16(src_p[2], 8u, 0x0fu); + dst_p->compressor_current = unpack_right_shift_u8(src_p[4], 0u, 0xffu); + dst_p->solenoid_current = unpack_right_shift_u8(src_p[5], 0u, 0xffu); + dst_p->sticky_brownout_fault = unpack_right_shift_u8(src_p[6], 0u, 0x01u); + dst_p->sticky_compressor_oc_fault = unpack_right_shift_u8(src_p[6], 1u, 0x02u); + dst_p->sticky_compressor_open_fault = unpack_right_shift_u8(src_p[6], 2u, 0x04u); + dst_p->sticky_solenoid_oc_fault = unpack_right_shift_u8(src_p[6], 3u, 0x08u); + dst_p->sticky_can_warning_fault = unpack_right_shift_u8(src_p[6], 4u, 0x10u); + dst_p->sticky_can_bus_off_fault = unpack_right_shift_u8(src_p[6], 5u, 0x20u); + dst_p->sticky_hardware_fault = unpack_right_shift_u8(src_p[6], 6u, 0x40u); + dst_p->sticky_firmware_fault = unpack_right_shift_u8(src_p[6], 7u, 0x80u); + dst_p->sticky_has_reset_fault = unpack_right_shift_u8(src_p[7], 0u, 0x01u); + dst_p->supply_voltage_5_v = unpack_right_shift_u8(src_p[7], 1u, 0xfeu); + + return (0); +} + +uint8_t PH_status_1_v_bus_encode(double value) +{ + return (uint8_t)((value - 4.0) / 0.0625); +} + +double PH_status_1_v_bus_decode(uint8_t value) +{ + return (((double)value * 0.0625) + 4.0); +} + +bool PH_status_1_v_bus_is_in_range(uint8_t value) +{ + return (value <= 192u); +} + +uint16_t PH_status_1_solenoid_voltage_encode(double value) +{ + return (uint16_t)(value / 0.0078125); +} + +double PH_status_1_solenoid_voltage_decode(uint16_t value) +{ + return ((double)value * 0.0078125); +} + +bool PH_status_1_solenoid_voltage_is_in_range(uint16_t value) +{ + return (value <= 4096u); +} + +uint8_t PH_status_1_compressor_current_encode(double value) +{ + return (uint8_t)(value / 0.125); +} + +double PH_status_1_compressor_current_decode(uint8_t value) +{ + return ((double)value * 0.125); +} + +bool PH_status_1_compressor_current_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PH_status_1_solenoid_current_encode(double value) +{ + return (uint8_t)(value / 0.125); +} + +double PH_status_1_solenoid_current_decode(uint8_t value) +{ + return ((double)value * 0.125); +} + +bool PH_status_1_solenoid_current_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PH_status_1_sticky_brownout_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_1_sticky_brownout_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_1_sticky_brownout_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_1_sticky_compressor_oc_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_1_sticky_compressor_oc_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_1_sticky_compressor_oc_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_1_sticky_compressor_open_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_1_sticky_compressor_open_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_1_sticky_compressor_open_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_1_sticky_solenoid_oc_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_1_sticky_solenoid_oc_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_1_sticky_solenoid_oc_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_1_sticky_can_warning_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_1_sticky_can_warning_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_1_sticky_can_warning_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_1_sticky_can_bus_off_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_1_sticky_can_bus_off_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_1_sticky_can_bus_off_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_1_sticky_hardware_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_1_sticky_hardware_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_1_sticky_hardware_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_1_sticky_firmware_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_1_sticky_firmware_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_1_sticky_firmware_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_1_sticky_has_reset_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status_1_sticky_has_reset_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status_1_sticky_has_reset_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status_1_supply_voltage_5_v_encode(double value) +{ + return (uint8_t)((value - 4.5) / 0.0078125); +} + +double PH_status_1_supply_voltage_5_v_decode(uint8_t value) +{ + return (((double)value * 0.0078125) + 4.5); +} + +bool PH_status_1_supply_voltage_5_v_is_in_range(uint8_t value) +{ + return (value <= 128u); +} + +int PH_clear_faults_pack( + uint8_t *dst_p, + const struct PH_clear_faults_t *src_p, + size_t size) +{ + (void)dst_p; + (void)src_p; + (void)size; + + return (0); +} + +int PH_clear_faults_unpack( + struct PH_clear_faults_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + (void)dst_p; + (void)src_p; + (void)size; + + return (0); +} + +int PH_version_pack( + uint8_t *dst_p, + const struct PH_version_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 8); + + dst_p[0] |= pack_left_shift_u8(src_p->firmware_fix, 0u, 0xffu); + dst_p[1] |= pack_left_shift_u8(src_p->firmware_minor, 0u, 0xffu); + dst_p[2] |= pack_left_shift_u8(src_p->firmware_year, 0u, 0xffu); + dst_p[3] |= pack_left_shift_u8(src_p->hardware_minor, 0u, 0xffu); + dst_p[4] |= pack_left_shift_u8(src_p->hardware_major, 0u, 0xffu); + dst_p[5] |= pack_left_shift_u32(src_p->unique_id, 0u, 0xffu); + dst_p[6] |= pack_right_shift_u32(src_p->unique_id, 8u, 0xffu); + dst_p[7] |= pack_right_shift_u32(src_p->unique_id, 16u, 0xffu); + + return (8); +} + +int PH_version_unpack( + struct PH_version_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + dst_p->firmware_fix = unpack_right_shift_u8(src_p[0], 0u, 0xffu); + dst_p->firmware_minor = unpack_right_shift_u8(src_p[1], 0u, 0xffu); + dst_p->firmware_year = unpack_right_shift_u8(src_p[2], 0u, 0xffu); + dst_p->hardware_minor = unpack_right_shift_u8(src_p[3], 0u, 0xffu); + dst_p->hardware_major = unpack_right_shift_u8(src_p[4], 0u, 0xffu); + dst_p->unique_id = unpack_right_shift_u32(src_p[5], 0u, 0xffu); + dst_p->unique_id |= unpack_left_shift_u32(src_p[6], 8u, 0xffu); + dst_p->unique_id |= unpack_left_shift_u32(src_p[7], 16u, 0xffu); + + return (0); +} + +uint8_t PH_version_firmware_fix_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_version_firmware_fix_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_version_firmware_fix_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PH_version_firmware_minor_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_version_firmware_minor_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_version_firmware_minor_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PH_version_firmware_year_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_version_firmware_year_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_version_firmware_year_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PH_version_hardware_minor_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_version_hardware_minor_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_version_hardware_minor_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PH_version_hardware_major_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_version_hardware_major_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_version_hardware_major_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint32_t PH_version_unique_id_encode(double value) +{ + return (uint32_t)(value); +} + +double PH_version_unique_id_decode(uint32_t value) +{ + return ((double)value); +} + +bool PH_version_unique_id_is_in_range(uint32_t value) +{ + return (value <= 16777215u); +} diff --git a/hal/src/main/native/systemcore/rev/PHFrames.h b/hal/src/main/native/systemcore/rev/PHFrames.h new file mode 100644 index 00000000000..cc9c23ccb3e --- /dev/null +++ b/hal/src/main/native/systemcore/rev/PHFrames.h @@ -0,0 +1,3816 @@ +/** + * The MIT License (MIT) + * + * Copyright (c) 2018-2019 Erik Moqvist + * + * 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. + */ + +/** + * This file was generated by cantools version + */ + +#ifndef PH_H +#define PH_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include + +#ifndef EINVAL +# define EINVAL 22 +#endif + +/* Frame ids. */ +#define PH_COMPRESSOR_CONFIG_FRAME_ID (0x9050840u) +#define PH_SET_ALL_FRAME_ID (0x9050c00u) +#define PH_PULSE_ONCE_FRAME_ID (0x9050c40u) +#define PH_STATUS_0_FRAME_ID (0x9051800u) +#define PH_STATUS_1_FRAME_ID (0x9051840u) +#define PH_CLEAR_FAULTS_FRAME_ID (0x9051b80u) +#define PH_VERSION_FRAME_ID (0x9052600u) + +/* Frame lengths in bytes. */ +#define PH_COMPRESSOR_CONFIG_LENGTH (5u) +#define PH_SET_ALL_LENGTH (4u) +#define PH_PULSE_ONCE_LENGTH (4u) +#define PH_STATUS_0_LENGTH (8u) +#define PH_STATUS_1_LENGTH (8u) +#define PH_CLEAR_FAULTS_LENGTH (0u) +#define PH_VERSION_LENGTH (8u) + +/* Extended or standard frame types. */ +#define PH_COMPRESSOR_CONFIG_IS_EXTENDED (1) +#define PH_SET_ALL_IS_EXTENDED (1) +#define PH_PULSE_ONCE_IS_EXTENDED (1) +#define PH_STATUS_0_IS_EXTENDED (1) +#define PH_STATUS_1_IS_EXTENDED (1) +#define PH_CLEAR_FAULTS_IS_EXTENDED (1) +#define PH_VERSION_IS_EXTENDED (1) + +/* Frame cycle times in milliseconds. */ + + +/* Signal choices. */ + + +/** + * Signals in message Compressor_Config. + * + * Configures compressor to use digital/analog sensors + * + * All signal values are as on the CAN bus. + */ +struct PH_compressor_config_t { + /** + * Range: 0..5000 (0..5 V) + * Scale: 0.001 + * Offset: 0 + */ + uint16_t minimum_tank_pressure : 16; + + /** + * Range: 0..5000 (0..5 V) + * Scale: 0.001 + * Offset: 0 + */ + uint16_t maximum_tank_pressure : 16; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t force_disable : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t use_digital : 1; +}; + +/** + * Signals in message Set_All. + * + * Set state of all channels + * + * All signal values are as on the CAN bus. + */ +struct PH_set_all_t { + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_0 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_1 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_2 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_3 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_4 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_5 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_6 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_7 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_8 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_9 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_10 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_11 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_12 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_13 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_14 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_15 : 2; +}; + +/** + * Signals in message Pulse_Once. + * + * Pulse selected channels once + * + * All signal values are as on the CAN bus. + */ +struct PH_pulse_once_t { + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_0 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_1 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_2 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_3 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_4 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_5 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_6 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_7 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_8 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_9 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_10 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_11 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_12 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_13 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_14 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_15 : 1; + + /** + * Range: 0..65535 (0..65535 -) + * Scale: 1 + * Offset: 0 + */ + uint16_t pulse_length_ms : 16; +}; + +/** + * Signals in message Status_0. + * + * Periodic status frame 0 + * + * All signal values are as on the CAN bus. + */ +struct PH_status_0_t { + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_0 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_1 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_2 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_3 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_4 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_5 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_6 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_7 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_8 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_9 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_10 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_11 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_12 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_13 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_14 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_15 : 1; + + /** + * Range: 0..255 (0..5.00055 V) + * Scale: 0.01961 + * Offset: 0 + */ + uint8_t analog_0 : 8; + + /** + * Range: 0..255 (0..5.00055 V) + * Scale: 0.01961 + * Offset: 0 + */ + uint8_t analog_1 : 8; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t digital_sensor : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t brownout_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t compressor_oc_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t compressor_open_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t solenoid_oc_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t can_warning_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t hardware_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_0_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_1_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_2_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_3_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_4_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_5_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_6_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_7_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_8_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_9_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_10_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_11_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_12_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_13_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_14_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_15_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t compressor_on : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t system_enabled : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t robo_rio_present : 1; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t compressor_config : 2; +}; + +/** + * Signals in message Status_1. + * + * Periodic status frame 1 + * + * All signal values are as on the CAN bus. + */ +struct PH_status_1_t { + /** + * Range: 0..192 (4..16 V) + * Scale: 0.0625 + * Offset: 4 + */ + uint8_t v_bus : 8; + + /** + * Range: 0..4096 (0..32 V) + * Scale: 0.0078125 + * Offset: 0 + */ + uint16_t solenoid_voltage : 12; + + /** + * Range: 0..256 (0..32 A) + * Scale: 0.125 + * Offset: 0 + */ + uint8_t compressor_current : 8; + + /** + * Range: 0..256 (0..32 A) + * Scale: 0.125 + * Offset: 0 + */ + uint8_t solenoid_current : 8; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_brownout_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_compressor_oc_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_compressor_open_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_solenoid_oc_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_can_warning_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_can_bus_off_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_hardware_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_firmware_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_has_reset_fault : 1; + + /** + * Range: 0..128 (4.5..5.5 V) + * Scale: 0.0078125 + * Offset: 4.5 + */ + uint8_t supply_voltage_5_v : 7; +}; + +/** + * Signals in message Clear_Faults. + * + * Clear sticky faults on the device + * + * All signal values are as on the CAN bus. + */ +struct PH_clear_faults_t { + /** + * Dummy signal in empty message. + */ + uint8_t dummy; +}; + +/** + * Signals in message Version. + * + * Get the version of the PH + * + * All signal values are as on the CAN bus. + */ +struct PH_version_t { + /** + * Range: 0..255 (0..255 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t firmware_fix : 8; + + /** + * Range: 0..255 (0..255 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t firmware_minor : 8; + + /** + * Range: 0..255 (0..255 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t firmware_year : 8; + + /** + * Range: 0..255 (0..255 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t hardware_minor : 8; + + /** + * Range: 0..255 (0..255 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t hardware_major : 8; + + /** + * Range: 0..16777215 (0..16777215 -) + * Scale: 1 + * Offset: 0 + */ + uint32_t unique_id : 24; +}; + +/** + * Pack message Compressor_Config. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PH_compressor_config_pack( + uint8_t *dst_p, + const struct PH_compressor_config_t *src_p, + size_t size); + +/** + * Unpack message Compressor_Config. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PH_compressor_config_unpack( + struct PH_compressor_config_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PH_compressor_config_minimum_tank_pressure_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_compressor_config_minimum_tank_pressure_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_compressor_config_minimum_tank_pressure_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PH_compressor_config_maximum_tank_pressure_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_compressor_config_maximum_tank_pressure_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_compressor_config_maximum_tank_pressure_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_compressor_config_force_disable_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_compressor_config_force_disable_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_compressor_config_force_disable_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_compressor_config_use_digital_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_compressor_config_use_digital_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_compressor_config_use_digital_is_in_range(uint8_t value); + +/** + * Pack message Set_All. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PH_set_all_pack( + uint8_t *dst_p, + const struct PH_set_all_t *src_p, + size_t size); + +/** + * Unpack message Set_All. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PH_set_all_unpack( + struct PH_set_all_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_set_all_channel_0_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_0_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_set_all_channel_0_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_set_all_channel_1_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_1_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_set_all_channel_1_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_set_all_channel_2_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_2_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_set_all_channel_2_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_set_all_channel_3_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_3_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_set_all_channel_3_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_set_all_channel_4_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_4_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_set_all_channel_4_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_set_all_channel_5_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_5_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_set_all_channel_5_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_set_all_channel_6_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_6_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_set_all_channel_6_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_set_all_channel_7_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_7_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_set_all_channel_7_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_set_all_channel_8_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_8_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_set_all_channel_8_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_set_all_channel_9_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_9_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_set_all_channel_9_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_set_all_channel_10_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_10_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_set_all_channel_10_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_set_all_channel_11_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_11_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_set_all_channel_11_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_set_all_channel_12_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_12_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_set_all_channel_12_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_set_all_channel_13_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_13_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_set_all_channel_13_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_set_all_channel_14_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_14_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_set_all_channel_14_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_set_all_channel_15_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_15_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_set_all_channel_15_is_in_range(uint8_t value); + +/** + * Pack message Pulse_Once. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PH_pulse_once_pack( + uint8_t *dst_p, + const struct PH_pulse_once_t *src_p, + size_t size); + +/** + * Unpack message Pulse_Once. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PH_pulse_once_unpack( + struct PH_pulse_once_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_pulse_once_channel_0_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_0_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_pulse_once_channel_0_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_pulse_once_channel_1_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_1_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_pulse_once_channel_1_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_pulse_once_channel_2_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_2_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_pulse_once_channel_2_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_pulse_once_channel_3_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_3_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_pulse_once_channel_3_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_pulse_once_channel_4_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_4_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_pulse_once_channel_4_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_pulse_once_channel_5_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_5_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_pulse_once_channel_5_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_pulse_once_channel_6_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_6_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_pulse_once_channel_6_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_pulse_once_channel_7_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_7_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_pulse_once_channel_7_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_pulse_once_channel_8_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_8_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_pulse_once_channel_8_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_pulse_once_channel_9_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_9_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_pulse_once_channel_9_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_pulse_once_channel_10_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_10_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_pulse_once_channel_10_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_pulse_once_channel_11_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_11_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_pulse_once_channel_11_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_pulse_once_channel_12_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_12_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_pulse_once_channel_12_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_pulse_once_channel_13_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_13_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_pulse_once_channel_13_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_pulse_once_channel_14_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_14_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_pulse_once_channel_14_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_pulse_once_channel_15_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_15_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_pulse_once_channel_15_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PH_pulse_once_pulse_length_ms_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_pulse_length_ms_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_pulse_once_pulse_length_ms_is_in_range(uint16_t value); + +/** + * Pack message Status_0. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PH_status_0_pack( + uint8_t *dst_p, + const struct PH_status_0_t *src_p, + size_t size); + +/** + * Unpack message Status_0. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PH_status_0_unpack( + struct PH_status_0_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_0_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_0_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_0_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_1_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_1_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_1_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_2_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_2_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_2_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_3_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_3_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_3_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_4_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_4_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_4_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_5_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_5_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_5_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_6_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_6_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_6_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_7_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_7_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_7_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_8_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_8_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_8_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_9_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_9_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_9_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_10_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_10_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_10_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_11_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_11_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_11_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_12_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_12_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_12_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_13_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_13_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_13_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_14_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_14_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_14_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_15_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_15_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_15_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_analog_0_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_analog_0_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_analog_0_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_analog_1_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_analog_1_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_analog_1_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_digital_sensor_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_digital_sensor_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_digital_sensor_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_brownout_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_brownout_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_brownout_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_compressor_oc_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_compressor_oc_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_compressor_oc_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_compressor_open_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_compressor_open_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_compressor_open_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_solenoid_oc_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_solenoid_oc_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_solenoid_oc_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_can_warning_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_can_warning_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_can_warning_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_hardware_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_hardware_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_hardware_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_0_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_0_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_0_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_1_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_1_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_1_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_2_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_2_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_2_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_3_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_3_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_3_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_4_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_4_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_4_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_5_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_5_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_5_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_6_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_6_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_6_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_7_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_7_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_7_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_8_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_8_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_8_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_9_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_9_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_9_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_10_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_10_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_10_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_11_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_11_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_11_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_12_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_12_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_12_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_13_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_13_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_13_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_14_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_14_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_14_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_channel_15_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_channel_15_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_channel_15_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_compressor_on_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_compressor_on_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_compressor_on_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_system_enabled_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_system_enabled_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_system_enabled_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_robo_rio_present_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_robo_rio_present_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_robo_rio_present_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_0_compressor_config_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_0_compressor_config_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_0_compressor_config_is_in_range(uint8_t value); + +/** + * Pack message Status_1. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PH_status_1_pack( + uint8_t *dst_p, + const struct PH_status_1_t *src_p, + size_t size); + +/** + * Unpack message Status_1. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PH_status_1_unpack( + struct PH_status_1_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_1_v_bus_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_1_v_bus_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_1_v_bus_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PH_status_1_solenoid_voltage_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_1_solenoid_voltage_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_1_solenoid_voltage_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_1_compressor_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_1_compressor_current_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_1_compressor_current_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_1_solenoid_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_1_solenoid_current_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_1_solenoid_current_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_1_sticky_brownout_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_1_sticky_brownout_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_1_sticky_brownout_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_1_sticky_compressor_oc_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_1_sticky_compressor_oc_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_1_sticky_compressor_oc_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_1_sticky_compressor_open_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_1_sticky_compressor_open_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_1_sticky_compressor_open_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_1_sticky_solenoid_oc_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_1_sticky_solenoid_oc_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_1_sticky_solenoid_oc_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_1_sticky_can_warning_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_1_sticky_can_warning_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_1_sticky_can_warning_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_1_sticky_can_bus_off_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_1_sticky_can_bus_off_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_1_sticky_can_bus_off_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_1_sticky_hardware_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_1_sticky_hardware_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_1_sticky_hardware_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_1_sticky_firmware_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_1_sticky_firmware_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_1_sticky_firmware_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_1_sticky_has_reset_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_1_sticky_has_reset_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_1_sticky_has_reset_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_status_1_supply_voltage_5_v_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_1_supply_voltage_5_v_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_status_1_supply_voltage_5_v_is_in_range(uint8_t value); + +/** + * Pack message Clear_Faults. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PH_clear_faults_pack( + uint8_t *dst_p, + const struct PH_clear_faults_t *src_p, + size_t size); + +/** + * Unpack message Clear_Faults. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PH_clear_faults_unpack( + struct PH_clear_faults_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Pack message Version. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PH_version_pack( + uint8_t *dst_p, + const struct PH_version_t *src_p, + size_t size); + +/** + * Unpack message Version. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PH_version_unpack( + struct PH_version_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_version_firmware_fix_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_version_firmware_fix_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_version_firmware_fix_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_version_firmware_minor_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_version_firmware_minor_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_version_firmware_minor_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_version_firmware_year_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_version_firmware_year_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_version_firmware_year_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_version_hardware_minor_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_version_hardware_minor_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_version_hardware_minor_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_version_hardware_major_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_version_hardware_major_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_version_hardware_major_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint32_t PH_version_unique_id_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_version_unique_id_decode(uint32_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_version_unique_id_is_in_range(uint32_t value); + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/outlineviewer/build.gradle b/outlineviewer/build.gradle index 31261324eef..0538767d704 100644 --- a/outlineviewer/build.gradle +++ b/outlineviewer/build.gradle @@ -1,6 +1,6 @@ import org.gradle.internal.os.OperatingSystem -if (project.hasProperty('onlylinuxathena')) { +if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxsystemcore')) { return; } diff --git a/processstarter/build.gradle b/processstarter/build.gradle index 962a55ce86e..00788a5ca4b 100644 --- a/processstarter/build.gradle +++ b/processstarter/build.gradle @@ -1,6 +1,6 @@ import org.gradle.internal.os.OperatingSystem -if (project.hasProperty('onlylinuxathena')) { +if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxsystemcore')) { return; } diff --git a/roborioteamnumbersetter/build.gradle b/roborioteamnumbersetter/build.gradle index 344dfc96053..7831565f655 100644 --- a/roborioteamnumbersetter/build.gradle +++ b/roborioteamnumbersetter/build.gradle @@ -1,6 +1,6 @@ import org.gradle.internal.os.OperatingSystem -if (project.hasProperty('onlylinuxathena')) { +if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxsystemcore')) { return; } diff --git a/shared/config.gradle b/shared/config.gradle index 4612e5a3ca4..e0d0cd065e5 100644 --- a/shared/config.gradle +++ b/shared/config.gradle @@ -10,12 +10,13 @@ nativeUtils.addWpiNativeUtils() nativeUtils.withCrossRoboRIO() nativeUtils.withCrossLinuxArm32() nativeUtils.withCrossLinuxArm64() +nativeUtils.withCrossSystemCore() nativeUtils { wpi { configureDependencies { - opencvYear = "frc2024" + opencvYear = "frc2025" niLibVersion = "2025.0.0" - opencvVersion = "4.8.0-4" + opencvVersion = "4.8.0-1" } } } diff --git a/shared/java/javacommon.gradle b/shared/java/javacommon.gradle index 7802818b483..f9ba25bea2d 100644 --- a/shared/java/javacommon.gradle +++ b/shared/java/javacommon.gradle @@ -81,7 +81,7 @@ test { finalizedBy jacocoTestReport } -if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxarm32') || project.hasProperty('onlylinuxarm64') || project.hasProperty('onlywindowsarm64')) { +if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxsystemcore') || project.hasProperty('onlylinuxarm32') || project.hasProperty('onlylinuxarm64') || project.hasProperty('onlywindowsarm64')) { test.enabled = false } diff --git a/shared/jni/setupBuild.gradle b/shared/jni/setupBuild.gradle index 9cf2be0ffe4..62745115ea2 100644 --- a/shared/jni/setupBuild.gradle +++ b/shared/jni/setupBuild.gradle @@ -111,6 +111,7 @@ model { enableCheckTask !project.hasProperty('skipJniCheck') javaCompileTasks << compileJava jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.roborio) + jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.systemcore) jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.linuxarm32) jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.linuxarm64) sources { @@ -159,6 +160,7 @@ model { enableCheckTask !project.hasProperty('skipJniCheck') javaCompileTasks << compileJava jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.roborio) + jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.systemcore) jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.linuxarm32) jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.linuxarm64) sources { diff --git a/shared/plugins/setupBuild.gradle b/shared/plugins/setupBuild.gradle index 91cbf11ddfe..0b43c117f84 100644 --- a/shared/plugins/setupBuild.gradle +++ b/shared/plugins/setupBuild.gradle @@ -2,8 +2,9 @@ apply plugin: 'cpp' apply plugin: 'edu.wpi.first.NativeUtils' apply plugin: ExtraTasks -if (!project.hasProperty('onlylinuxathena')) { +if (!project.hasProperty('onlylinuxathena') && !project.hasProperty('onlylinuxsystemcore')) { ext.skiplinuxathena = true + ext.skiplinuxsystemcore = true apply from: "${rootDir}/shared/config.gradle" model { @@ -57,7 +58,7 @@ if (!project.hasProperty('onlylinuxathena')) { } } binaries.all { - if (!project.hasProperty('onlylinuxathena')) { + if (!project.hasProperty('onlylinuxathena') && !project.hasProperty('onlylinuxsystemcore')) { project(':hal').addHalDependency(it, 'shared') lib library: pluginName if (project.hasProperty('includeNtCore')) { @@ -84,7 +85,7 @@ if (!project.hasProperty('onlylinuxathena')) { model { tasks { def c = $.components - if (!project.hasProperty('onlylinuxathena')) { + if (!project.hasProperty('onlylinuxathena') && !project.hasProperty('onlylinuxsystemcore')) { project.tasks.create('runCpp', Exec) { group = 'WPILib' description = "Run the ${pluginName}Dev executable" diff --git a/simulation/halsim_ds_socket/build.gradle b/simulation/halsim_ds_socket/build.gradle index b9a39a8ab1f..60148d7b18b 100644 --- a/simulation/halsim_ds_socket/build.gradle +++ b/simulation/halsim_ds_socket/build.gradle @@ -1,4 +1,4 @@ -if (project.hasProperty('onlylinuxathena')) { +if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxsystemcore')) { return; } diff --git a/simulation/halsim_gui/build.gradle b/simulation/halsim_gui/build.gradle index 031cc775a8f..e346002c5e8 100644 --- a/simulation/halsim_gui/build.gradle +++ b/simulation/halsim_gui/build.gradle @@ -1,4 +1,4 @@ -if (project.hasProperty('onlylinuxathena')) { +if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxsystemcore')) { return; } diff --git a/simulation/halsim_ws_client/build.gradle b/simulation/halsim_ws_client/build.gradle index fb563ab5586..0bd66437e80 100644 --- a/simulation/halsim_ws_client/build.gradle +++ b/simulation/halsim_ws_client/build.gradle @@ -1,4 +1,4 @@ -if (project.hasProperty('onlylinuxathena')) { +if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxsystemcore')) { return; } diff --git a/simulation/halsim_ws_core/build.gradle b/simulation/halsim_ws_core/build.gradle index 5dd8d2aadf8..30f2d5b5c15 100644 --- a/simulation/halsim_ws_core/build.gradle +++ b/simulation/halsim_ws_core/build.gradle @@ -1,4 +1,4 @@ -if (project.hasProperty('onlylinuxathena')) { +if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxsystemcore')) { return; } diff --git a/simulation/halsim_ws_server/build.gradle b/simulation/halsim_ws_server/build.gradle index 8db195162e0..0a4e32c2646 100644 --- a/simulation/halsim_ws_server/build.gradle +++ b/simulation/halsim_ws_server/build.gradle @@ -1,4 +1,4 @@ -if (project.hasProperty('onlylinuxathena')) { +if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxsystemcore')) { return; } diff --git a/simulation/halsim_xrp/build.gradle b/simulation/halsim_xrp/build.gradle index e745820650a..21457efba3b 100644 --- a/simulation/halsim_xrp/build.gradle +++ b/simulation/halsim_xrp/build.gradle @@ -1,4 +1,4 @@ -if (project.hasProperty('onlylinuxathena')) { +if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxsystemcore')) { return; } diff --git a/sysid/build.gradle b/sysid/build.gradle index 58bbcddbe05..851be251914 100644 --- a/sysid/build.gradle +++ b/sysid/build.gradle @@ -1,6 +1,6 @@ import org.gradle.internal.os.OperatingSystem -if (project.hasProperty('onlylinuxathena')) { +if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxsystemcore')) { return; } diff --git a/thirdparty/imgui_suite/build.gradle b/thirdparty/imgui_suite/build.gradle index f5ddcc98830..1a695cb0b3f 100644 --- a/thirdparty/imgui_suite/build.gradle +++ b/thirdparty/imgui_suite/build.gradle @@ -1,6 +1,6 @@ import org.gradle.internal.os.OperatingSystem -if (project.hasProperty('onlylinuxathena')) { +if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxsystemcore')) { return; } diff --git a/wpigui/build.gradle b/wpigui/build.gradle index 959a821fe4b..48e78103a96 100644 --- a/wpigui/build.gradle +++ b/wpigui/build.gradle @@ -1,6 +1,6 @@ import org.gradle.internal.os.OperatingSystem -if (project.hasProperty('onlylinuxathena')) { +if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxsystemcore')) { return; } diff --git a/wpilibcIntegrationTests/build.gradle b/wpilibcIntegrationTests/build.gradle index 7ab64e7ecb7..fb65f0c63d0 100644 --- a/wpilibcIntegrationTests/build.gradle +++ b/wpilibcIntegrationTests/build.gradle @@ -98,7 +98,7 @@ model { } // This is in a separate if statement because of what I would assume is a bug in grade. // Will file an issue on their side. - if (!project.hasProperty('skiponlyathena')) { + if (!project.hasProperty('skiponlyathena') && !project.hasProperty('skiponlysystemcore')) { build.dependsOn copyWpilibCTestLibrariesToOutput } } diff --git a/wpilibjExamples/build.gradle b/wpilibjExamples/build.gradle index 115b2539650..61d7199a68d 100644 --- a/wpilibjExamples/build.gradle +++ b/wpilibjExamples/build.gradle @@ -197,7 +197,7 @@ model { } testTask.systemProperty 'java.library.path', filePath - if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxarm32') || project.hasProperty('onlylinuxarm64') || project.hasProperty('onlywindowsarm64')) { + if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxsystemcore') || project.hasProperty('onlylinuxarm32') || project.hasProperty('onlylinuxarm64') || project.hasProperty('onlywindowsarm64')) { testTask.enabled = false } test.dependsOn(testTask)