project_boditech_vidas_a8000_v3
/
a8000_subboard
2
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

update

master
zhaohe 1 year ago
parent
d8016d71c6
commit
6cbaa88545
8 changed files with 1037 additions and 3 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      a8000_protocol
  2. 2
      sdk
  3. 1
      usrc/public_service/gins.c
  4. 1
      usrc/public_service/gins.h
  5. 698
      usrc/subboards/subboard90_optical_module/optical_module.cpp
  6. 185
      usrc/subboards/subboard90_optical_module/optical_module.hpp
  7. 114
      usrc/subboards/subboard90_optical_module/subboard90_optical_module.cpp
  8. 37
      usrc/subboards/subboard90_optical_module/subboard90_optical_module_board.c

2
a8000_protocol

@ -1 +1 @@
Subproject commit a85673b9c1b9a54c0e91a3e3f4299106a749e56d
Subproject commit c3b66e0656239c61edc573842b420863a5cd3832

2
sdk

@ -1 +1 @@
Subproject commit 9aee8f2a2c26251c921e81bca81abc3a3a2f6927
Subproject commit 49aa1c4693983ad6031f71b0c0b785c6721d9868

1
usrc/public_service/gins.c
View File

@ -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 *

1
usrc/public_service/gins.h
View File

@ -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 *

698
usrc/subboards/subboard90_optical_module/optical_module.cpp
View File

@ -0,0 +1,698 @@
#include "optical_module.hpp"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
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;
}

185
usrc/subboards/subboard90_optical_module/optical_module.hpp
View File

@ -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

114
usrc/subboards/subboard90_optical_module/subboard90_optical_module.cpp
View File

@ -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<StepMotorCtrlModule*>(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
}

37
usrc/subboards/subboard90_optical_module/subboard90_optical_module_board.c
View File

@ -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();
}
Write Preview
Loading…
Cancel
Save