mirror of
https://github.com/BotChain-Robots/firmware.git
synced 2026-07-08 09:37:21 +02:00
44 lines
1.5 KiB
C++
44 lines
1.5 KiB
C++
//
|
|
// Created by Johnathon Slightham on 2025-07-26.
|
|
//
|
|
|
|
#include "constants/module.h"
|
|
#include "OrientationDetection.h"
|
|
#include "ConfigManager.h"
|
|
#include "iostream"
|
|
|
|
#define TAG "OrientationDetection"
|
|
|
|
void OrientationDetection::init() {
|
|
const auto& config_manager = ConfigManager::get_instance();
|
|
for (int i = 0; i < MODULE_TO_NUM_CHANNELS_MAP[config_manager.get_module_type()]; i++) {
|
|
setup_gpio(static_cast<gpio_num_t>(CHANNEL_TO_0_DEG_MAP[i]));
|
|
setup_gpio(static_cast<gpio_num_t>(CHANNEL_TO_90_DEG_MAP[i]));
|
|
setup_gpio(static_cast<gpio_num_t>(CHANNEL_TO_180_DEG_MAP[i]));
|
|
setup_gpio(static_cast<gpio_num_t>(CHANNEL_TO_270_DEG_MAP[i]));
|
|
}
|
|
}
|
|
|
|
Orientation OrientationDetection::get_orientation(const uint8_t channel) {
|
|
if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_90_DEG_MAP[channel]))) {
|
|
return Orientation_Deg90;
|
|
} else if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_180_DEG_MAP[channel]))) {
|
|
return Orientation_Deg180;
|
|
} else if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_270_DEG_MAP[channel]))) {
|
|
return Orientation_Deg270;
|
|
} else {
|
|
return Orientation_Deg0;
|
|
}
|
|
}
|
|
|
|
void OrientationDetection::setup_gpio(const gpio_num_t pin) {
|
|
const gpio_config_t io_conf = {
|
|
.pin_bit_mask = (1ULL << pin),
|
|
.mode = GPIO_MODE_INPUT,
|
|
.pull_up_en = GPIO_PULLUP_DISABLE,
|
|
.pull_down_en = GPIO_PULLDOWN_ENABLE,
|
|
.intr_type = GPIO_INTR_DISABLE
|
|
};
|
|
gpio_config(&io_conf);
|
|
}
|