diff --git a/samples/device/camera.cc b/samples/device/camera.cc index f415a41..7529db1 100644 --- a/samples/device/camera.cc +++ b/samples/device/camera.cc @@ -49,8 +49,6 @@ int main(int argc, char *argv[]) { device->SetOptionValue(Option::MAX_EXPOSURE_TIME, 120); // [0,240] device->SetOptionValue(Option::DESIRED_BRIGHTNESS, 200); // [0,255] } - */ - /* { // manual-exposure device->SetOptionValue(Option::EXPOSURE_MODE, 1); device->SetOptionValue(Option::GAIN, 20); // [0.48] @@ -61,6 +59,8 @@ int main(int argc, char *argv[]) { */ device->LogOptionInfos(); + // device->RunOptionAction(Option::ZERO_DRIFT_CALIBRATION); + std::size_t left_count = 0; device->SetStreamCallback( Stream::LEFT, [&left_count](const device::StreamData &data) { diff --git a/src/device/device.cc b/src/device/device.cc index 3dbd9bd..b76e4ab 100644 --- a/src/device/device.cc +++ b/src/device/device.cc @@ -131,6 +131,10 @@ ImuExtrinsics Device::GetImuExtrinsics() const { return imu_extrinsics_; } +void Device::LogOptionInfos() const { + channels_->LogControlInfos(); +} + OptionInfo Device::GetOptionInfo(const Option &option) const { if (!Supports(option)) { LOG(WARNING) << "Unsupported option: " << option; @@ -156,8 +160,12 @@ void Device::SetOptionValue(const Option &option, std::int32_t value) { channels_->SetControlValue(option, value); } -void Device::LogOptionInfos() const { - channels_->LogControlInfos(); +bool Device::RunOptionAction(const Option &option) const { + if (!Supports(option)) { + LOG(WARNING) << "Unsupported option: " << option; + return false; + } + return channels_->RunControlAction(option); } void Device::SetStreamCallback( diff --git a/src/device/device.h b/src/device/device.h index d9061c6..f43c734 100644 --- a/src/device/device.h +++ b/src/device/device.h @@ -61,10 +61,13 @@ class Device { ImuIntrinsics GetImuIntrinsics() const; ImuExtrinsics GetImuExtrinsics() const; + void LogOptionInfos() const; OptionInfo GetOptionInfo(const Option &option) const; + std::int32_t GetOptionValue(const Option &option) const; void SetOptionValue(const Option &option, std::int32_t value); - void LogOptionInfos() const; + + bool RunOptionAction(const Option &option) const; void SetStreamCallback(const Stream &stream, stream_callback_t callback); void SetMotionCallback(motion_callback_t callback); diff --git a/src/internal/channels.cc b/src/internal/channels.cc index 5b18002..d2ba72f 100644 --- a/src/internal/channels.cc +++ b/src/internal/channels.cc @@ -39,6 +39,19 @@ int XuCamCtrlId(Option option) { } } +int XuHalfDuplexId(Option option) { + switch (option) { + case Option::ZERO_DRIFT_CALIBRATION: + return 0; + break; + case Option::ERASE_CHIP: + return 1; + break; + default: + LOG(FATAL) << "No half duplex id for " << option; + } +} + } // namespace Channels::Channels(std::shared_ptr device) : device_(device) { @@ -161,6 +174,30 @@ void Channels::SetControlValue(const Option &option, std::int32_t value) { } } +bool Channels::RunControlAction(const Option &option) const { + switch (option) { + case Option::ZERO_DRIFT_CALIBRATION: + return XuHalfDuplexSet(option, XU_CMD_ZDC); + case Option::ERASE_CHIP: + return XuHalfDuplexSet(option, XU_CMD_ERASE); + case Option::GAIN: + case Option::BRIGHTNESS: + case Option::CONTRAST: + case Option::FRAME_RATE: + case Option::IMU_FREQUENCY: + case Option::EXPOSURE_MODE: + case Option::MAX_GAIN: + case Option::MAX_EXPOSURE_TIME: + case Option::DESIRED_BRIGHTNESS: + case Option::IR_CONTROL: + case Option::HDR_MODE: + LOG(WARNING) << option << " run action useless"; + return false; + default: + LOG(FATAL) << "Unsupported option " << option; + } +} + bool Channels::PuControlRange( Option option, int32_t *min, int32_t *max, int32_t *def) const { CHECK_NOTNULL(device_); @@ -174,8 +211,9 @@ bool Channels::PuControlQuery( } bool Channels::XuControlQuery( - uint8_t selector, uvc::xu_query query, uint16_t size, uint8_t *data) const { - return XuControlQuery({3}, selector, query, size, data); + channel_t channel, uvc::xu_query query, uint16_t size, + uint8_t *data) const { + return XuControlQuery({3}, channel >> 8, query, size, data); } bool Channels::XuControlQuery( @@ -187,7 +225,7 @@ bool Channels::XuControlQuery( bool Channels::XuCamCtrlQuery( uvc::xu_query query, uint16_t size, uint8_t *data) const { - return XuControlQuery(CHANNEL_CAM_CTRL >> 8, query, size, data); + return XuControlQuery(CHANNEL_CAM_CTRL, query, size, data); } std::int32_t Channels::XuCamCtrlGet(Option option) const { @@ -195,7 +233,7 @@ std::int32_t Channels::XuCamCtrlGet(Option option) const { std::uint8_t data[3] = {static_cast((id | 0x80) & 0xFF), 0, 0}; if (!XuCamCtrlQuery(uvc::XU_QUERY_SET, 3, data)) { - LOG(WARNING) << "Get control value of " << option << " failed"; + LOG(WARNING) << "XuCamCtrlGet value of " << option << " failed"; return -1; } @@ -203,7 +241,7 @@ std::int32_t Channels::XuCamCtrlGet(Option option) const { if (XuCamCtrlQuery(uvc::XU_QUERY_GET, 3, data)) { return (data[1] << 8) + (data[2]); } else { - LOG(WARNING) << "Get control value of " << option << " failed"; + LOG(WARNING) << "XuCamCtrlGet value of " << option << " failed"; return -1; } } @@ -214,14 +252,33 @@ void Channels::XuCamCtrlSet(Option option, std::int32_t value) const { static_cast((value >> 8) & 0xFF), static_cast(value & 0xFF)}; if (!XuCamCtrlQuery(uvc::XU_QUERY_SET, 3, data)) { - LOG(WARNING) << "Set control value of " << option << " failed"; + LOG(WARNING) << "XuCamCtrlSet value (" << value << ") of " << option + << " failed"; + } else { + VLOG(2) << "XuCamCtrlSet value (" << value << ") of " << option + << " success"; + } +} + +bool Channels::XuHalfDuplexSet(Option option, xu_cmd_t cmd) const { + int id = XuHalfDuplexId(option); + std::uint8_t data[3] = {// must be 3 now + static_cast(id & 0xFF), cmd}; + if (!XuControlQuery(CHANNEL_HALF_DUPLEX, uvc::XU_QUERY_SET, 3, data)) { + LOG(WARNING) << "XuHalfDuplexSet value (0x" << std::hex << std::uppercase + << cmd << ") of " << option << " failed"; + return false; + } else { + VLOG(2) << "XuHalfDuplexSet value (0x" << std::hex << std::uppercase << cmd + << ") of " << option << " success"; + return true; } } Channels::control_info_t Channels::PuControlInfo(Option option) const { int32_t min = 0, max = 0, def = 0; if (!PuControlRange(option, &min, &max, &def)) { - LOG(WARNING) << "Get control range of " << option << " failed"; + LOG(WARNING) << "Get PuControlInfo of " << option << " failed"; } return {min, max, def}; } @@ -231,7 +288,7 @@ Channels::control_info_t Channels::XuControlInfo(Option option) const { std::uint8_t data[3] = {static_cast((id | 0x80) & 0xFF), 0, 0}; if (!XuCamCtrlQuery(uvc::XU_QUERY_SET, 3, data)) { - LOG(WARNING) << "Get control range of " << option << " failed"; + LOG(WARNING) << "Get XuControlInfo of " << option << " failed"; return {0, 0, 0}; } @@ -241,17 +298,17 @@ Channels::control_info_t Channels::XuControlInfo(Option option) const { if (XuCamCtrlQuery(uvc::XU_QUERY_MIN, 3, data)) { info.min = (data[1] << 8) + (data[2]); } else { - LOG(WARNING) << "Get control range min of " << option << " failed"; + LOG(WARNING) << "Get XuControlInfo.min of " << option << " failed"; } if (XuCamCtrlQuery(uvc::XU_QUERY_MAX, 3, data)) { info.max = (data[1] << 8) + (data[2]); } else { - LOG(WARNING) << "Get control range max of " << option << " failed"; + LOG(WARNING) << "Get XuControlInfo.max of " << option << " failed"; } if (XuCamCtrlQuery(uvc::XU_QUERY_DEF, 3, data)) { info.def = (data[1] << 8) + (data[2]); } else { - LOG(WARNING) << "Get control range def of " << option << " failed"; + LOG(WARNING) << "Get XuControlInfo.def of " << option << " failed"; } return info; diff --git a/src/internal/channels.h b/src/internal/channels.h index 6c43619..3c02239 100644 --- a/src/internal/channels.h +++ b/src/internal/channels.h @@ -34,24 +34,31 @@ class Channels { std::int32_t def; } control_info_t; + typedef enum XuCmd { + XU_CMD_ZDC = 0xE6, // zero drift calibration + XU_CMD_ERASE = 0xDE, // erase chip + XU_CMD_LAST + } xu_cmd_t; + explicit Channels(std::shared_ptr device); ~Channels(); void LogControlInfos() const; - void UpdateControlInfos(); - control_info_t GetControlInfo(const Option &option) const; + std::int32_t GetControlValue(const Option &option) const; void SetControlValue(const Option &option, std::int32_t value); + bool RunControlAction(const Option &option) const; + private: bool PuControlRange( Option option, int32_t *min, int32_t *max, int32_t *def) const; bool PuControlQuery(Option option, uvc::pu_query query, int32_t *value) const; bool XuControlQuery( - uint8_t selector, uvc::xu_query query, uint16_t size, + channel_t channel, uvc::xu_query query, uint16_t size, uint8_t *data) const; bool XuControlQuery( const uvc::xu &xu, uint8_t selector, uvc::xu_query query, uint16_t size, @@ -61,6 +68,8 @@ class Channels { std::int32_t XuCamCtrlGet(Option option) const; void XuCamCtrlSet(Option option, std::int32_t value) const; + bool XuHalfDuplexSet(Option option, xu_cmd_t cmd) const; + control_info_t PuControlInfo(Option option) const; control_info_t XuControlInfo(Option option) const;