diff --git a/a8000_protocol b/a8000_protocol index a85673b..c3b66e0 160000 --- a/a8000_protocol +++ b/a8000_protocol @@ -1 +1 @@ -Subproject commit a85673b9c1b9a54c0e91a3e3f4299106a749e56d +Subproject commit c3b66e0656239c61edc573842b420863a5cd3832 diff --git a/sdk b/sdk index 9aee8f2..49aa1c4 160000 --- a/sdk +++ b/sdk @@ -1 +1 @@ -Subproject commit 9aee8f2a2c26251c921e81bca81abc3a3a2f6927 +Subproject commit 49aa1c4693983ad6031f71b0c0b785c6721d9868 diff --git a/usrc/public_service/gins.c b/usrc/public_service/gins.c index 4021de9..3e013fd 100644 --- a/usrc/public_service/gins.c +++ b/usrc/public_service/gins.c @@ -31,6 +31,7 @@ DEFINE_GLOBAL(DMA_HandleTypeDef, hdma4_stream3); DEFINE_GLOBAL(SPI_HandleTypeDef, hspi1); DEFINE_GLOBAL(SPI_HandleTypeDef, hspi2); +DEFINE_GLOBAL(SPI_HandleTypeDef, hspi3); /*********************************************************************************************************************** * PTR * diff --git a/usrc/public_service/gins.h b/usrc/public_service/gins.h index 63e0b07..1ac1a57 100644 --- a/usrc/public_service/gins.h +++ b/usrc/public_service/gins.h @@ -53,6 +53,7 @@ EXTERN_GLOBAL(DMA_HandleTypeDef, hdma4_stream3); EXTERN_GLOBAL(SPI_HandleTypeDef, hspi1); EXTERN_GLOBAL(SPI_HandleTypeDef, hspi2); +EXTERN_GLOBAL(SPI_HandleTypeDef, hspi3); /*********************************************************************************************************************** * PTR * diff --git a/usrc/subboards/subboard90_optical_module/optical_module.cpp b/usrc/subboards/subboard90_optical_module/optical_module.cpp new file mode 100644 index 0000000..e756441 --- /dev/null +++ b/usrc/subboards/subboard90_optical_module/optical_module.cpp @@ -0,0 +1,698 @@ +#include "optical_module.hpp" + +#include +#include +#include + +using namespace iflytop; +using namespace std; +#define RAW_SECTION_SIZE 128 + +#define TAG "OPT" +#define LOGI(fmt, ...) ZLOGI(TAG, fmt, ##__VA_ARGS__) + +#define ACTION_SELECT_CHANNEL 1 // 选择通道 +#define ACTION_OPEN_LASER 2 // 打开激光器 +#define ACTION_CLOSE_LASER 3 // 关闭激光器 +#define ACTION_READ_SCAN_ADC 4 // 读取扫描放大器adc值 +#define ACTION_READ_LASTER_ADC 5 // 读取激光器adc值 +#define ACTION_READ_ONE_POINT 6 // 读取一个点的数据 +#define ACTION_SET_LASTER_GAIN 7 // +#define ACTION_SET_SCANER_GAIN 8 // +#define ACTION_CLOSE_ALL 9 // +#define ACTION_OPEN_AMP_NEGATIVE_5V 10 // 打开-5V +#define ACTION_CLOSE_AMP_NEGATIVE_5V 11 // 关闭-5V +#define ACTION_DUMP_RESULT 12 // 打印结果 +#define ACTION_AUTO_ADJUST_DETECTOR_GAIN 13 // 自动调整增益倍数 + +#define UVLED_ON_DURATION_TIME 500 // 500us +#define UVLED_OFF_DURATION_TIME 100 // 100us +#define SCAN_DELAY_TIME 400 // 400us +#define SCAN_DURATION_TIME 1000 // 1000us +#define TIME_RESOLVING_DENSITY 100 + +void OpticalModule::initialize(int32_t moduleid, hardware_config_t* hardwarecfg) { // + m_id = moduleid; + m_hardwarecfg = *hardwarecfg; + + m_t_optical_amp_mcp41_ohm.initialize(&m_hardwarecfg.t_optical_amp_mcp41); + m_t_optical_scaner_mcp41_ohm.initialize(&m_hardwarecfg.t_optical_scaner_mcp41); + m_f_optical_amp_mcp41_ohm.initialize(&m_hardwarecfg.f_optical_amp_mcp41); + m_f_optical_scaner_mcp41_ohm.initialize(&m_hardwarecfg.f_optical_scaner_mcp41); + + m_t_laster_enable_io.initAsOutput(&m_hardwarecfg.t_laster_enable_io); + // m_t_opt_scan_amp_enable_io.initAsOutput(&m_hardwarecfg.t_opt_scan_amp_enable_io); + m_f_opt_laster_enable_io.initAsOutput(&m_hardwarecfg.f_opt_laster_enable_io); + // m_f_opt_scan_amp_enable_io.initAsOutput(&m_hardwarecfg.f_opt_scan_amp_enable_io); + m_channel_select_io_red_led3.initAsOutput(&m_hardwarecfg.channel_select_io_red_led3); + m_channel_select_io_blue_led1.initAsOutput(&m_hardwarecfg.channel_select_io_blue_led1); + m_channel_select_io_green_led1.initAsOutput(&m_hardwarecfg.channel_select_io_green_led1); + m_amp_negative_5v_gpio.initAsOutput(&m_hardwarecfg.amp_negative_5v_gpio); + + m_t_amp_sw_io.initAsOutput(&m_hardwarecfg.t_amp_sw_io); + m_f_amp_sw_io.initAsOutput(&m_hardwarecfg.f_amp_sw_io); + + t_laster_adc.initialize(&m_hardwarecfg.t_laster_adc); + t_result_adc.initialize(&m_hardwarecfg.t_result_adc); + f_laster_adc.initialize(&m_hardwarecfg.f_laster_adc); + f_result_adc.initialize(&m_hardwarecfg.f_result_adc); + + motor = (StepMotorCtrlModule*)m_hardwarecfg.motor; + + m_t_amp_sw_io.setState(true); + m_f_amp_sw_io.setState(true); + + m_reg.module_raw_sector_size = RAW_SECTION_SIZE; + m_reg.module_raw_sector_num = 0; + + m_reg.trf_uvled_on_duration_us = UVLED_ON_DURATION_TIME; + m_reg.trf_uvled_off_duration_us = UVLED_OFF_DURATION_TIME; + m_reg.trf_scan_delay_us = SCAN_DELAY_TIME; + m_reg.trf_scan_duration_us = SCAN_DURATION_TIME; + + ZLOGI(TAG, "-----------------module info--------------") ZLOGI(TAG, "f_detector max gain : %f", f_detector_raw_gain_to_gain(255)); + ZLOGI(TAG, "f_detector min gain : %f", f_detector_raw_gain_to_gain(1)); + ZLOGI(TAG, "t_detector max gain : %f", t_detector_raw_gain_to_gain(255)); + ZLOGI(TAG, "t_detector min gain : %f", t_detector_raw_gain_to_gain(1)); + + m_thread.init("optical_module", 1024); +} + +int32_t OpticalModule::module_get_status(int32_t* status) { + *status = m_thread.isworking() ? 1 : 0; + return 0; +} + +int32_t OpticalModule::module_factory_reset() { return 0; } +int32_t OpticalModule::module_flush_cfg() { return 0; } +int32_t OpticalModule::module_active_cfg() { return 0; } + +/** + * @brief F光学 采样增益倍数 向 原始寄存器值 转换 + * + * @param scan_gain + * @return int32_t + */ +int32_t OpticalModule::f_detector_gain_to_raw_gain(float scan_gain) { + // int32_t scan_gain = (((100. / 256.) * (float)scan_gain_raw) + 0.125) / 2.15 + 1.; + int32_t scan_gain_raw = 0; + scan_gain_raw = ((scan_gain - 1.0) * 2.15 - 0.125) * 255. / 100. + 0.5; + if (scan_gain_raw < 1) scan_gain_raw = 1; + if (scan_gain_raw > 255) scan_gain_raw = 255; + return scan_gain_raw; +} +/** + * @brief F光学 原始寄存器值 向 采样增益倍数 转换 + * + * @param gain + * @return float + */ +float OpticalModule::f_detector_raw_gain_to_gain(int32_t gain) { + float scan_gain = 0; + scan_gain = (((100. / 256.) * (float)gain) + 0.125) / 2.15 + 1.; + return scan_gain; +} +int32_t OpticalModule::t_detector_gain_to_raw_gain(float scan_gain) { + // opamp_gain = (((100.0 * (float) scan_gain_raw) / 255) + 2.4) / 4.7; + int32_t scan_gain_raw = 0; + scan_gain_raw = (scan_gain * 4.7 - 2.4) * 256. / 100. + 0.5; + if (scan_gain_raw < 1) scan_gain_raw = 1; + if (scan_gain_raw > 255) scan_gain_raw = 255; + return scan_gain_raw; +} +float OpticalModule::t_detector_raw_gain_to_gain(int32_t gain) { + float scan_gain = 0; + scan_gain = (((100.0 * (float)gain) / 256) + 2.4) / 4.7; + return scan_gain; +} + +int32_t OpticalModule::adjust_detector_gain() { + if (m_reg.scan_type == kf_optical) { + // 1. 找到最大的ADC采样数值 + // 2. 计算理论放大倍数 + int32_t adcgoal = 3600; + int32_t maxadc = adc_capture_buf[0]; + for (int32_t i = 1; i < adc_capture_point_num; i++) { + if (maxadc < adc_capture_buf[i]) { + maxadc = adc_capture_buf[i]; + } + } + + float nowgain = f_detector_raw_gain_to_gain(m_reg.scan_gain); + float gain_adjust = 0; + if (maxadc != 0) { + gain_adjust = nowgain * ((float)adcgoal / (float)maxadc); + } else { + gain_adjust = nowgain; + } + int32_t rawgain = f_detector_gain_to_raw_gain(gain_adjust); + m_reg.scan_gain_adjust_suggestion = rawgain; + m_reg.adc_result_overflow = maxadc > adcgoal ? 1 : 0; + + a8000_optical_open_laser(kf_optical); + osDelay(100); + a8000_optical_read_laster_adc_val(kf_optical, &m_reg.laster_intensity); + a8000_optical_close_laser(kf_optical); + + ZLOGI(TAG, "----------------result info----------------"); + ZLOGI(TAG, "maxadc :%d", maxadc); + ZLOGI(TAG, "nowgain :%d(%f)", m_reg.scan_gain, nowgain); + ZLOGI(TAG, "gain_adjust :%d(%f)", m_reg.scan_gain_adjust_suggestion, gain_adjust); + ZLOGI(TAG, "laster_intensity :%d", m_reg.laster_intensity); + + } else if (m_reg.scan_type == kt_optical) { + int32_t adcgoal = 3600; + int32_t maxadc = adc_capture_buf[0]; + int32_t sumadc = 0; + int32_t avg_adc = 0; + for (int32_t i = 1; i < adc_capture_point_num; i++) { + if (maxadc < adc_capture_buf[i]) { + maxadc = adc_capture_buf[i]; + } + sumadc += adc_capture_buf[i]; + } + avg_adc = sumadc * 1.0 / adc_capture_point_num; + + float nowgain = t_detector_raw_gain_to_gain(m_reg.scan_gain); + float gain_adjust = 0; + if (maxadc != 0) { + gain_adjust = nowgain * ((float)adcgoal / (float)maxadc); + } else { + gain_adjust = nowgain; + } + int32_t rawgain = 0; + + if (maxadc > adcgoal) { + rawgain = 1; + } else if ((avg_adc > 3000) || (avg_adc < 10)) { + rawgain = 6; // 这里参考巴迪泰源码,巴迪泰的也没有解释为什么做这个判断 + } else { + rawgain = t_detector_gain_to_raw_gain(gain_adjust); + } + + m_reg.scan_gain_adjust_suggestion = rawgain; + m_reg.adc_result_overflow = maxadc > adcgoal ? 1 : 0; + + int32_t lasterval = 0; + a8000_optical_open_laser(kt_optical); + _open_t_amp_n5v(); + osDelay(300); + a8000_optical_read_laster_adc_val(kt_optical, &m_reg.laster_intensity); // 读取两次,第一次读取的值不准确 + a8000_optical_close_laser(kt_optical); + _close_t_amp_n5v(); + + ZLOGI(TAG, "----------------result info----------------"); + ZLOGI(TAG, "maxadc :%d", maxadc); + ZLOGI(TAG, "avgadc :%d", avg_adc); + ZLOGI(TAG, "nowgain :%d(%f)", m_reg.scan_gain, nowgain); + ZLOGI(TAG, "gain_adjust :%d(%f)", m_reg.scan_gain_adjust_suggestion, gain_adjust); + ZLOGI(TAG, "laster_intensity :%d", m_reg.laster_intensity); + } + return 0; +} + +int32_t OpticalModule::module_xxx_reg(int32_t param_id, bool read, int32_t& val) { + switch (param_id) { + MODULE_COMMON_PROCESS_REG_CB(); + // PROCESS_REG(kreg_module_version, /* */ REG_GET(0x0001), ACTION_NONE); + // PROCESS_REG(kreg_module_type, /* */ REG_GET(0), ACTION_NONE); + // PROCESS_REG(kreg_module_do_action0, /* */ ACTION_NONE, do_action(val)); + PROCESS_REG(kreg_laster_scaner_raw_sector_size, raw_get_sector_size(&val), ACTION_NONE); + PROCESS_REG(kreg_laster_scaner_raw_sector_num, raw_get_sector_num(&val), ACTION_NONE); + + PROCESS_REG(kreg_boditech_optical_scan_type, REG_GET(m_reg.scan_type), REG_SET(m_reg.scan_type)); + PROCESS_REG(kreg_boditech_optical_scan_start_pos, REG_GET(m_reg.scan_start_pos), REG_SET(m_reg.scan_start_pos)); + PROCESS_REG(kreg_boditech_optical_scan_direction, REG_GET(m_reg.scan_direction), REG_SET(m_reg.scan_direction)); + PROCESS_REG(kreg_boditech_optical_scan_step_interval, REG_GET(m_reg.scan_step_interval), REG_SET(m_reg.scan_step_interval)); + PROCESS_REG(kreg_boditech_optical_scan_pointnum, REG_GET(m_reg.scan_pointnum), REG_SET(m_reg.scan_pointnum)); + PROCESS_REG(kreg_boditech_optical_channel_select_num, REG_GET(m_reg.channel_select_num), REG_SET(m_reg.channel_select_num)); + PROCESS_REG(kreg_boditech_optical_laster_gain, REG_GET(m_reg.laster_gain), REG_SET(m_reg.laster_gain)); + PROCESS_REG(kreg_boditech_optical_scan_gain, REG_GET(m_reg.scan_gain), REG_SET(m_reg.scan_gain)); + + PROCESS_REG(kreg_boditech_optical_trf_uvled_on_duration_us, REG_GET(m_reg.trf_uvled_on_duration_us), REG_SET(m_reg.trf_uvled_on_duration_us)); + PROCESS_REG(kreg_boditech_optical_trf_uvled_off_duration_us, REG_GET(m_reg.trf_uvled_off_duration_us), REG_SET(m_reg.trf_uvled_off_duration_us)); + PROCESS_REG(kreg_boditech_optical_trf_scan_delay_us, REG_GET(m_reg.trf_scan_delay_us), REG_SET(m_reg.trf_scan_delay_us)); + PROCESS_REG(kreg_boditech_optical_trf_scan_duration_us, REG_GET(m_reg.trf_scan_duration_us), REG_SET(m_reg.trf_scan_duration_us)); + PROCESS_REG(kreg_boditech_optical_scan_gain_adjust_suggestion, REG_GET(m_reg.scan_gain_adjust_suggestion), ACTION_NONE); + PROCESS_REG(kreg_boditech_optical_adc_result_overflow, REG_GET(m_reg.adc_result_overflow), ACTION_NONE); + PROCESS_REG(kreg_boditech_optical_laster_intensity, REG_GET(m_reg.laster_intensity), ACTION_NONE); + + // PROCESS_REG(kreg_module_action_param1, /* */ REG_GET(m_reg.module_action_param1), REG_SET(m_reg.module_action_param1)); + // PROCESS_REG(kreg_module_action_param2, /* */ REG_GET(m_reg.module_action_param2), REG_SET(m_reg.module_action_param2)); + // PROCESS_REG(kreg_module_action_param3, /* */ REG_GET(m_reg.module_action_param3), REG_SET(m_reg.module_action_param3)); + // PROCESS_REG(kreg_module_action_ack1, /* */ REG_GET(m_reg.module_action_ack1), ACTION_NONE); + // PROCESS_REG(kreg_module_action_ack2, /* */ REG_GET(m_reg.module_action_ack2), ACTION_NONE); + default: + return err::kmodule_not_find_config_index; + break; + } + return 0; +} + +int32_t OpticalModule::do_action(int32_t action) { + if (action == ACTION_SELECT_CHANNEL) { + return select_f_channel(m_reg.channel_select_num); + } else if (action == ACTION_OPEN_LASER) { + return a8000_optical_open_laser(m_reg.scan_type); + } else if (action == ACTION_CLOSE_LASER) { + return a8000_optical_close_laser(m_reg.scan_type); + } else if (action == ACTION_READ_SCAN_ADC) { + // TODO: + // int32_t ret = a8000_optical_read_scanner_adc_val(m_reg.scan_type, &m_com_reg.module_action_ack1); + return 0; + } else if (action == ACTION_READ_LASTER_ADC) { + // TODO: + // return a8000_optical_read_laster_adc_val(m_reg.scan_type, &m_com_reg.module_action_ack1); + return 0; + } else if (action == ACTION_READ_ONE_POINT) { + // TODO: + // return a8000_optical_scan_current_point_amp_adc_val(m_reg.scan_type, m_com_reg.module_action_param1, m_com_reg.module_action_param2, &m_com_reg.module_action_ack1, &m_com_reg.module_action_ack2); + return 0; + + } else if (action == ACTION_SET_LASTER_GAIN) { + return a8000_optical_set_laster_gain(m_reg.scan_type, m_reg.laster_gain); + } else if (action == ACTION_SET_SCANER_GAIN) { + return a8000_optical_set_scan_amp_gain(m_reg.scan_type, m_reg.scan_gain); + } else if (action == ACTION_CLOSE_ALL) { + a8000_optical_close_laser(kf_optical); + a8000_optical_close_laser(kt_optical); + } else if (action == ACTION_OPEN_AMP_NEGATIVE_5V) { + return _open_t_amp_n5v(); + } else if (action == ACTION_CLOSE_AMP_NEGATIVE_5V) { + return _close_t_amp_n5v(); + } else if (action == ACTION_DUMP_RESULT) { + return dumpresult(); + } + return err::kmodule_not_support_action; +} + +int32_t OpticalModule::module_stop() { + m_thread.stop(); + return 0; +} +int32_t OpticalModule::module_break() { + m_thread.stop(); + return 0; +} +int32_t OpticalModule::module_start() { + if (m_reg.scan_type == 0) { // main,f光学 + return start_f_optical_scan(); + } else if (m_reg.scan_type == 1) { // trf,t光学 + return start_t_optical_scan(); + } else { + return err::kparam_out_of_range; + } + return 0; +} + +int32_t OpticalModule::start_t_optical_scan() { + m_thread.stop(); + m_thread.start([this]() { + ZLOGI(TAG, "--------------start_t_optical_scan-----------"); + ZLOGI(TAG, "scan_start_pos : %d", m_reg.scan_start_pos); + ZLOGI(TAG, "scan_direction : %d", m_reg.scan_direction); + ZLOGI(TAG, "scan_step_interval : %d", m_reg.scan_step_interval); + ZLOGI(TAG, "scan_pointnum : %d", m_reg.scan_pointnum); + ZLOGI(TAG, "channel_select_num : %d", m_reg.channel_select_num); + ZLOGI(TAG, "laster_gain : %d", m_reg.laster_gain); + ZLOGI(TAG, "scan_gain : %d", m_reg.scan_gain); + ZLOGI(TAG, ""); + + move_to_block(m_reg.scan_start_pos); + + // 设置激光亮度增益 + a8000_optical_set_laster_gain(kt_optical, m_reg.laster_gain); + // 设置接收器增益 + a8000_optical_set_scan_amp_gain(kt_optical, m_reg.scan_gain); + // 选通ADC反馈通道 + // select_f_channel(m_reg.channel_select_num); + // 打开负5v电源 + _open_t_amp_n5v(); + // 打开激光器 + // a8000_optical_open_laser(kt_optical); + + osDelay(10); + + int step = 0; + int pointnum = 0; + int32_t adcval = 0; + adc_capture_point_num = 0; + + int32_t interval_step = 0; + if (m_reg.scan_direction >= 0) { + interval_step = m_reg.scan_step_interval; + } else { + interval_step = -m_reg.scan_step_interval; + } + motor_prepare(); + while (true) { + // 移动到第一个点 + move_to_block(m_reg.scan_start_pos + step); + // osDelay(1); + // f_read_one_point(m_reg.laster_gain, m_reg.scan_gain, adcval); + t_read_one_point(m_reg.laster_gain, m_reg.scan_gain, adcval); + + step += interval_step; + pointnum++; + adc_capture_point_num++; + adc_capture_buf[adc_capture_point_num] = adcval; + + if (pointnum >= m_reg.scan_pointnum) { + break; + } + + if (m_thread.getExitFlag()) { + break; + } + } + m_t_laster_enable_io.setState(false); + motor_release(); + _close_t_amp_n5v(); + a8000_optical_close_laser(kf_optical); + if (!m_thread.getExitFlag()) adjust_detector_gain(); + }); + return 0; + + // adc_capture_buf[0] +} +int32_t OpticalModule::start_f_optical_scan() { + m_thread.stop(); + m_thread.start([this]() { + ZLOGI(TAG, "--------------start_f_optical_scan-----------"); + ZLOGI(TAG, "scan_start_pos : %d", m_reg.scan_start_pos); + ZLOGI(TAG, "scan_direction : %d", m_reg.scan_direction); + ZLOGI(TAG, "scan_step_interval : %d", m_reg.scan_step_interval); + ZLOGI(TAG, "scan_pointnum : %d", m_reg.scan_pointnum); + ZLOGI(TAG, "channel_select_num : %d", m_reg.channel_select_num); + ZLOGI(TAG, "laster_gain : %d", m_reg.laster_gain); + ZLOGI(TAG, "scan_gain : %d", m_reg.scan_gain); + ZLOGI(TAG, ""); + + move_to_block(m_reg.scan_start_pos); + + // 设置激光亮度增益 + a8000_optical_set_laster_gain(kf_optical, m_reg.laster_gain); + // 设置接收器增益 + a8000_optical_set_scan_amp_gain(kf_optical, m_reg.scan_gain); + // 选通ADC反馈通道 + select_f_channel(m_reg.channel_select_num); + // 打开负5v电源 + _close_t_amp_n5v(); + // 打开激光器 + a8000_optical_open_laser(kf_optical); + + osDelay(10); + + int step = 0; + int pointnum = 0; + int32_t adcval = 0; + adc_capture_point_num = 0; + + int32_t interval_step = 0; + if (m_reg.scan_direction >= 0) { + interval_step = m_reg.scan_step_interval; + } else { + interval_step = -m_reg.scan_step_interval; + } + motor_prepare(); + while (true) { + // 移动到第一个点 + move_to_block(m_reg.scan_start_pos + step); + f_read_one_point(m_reg.laster_gain, m_reg.scan_gain, adcval); + + step += interval_step; + pointnum++; + adc_capture_point_num++; + adc_capture_buf[adc_capture_point_num] = adcval; + + if (pointnum >= m_reg.scan_pointnum) { + break; + } + + if (m_thread.getExitFlag()) { + break; + } + osDelay(1); + } + motor_release(); + a8000_optical_close_laser(kf_optical); + adjust_detector_gain(); + }); + + // adc_capture_buf[0] + return 0; +} + +int32_t OpticalModule::dumpresult() { + for (int32_t i = 0; i < adc_capture_point_num; i++) { + printf("%d,%d,%d", i, (i * m_reg.scan_step_interval * m_reg.scan_direction + m_reg.scan_start_pos), adc_capture_buf[i]); + if (i != 0 && i % 5 == 0) + printf("\n"); + else { + printf("|"); + } + } + printf("\n"); + return 0; +} +int32_t OpticalModule::raw_get_sector_size(int32_t* val) { + *val = RAW_SECTION_SIZE; + return 0; +} +int32_t OpticalModule::raw_get_sector_num(int32_t* val) { + *val = adc_capture_point_num * 2 / RAW_SECTION_SIZE; + if (adc_capture_point_num * 2 % RAW_SECTION_SIZE != 0) { + *val += 1; + } + return 0; +} +int32_t OpticalModule::module_read_raw(int32_t index, uint8_t* data, int32_t* len) { + int32_t sector_size = RAW_SECTION_SIZE; + int32_t rawsize = adc_capture_point_num * 2; + + if (sector_size > *len) return err::kbuffer_not_enough; + uint16_t add = index * sector_size; + + if (add >= rawsize) return err::kparam_out_of_range; + + if (add + sector_size > rawsize) { // rawsize 127 sector_size 128 add = 0 + *len = rawsize - add; + } else { + *len = sector_size; + } + memcpy(data, (uint8_t*)adc_capture_buf + add, *len); + return 0; +} + +void OpticalModule::opt_module_power_ctrl(bool on) { m_amp_negative_5v_gpio.setState(on); } +int32_t OpticalModule::a8000_optical_module_power_ctrl(int32_t state) { + opt_module_power_ctrl(state > 0 ? true : false); + return 0; +} + +/******************************************************************************* + * DEBUG_INTERFACE * + *******************************************************************************/ + +int32_t OpticalModule::select_f_channel(int32_t channel) { + ZLOGI(TAG, "CAHNNEL select %d", channel); + if (channel == 1) { // MAIN_DETECTOR_SEL + m_channel_select_io_red_led3.setState(true); + m_channel_select_io_blue_led1.setState(false); + m_channel_select_io_green_led1.setState(false); + } else if (channel == 2) { // HBA1C_DETECTOR + m_channel_select_io_red_led3.setState(false); + m_channel_select_io_blue_led1.setState(true); + m_channel_select_io_green_led1.setState(false); + } else if (channel == 3) { // BARCODE_DETECTOR + m_channel_select_io_red_led3.setState(false); + m_channel_select_io_blue_led1.setState(false); + m_channel_select_io_green_led1.setState(true); + } else { + m_channel_select_io_red_led3.setState(false); + m_channel_select_io_blue_led1.setState(false); + m_channel_select_io_green_led1.setState(false); + } + return 0; +} + +int32_t OpticalModule::a8000_optical_open_laser(int32_t type) { // + // LOGI("a8000_optical_open_laser %d", type); + if (type == kf_optical) { + m_f_opt_laster_enable_io.setState(true); + } else if (type == kt_optical) { + m_t_laster_enable_io.setState(true); + } + return 0; +} +int32_t OpticalModule::a8000_optical_close_laser(int32_t type) { // + // LOGI("a8000_optical_close_laser %d", type); + if (type == kf_optical) { + m_f_opt_laster_enable_io.setState(false); + } else if (type == kt_optical) { + m_t_laster_enable_io.setState(false); + } + return 0; +} +int32_t OpticalModule::a8000_optical_set_laster_gain(int32_t type, int32_t gain) { // + // LOGI("a8000_optical_set_laster_gain %d %d", type, gain); + if (type == kf_optical) { + m_f_optical_amp_mcp41_ohm.setPotentiometerValue_0((uint8_t)gain); + } else if (type == kt_optical) { + m_t_optical_amp_mcp41_ohm.setPotentiometerValue_0((uint8_t)gain); + } + return 0; +} +int32_t OpticalModule::a8000_optical_set_scan_amp_gain(int32_t type, int32_t gain) { // + // LOGI("a8000_optical_set_scan_amp_gain %d %d", type, gain); + if (type == kf_optical) { + ZLOGI(TAG, "set f scan gain %d %.2f", gain, f_detector_raw_gain_to_gain(gain)); + m_f_optical_scaner_mcp41_ohm.setPotentiometerValue_0((uint8_t)gain); + } else if (type == kt_optical) { + ZLOGI(TAG, "set t scan gain %d %.2f", gain, t_detector_raw_gain_to_gain(gain)); + m_t_optical_scaner_mcp41_ohm.setPotentiometerValue_0((uint8_t)gain); + } + return 0; +} +int32_t OpticalModule::a8000_optical_read_laster_adc_val(int32_t type, int32_t* adcval) { // + // LOGI("a8000_optical_read_laster_adc_val %d", m_reg.scan_type); + if (type == kf_optical) { + f_laster_adc.get_adc_value(*adcval); + } else if (type == kt_optical) { + t_laster_adc.get_adc_value(*adcval); + } + ZLOGI(TAG, "read laster adc %d %fv", *adcval, *adcval / 4096.0 * 3.3); + return 0; +} +int32_t OpticalModule::a8000_optical_read_scanner_adc_val(int32_t type, int32_t* adcval) { + if (type == kf_optical) { + f_result_adc.get_adc_value(*adcval); + } else if (type == kt_optical) { + t_result_adc.get_adc_value(*adcval); + } + ZLOGI(TAG, "read scan adc %d %fv", *adcval, *adcval / 4096.0 * 3.3); + return 0; +} +// int32_t OpticalModule::a8000_optical_scan_current_point_amp_adc_val(int32_t type, int32_t lastergain, int32_t ampgain, int32_t* laster_fb_val, int32_t* adcval) { // +// // LOGI("a8000_optical_scan_current_point_amp_adc_val %d %d %d", type, lastergain, ampgain); +// if (type == kf_optical) { +// return f_read_one_point(lastergain, ampgain, *laster_fb_val, *adcval); +// } else if (type == kt_optical) { +// return t_read_one_point(lastergain, ampgain, *laster_fb_val, *adcval); +// } +// return err::kparam_out_of_range; +// } + +#define MAX_SAMPLING_NUM 14 + +int32_t OpticalModule::f_read_one_point(int32_t lastergain, int32_t ampgain, int32_t& adcval) { // + for (int32_t i = 0; i < MAX_SAMPLING_NUM; i++) { + f_result_adc.get_adc_value(f_adc_cache[i]); + } + // a8000_optical_close_laser(kf_optical); + + // 对采样值进行排序 + for (int32_t i = 0; i < MAX_SAMPLING_NUM; i++) { + for (int32_t j = i + 1; j < MAX_SAMPLING_NUM; j++) { + if (f_adc_cache[i] > f_adc_cache[j]) { + int32_t temp = f_adc_cache[i]; + f_adc_cache[i] = f_adc_cache[j]; + f_adc_cache[j] = temp; + } + } + } + + int32_t adcsumval = 0; + for (int32_t i = 3; i < (MAX_SAMPLING_NUM - 3); i++) { + adcsumval += f_adc_cache[i]; + } + + adcval = (((float)adcsumval / (float)(MAX_SAMPLING_NUM - 6)) + 0.5L); + return 0; +} + +int32_t OpticalModule::t_read_one_point(int32_t lastergain, int32_t ampgain, int32_t& adcval) { + // a8000_optical_set_laster_gain(kt_optical, lastergain); + // a8000_optical_set_scan_amp_gain(kt_optical, ampgain); + + GPIO_TypeDef* tlaster_port = m_t_laster_enable_io.getPort(); + uint16_t tlaster_pin = m_t_laster_enable_io.getPin(); + uint32_t openstate = 0; + uint32_t closestate = 0; + + if (m_t_laster_enable_io.isMirror()) { + openstate = tlaster_pin << 16U; + closestate = tlaster_pin; + } else { + openstate = tlaster_pin; + closestate = tlaster_pin << 16U; + } + + /** + * @brief + * 1.以下延迟需要精准控制,需要临时禁制操作系统的调度 + * 2.以下操作禁止打印任何日志,以防止时间错乱 + * 3.由于延时属于us级别延迟,这里直接操作GPIO寄存器 + */ + chip_critical_enter(); + tlaster_port->BSRR = closestate; + zchip_clock_early_delayus2(UVLED_OFF_DURATION_TIME); + tlaster_port->BSRR = openstate; + zchip_clock_early_delayus2(UVLED_ON_DURATION_TIME); + tlaster_port->BSRR = closestate; + zchip_clock_early_delayus2(SCAN_DELAY_TIME); + zchip_clock_early_delayus_timer_start(); + + int32_t sumadcval = 0; + int32_t currentadcval = 0; + + // t_laster_adc.get_adc_value(laster_fb_val); + for (int32_t i = 0; i < TIME_RESOLVING_DENSITY; i++) { + t_result_adc.get_adc_value(currentadcval); + sumadcval += currentadcval; + } + adcval = (sumadcval / TIME_RESOLVING_DENSITY); + + uint32_t haspassed = zchip_clock_early_delayus_timer_haspassed(); + zchip_clock_early_delayus_timer_stop(); + // if (haspassed < SCAN_DURATION_TIME) zchip_clock_early_delayus2(SCAN_DURATION_TIME - haspassed); + chip_critical_exit(); + if (!(haspassed < 700 && haspassed > 400)) { + ZLOGW(TAG, "t capture adc time out of range %d", haspassed); + } + // tlaster_port->BSRR = openstate; + return 0; +} + +int32_t OpticalModule::_open_t_amp_n5v() { + m_amp_negative_5v_gpio.setState(true); + return 0; +} +int32_t OpticalModule::_close_t_amp_n5v() { + m_amp_negative_5v_gpio.setState(false); + return 0; +} + +int32_t OpticalModule::motor_prepare() { + auto _motor = motor->getMotor(); + // _motor->setNoAccLimit(true); + return 0; +} +int32_t OpticalModule::motor_release() { + auto _motor = motor->getMotor(); + // _motor->setNoAccLimit(false); + return 0; +} + +int32_t OpticalModule::move_to_block(int32_t pos) { + auto _motor = motor->getMotor(); + _motor->setAcceleration(3600); + _motor->setDeceleration(3600); + _motor->moveTo(pos, motor->getCfg()->motor_default_velocity); + while (!_motor->isReachTarget() && !m_thread.getExitFlag()) { + // zos_early_delayus(100); + // osDelay(1); + } + _motor->stop(); + return 0; +} diff --git a/usrc/subboards/subboard90_optical_module/optical_module.hpp b/usrc/subboards/subboard90_optical_module/optical_module.hpp new file mode 100644 index 0000000..19943a7 --- /dev/null +++ b/usrc/subboards/subboard90_optical_module/optical_module.hpp @@ -0,0 +1,185 @@ +#pragma once +#include "sdk\components\sensors\mcp41xxx\mcp41xxx.hpp" +#include "sdk\os\zos.hpp" +// +#include "a8000_protocol\protocol.hpp" +#include "sdk\chip\api\zi_adc.hpp" +#include "sdk\components\hardware\adc\z_simple_adc.hpp" +#include "sdk\components\step_motor_ctrl_module\step_motor_ctrl_module.hpp" + +namespace iflytop { +using namespace std; + +class OpticalModule : public ZIModule, public ZIA8000OpticalModule { + ENABLE_MODULE(OpticalModule, ka8000_optical_module, 1); + + public: + typedef struct { + ZGPIO::OutputGpioCfg_t amp_negative_5v_gpio; + + ZADC::adc_config_t t_laster_adc; + ZADC::adc_config_t t_result_adc; + MCP41XXX::hardware_config_t t_optical_amp_mcp41; + MCP41XXX::hardware_config_t t_optical_scaner_mcp41; + ZGPIO::OutputGpioCfg_t t_laster_enable_io; + ZGPIO::OutputGpioCfg_t t_amp_sw_io; + + MCP41XXX::hardware_config_t f_optical_amp_mcp41; + MCP41XXX::hardware_config_t f_optical_scaner_mcp41; + ZADC::adc_config_t f_laster_adc; + ZADC::adc_config_t f_result_adc; + ZGPIO::OutputGpioCfg_t f_opt_laster_enable_io; + ZGPIO::OutputGpioCfg_t f_amp_sw_io; + + ZGPIO::OutputGpioCfg_t channel_select_io_red_led3; // 引脚命名来源于巴迪泰原理图 + ZGPIO::OutputGpioCfg_t channel_select_io_blue_led1; // 引脚命名来源于巴迪泰原理图 + ZGPIO::OutputGpioCfg_t channel_select_io_green_led1; // 引脚命名来源于巴迪泰原理图 + + StepMotorCtrlModule* motor; + + } hardware_config_t; + + typedef struct { + int32_t module_raw_sector_size; + int32_t module_raw_sector_num; + + int32_t scan_type; + int32_t scan_start_pos; + int32_t scan_direction; + int32_t scan_step_interval; + int32_t scan_pointnum; + int32_t channel_select_num; + int32_t laster_gain; + int32_t scan_gain; + + /******************************************************************************* + * TRF_DELAY_TIME * + *******************************************************************************/ + int32_t trf_uvled_on_duration_us; + int32_t trf_uvled_off_duration_us; + int32_t trf_scan_delay_us; + int32_t trf_scan_duration_us; + + int32_t scan_gain_adjust_suggestion; + int32_t adc_result_overflow; + int32_t laster_intensity; + + } reg_table_t; + + private: + int32_t m_id = 0; + hardware_config_t m_hardwarecfg; + reg_table_t m_reg = {0}; + + MCP41XXX m_t_optical_amp_mcp41_ohm; + MCP41XXX m_t_optical_scaner_mcp41_ohm; + MCP41XXX m_f_optical_amp_mcp41_ohm; + MCP41XXX m_f_optical_scaner_mcp41_ohm; + // amp_negative_5v + ZGPIO m_amp_negative_5v_gpio; + + ZGPIO m_t_laster_enable_io; + // ZGPIO m_t_opt_scan_amp_enable_io; + ZGPIO m_f_opt_laster_enable_io; + // ZGPIO m_f_opt_scan_amp_enable_io; + ZGPIO m_channel_select_io_red_led3; + ZGPIO m_channel_select_io_blue_led1; + ZGPIO m_channel_select_io_green_led1; + + ZGPIO m_t_amp_sw_io; + ZGPIO m_f_amp_sw_io; + + ZADC t_laster_adc; + ZADC t_result_adc; + ZADC f_laster_adc; + ZADC f_result_adc; + + StepMotorCtrlModule* motor = nullptr; + + ZThread m_thread; + + int16_t adc_capture_buf[4096] = {0}; + int32_t adc_capture_point_num = 0; + + int32_t f_adc_cache[20]; + + public: + virtual ~OpticalModule(){}; + + void initialize(int32_t moduleid, hardware_config_t* hardwarecfg); + + public: + virtual int32_t getid(int32_t* id) { + *id = m_id; + return 0; + } + + virtual int32_t module_ping() { return 0; } + + virtual int32_t module_stop() override; + virtual int32_t module_break(); // TODO + virtual int32_t module_start(); // TODO + + virtual int32_t module_factory_reset(); // TODO + virtual int32_t module_flush_cfg(); // + virtual int32_t module_active_cfg() override; + virtual int32_t module_get_status(int32_t* status) override; + + virtual int32_t module_read_raw(int32_t index, uint8_t* data, int32_t* len); // TODO + + virtual int32_t a8000_optical_open_laser(int32_t type) override; + virtual int32_t a8000_optical_close_laser(int32_t type) override; + virtual int32_t a8000_optical_set_laster_gain(int32_t type, int32_t gain) override; + virtual int32_t a8000_optical_set_scan_amp_gain(int32_t type, int32_t gain) override; + virtual int32_t a8000_optical_read_scanner_adc_val(int32_t type, int32_t* adcval) override; + virtual int32_t a8000_optical_read_laster_adc_val(int32_t type, int32_t* adcval) override; + virtual int32_t a8000_optical_module_power_ctrl(int32_t state) override; + + private: + int32_t start_t_optical_scan(); + int32_t start_f_optical_scan(); + + int32_t f_read_one_point(int32_t lastergain, int32_t ampgain, int32_t& adcval); + int32_t t_read_one_point(int32_t lastergain, int32_t ampgain, int32_t& adcval); + + int32_t adjust_detector_gain(); + + int32_t dumpresult(); + +#if 0 + void t_open_laser(); + void t_close_laser(); + void t_set_laster_gain(int32_t gain); + void t_set_scan_amp_gain(int32_t gain); + + void f_open_laser(); + void f_close_laser(); + void f_set_laster_gain(int32_t gain); + void f_set_scan_amp_gain(int32_t gain); + void select_f_channel(); + +#endif + + int32_t move_to_block(int32_t pos); + virtual int32_t module_xxx_reg(int32_t param_id, bool read, int32_t& val) override; + void opt_module_power_ctrl(bool on); + + int32_t select_f_channel(int32_t channel); + virtual int32_t do_action(int32_t action); + + int32_t _open_t_amp_n5v(); + int32_t _close_t_amp_n5v(); + + int32_t motor_prepare(); + int32_t motor_release(); + + int32_t f_detector_gain_to_raw_gain(float gain); + int32_t t_detector_gain_to_raw_gain(float gain); + + float f_detector_raw_gain_to_gain(int32_t gain); + float t_detector_raw_gain_to_gain(int32_t gain); + + int32_t raw_get_sector_size(int32_t* val); + int32_t raw_get_sector_num(int32_t* val); +}; +} // namespace iflytop diff --git a/usrc/subboards/subboard90_optical_module/subboard90_optical_module.cpp b/usrc/subboards/subboard90_optical_module/subboard90_optical_module.cpp index 0fd7679..7f093b5 100644 --- a/usrc/subboards/subboard90_optical_module/subboard90_optical_module.cpp +++ b/usrc/subboards/subboard90_optical_module/subboard90_optical_module.cpp @@ -2,6 +2,7 @@ extern "C" { #include "subboard90_optical_module_board.h" } +#include "optical_module.hpp" #include "pri_board.h" #include "public_service/instance_init.hpp" #include "public_service/public_service.hpp" @@ -16,7 +17,6 @@ extern "C" { using namespace iflytop; - /*********************************************************************************************************************** * IMPL * ***********************************************************************************************************************/ @@ -24,12 +24,124 @@ void Subboard90OpticalModule::initialize() { IO_INIT(); GService::inst()->getZCanProtocolParser()->registerModule(this); #if 1 + static StepMotorCtrlModule* opt_scan_motor = nullptr; /*********************************************************************************************************************** * ID1 * ***********************************************************************************************************************/ { TMC5130_MOTOR_INITER(1, 1); } { TMC5130_MOTOR_INITER(2, 2); } + opt_scan_motor = dynamic_cast(GService::inst()->getZCanProtocolParser()->getModule(1)); + ZASSERT(opt_scan_motor); + { + OpticalModule::hardware_config_t cfg = { + .amp_negative_5v_gpio = + { + .pin = PC5, + .mode = ZGPIO::kMode_nopull, + .mirror = true, + .initLevel = false, + .log_when_setstate = true, + }, + /******************************************************************************* + * T光学 * + *******************************************************************************/ + .t_laster_adc = + { + .hadc1 = &hadc1, + .channel = ADC_CHANNEL_13, + .samplingTime = ADC_SAMPLETIME_480CYCLES, + }, + .t_result_adc = + { + .hadc1 = &hadc1, + .channel = ADC_CHANNEL_12, + .samplingTime = ADC_SAMPLETIME_56CYCLES, // 注意这里必须要使用112周期采样时间,否则optical_module中的单次采样时间会大于1ms + }, + .t_optical_amp_mcp41 = + { + .spihandler = &hspi2, + .ncspin = PE15, + .mark = "t_optical_amp_mcp41", + }, + .t_optical_scaner_mcp41 = + { + .spihandler = &hspi2, + .ncspin = PE14, + .mark = "t_optical_scaner_mcp41", + }, + .t_laster_enable_io = + { + .pin = PB0, + .mode = ZGPIO::kMode_nopull, + .mirror = false, + .initLevel = false, + .log_when_setstate = true, + + }, + .t_amp_sw_io = + { + .pin = PinNull, + }, + /******************************************************************************* + * F光学 * + *******************************************************************************/ + .f_optical_amp_mcp41 = + { + .spihandler = &hspi3, + .ncspin = PC13, + .mark = "f_optical_amp_mcp41", + }, + .f_optical_scaner_mcp41 = + { + .spihandler = &hspi2, + .ncspin = PE13, + .mark = "f_optical_scaner_mcp41", + }, + // 激光亮度反馈ADC PC0 + .f_laster_adc = + { + .hadc1 = &hadc1, + .channel = ADC_CHANNEL_10, + .samplingTime = ADC_SAMPLETIME_480CYCLES, + }, + .f_result_adc = + { + .hadc1 = &hadc1, + .channel = ADC_CHANNEL_11, + .samplingTime = ADC_SAMPLETIME_480CYCLES, + }, + .f_opt_laster_enable_io = + { + .pin = PA8, + .mode = ZGPIO::kMode_nopull, + .mirror = false, + .initLevel = false, + .log_when_setstate = true, + }, + .f_amp_sw_io = + { + .pin = PD6, + .mode = ZGPIO::kMode_nopull, + .mirror = true, + .initLevel = false, + .log_when_setstate = true, + }, + + /******************************************************************************* + * F_CHANNEL_SELECT * + *******************************************************************************/ + .channel_select_io_red_led3 = {.pin = PB11, .mode = ZGPIO::kMode_nopull, .mirror = false, .initLevel = false, .log_when_setstate = true}, + .channel_select_io_blue_led1 = {.pin = PA15, .mode = ZGPIO::kMode_nopull, .mirror = false, .initLevel = false, .log_when_setstate = true}, + .channel_select_io_green_led1 = {.pin = PB10, .mode = ZGPIO::kMode_nopull, .mirror = false, .initLevel = false, .log_when_setstate = true}, + /******************************************************************************* + * 电机 * + *******************************************************************************/ + .motor = opt_scan_motor, + }; + static OpticalModule opticalModule; + GService::inst()->getZCanProtocolParser()->registerModule(&opticalModule); + } #endif } diff --git a/usrc/subboards/subboard90_optical_module/subboard90_optical_module_board.c b/usrc/subboards/subboard90_optical_module/subboard90_optical_module_board.c index 66d8cde..c720087 100644 --- a/usrc/subboards/subboard90_optical_module/subboard90_optical_module_board.c +++ b/usrc/subboards/subboard90_optical_module/subboard90_optical_module_board.c @@ -62,6 +62,42 @@ static void MX_SPI2_Init(void) { hspi2_enable = true; } +void MX_SPI3_Init(void) { + GPIO_InitTypeDef GPIO_InitStruct = {0}; + + __HAL_RCC_SPI3_CLK_ENABLE(); + __HAL_RCC_GPIOC_CLK_ENABLE(); + + hspi3.Instance = SPI3; + hspi3.Init.Mode = SPI_MODE_MASTER; + hspi3.Init.Direction = SPI_DIRECTION_2LINES; + hspi3.Init.DataSize = SPI_DATASIZE_8BIT; + hspi3.Init.CLKPolarity = SPI_POLARITY_LOW; + hspi3.Init.CLKPhase = SPI_PHASE_1EDGE; + hspi3.Init.NSS = SPI_NSS_SOFT; + hspi3.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_64; + hspi3.Init.FirstBit = SPI_FIRSTBIT_MSB; + hspi3.Init.TIMode = SPI_TIMODE_DISABLE; + hspi3.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; + hspi3.Init.CRCPolynomial = 10; + if (HAL_SPI_Init(&hspi3) != HAL_OK) { + Error_Handler(); + } + + /**SPI3 GPIO Configuration + PC10 ------> SPI3_SCK + PC11 ------> SPI3_MISO + PC12 ------> SPI3_MOSI + */ + GPIO_InitStruct.Pin = GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + GPIO_InitStruct.Alternate = GPIO_AF6_SPI3; + HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + hspi3_enable = true; +} + /** * @brief 板夹仓初始化 */ @@ -70,4 +106,5 @@ void subboard90_optical_module_board_init() { common_hardware_init(); MX_SPI1_Init(); MX_SPI2_Init(); + MX_SPI3_Init(); }