Skip to content

change sonar to laser tfmini-s #131

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
w407022008 wants to merge 3 commits into
base: master
Choose a base branch
from
Open

change sonar to laser tfmini-s #131

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
b45ab4a
change sonar to laser tfmini-s
w407022008 Feb 20, 2021
481924f
Delete CMakeLists.txt.user
w407022008 Feb 23, 2021
69a87f9
Update settings.c
w407022008 Feb 23, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions src/modules/flow/i2c.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -298,7 +298,7 @@ void update_TX_buffer(float pixel_flow_x, float pixel_flow_y,

/* calculate focal_length in pixel */
const float focal_length_px = ((global_data.param[PARAM_FOCAL_LENGTH_MM])
/ (4.0f * 6.0f) * 1000.0f); //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled
/ (4.0f * 6.0f) * 1000.0f); //original focal lenght: 16mm pixelsize: 6um, binning 4 enabled

// reset if readout has been performed
if (stop_accumulation == 1) {
Expand Down Expand Up @@ -358,7 +358,7 @@ void update_TX_buffer(float pixel_flow_x, float pixel_flow_y,
// are used to passs the data to uavcan.

uavcan_export(&pd->frame, &f, I2C_FRAME_SIZE);
uavcan_export(&pd->integral_frame, &f_integral, I2C_INTEGRAL_FRAME_SIZE);
uavcan_export(&pd->integral_frame, &f_integral, I2C_INTEGRAL_FRAME_SIZE); // by default: px4 will recieve integral_frame

// fill I2C transmitbuffer1 with frame1 values
memcpy(&(txDataFrame1[notpublishedIndexFrame1]),
Expand Down
92 changes: 48 additions & 44 deletions src/modules/flow/sonar.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,14 +52,14 @@
#include "sonar.h"
#include "sonar_mode_filter.h"

#define SONAR_SCALE 1000.0f
#define SONAR_MIN 0.12f /** 0.12m sonar minimum distance */
#define SONAR_MAX 3.5f /** 3.50m sonar maximum distance */
#define SONAR_SCALE 1000.0f /** m -> mm */
#define SONAR_MIN 0.3f /** 0.3m laser ; 0.12m sonar minimum distance */
#define SONAR_MAX 12.0f /** 12m laser ; 3.50m sonar maximum distance */

#define atoi(nptr) strtol((nptr), NULL, 10)
extern uint32_t get_boot_time_us(void);

static char data_buffer[5]; // array for collecting decoded data
static char data_buffer[9]; // array for collecting decoded data

static volatile uint32_t last_measure_time = 0;
static volatile uint32_t measure_time = 0;
Expand Down Expand Up @@ -102,56 +102,60 @@ void UART4_IRQHandler(void)
/* Read one byte from the receive data register */
uint8_t data = (USART_ReceiveData(UART4));

if (data == 'R')
if (data == 0x59)
{
/* this is the first char (start of transmission) */
data_counter = 0;
data_valid = 1;

/* this is the first two chars (start of transmission) */
if (data_valid==0 && data_counter < 2)
{
data_buffer[data_counter]=0x59;
data_counter++;
if (data_counter==2)
{
data_valid = 1;
} else {
data_valid = 0;
}
}
/* set sonar pin 4 to low -> we want triggered mode */
GPIO_ResetBits(GPIOE, GPIO_Pin_8);
}
else if (0x30 <= data && data <= 0x39)
{
// GPIO_ResetBits(GPIOE, GPIO_Pin_8);
} else {

if (data_valid)
{
data_buffer[data_counter] = data;
data_counter++;
} else {
data_counter = 0;
}
}
else if (data == 0x0D)
{
if (data_valid && data_counter == 4)
{
data_buffer[4] = 0;
int temp = atoi(data_buffer);

/* use real-world maximum ranges to cut off pure noise */
if ((temp > SONAR_MIN*SONAR_SCALE) && (temp < SONAR_MAX*SONAR_SCALE))
if (data_valid && data_counter == 9)
{
if (data_buffer[8] == ((data_buffer[0] + data_buffer[1] + data_buffer[2] + data_buffer[3] + data_buffer[4] + data_buffer[5] + data_buffer[6] + data_buffer[7]) & 0xff))
{
/* it is in normal sensor range, take it */
last_measure_time = measure_time;
measure_time = get_boot_time_us();
sonar_measure_time_interrupt = measure_time;
dt = ((float)(measure_time - last_measure_time)) / 1000000.0f;

valid_data = temp;
// the mode filter turned out to be more problematic
// than using the raw value of the sonar
//insert_sonar_value_and_get_mode_value(valid_data / SONAR_SCALE);
sonar_mode = valid_data / SONAR_SCALE;
new_value = 1;
sonar_valid = true;
} else {
sonar_valid = false;
int temp = data_buffer[2] | (data_buffer[3] << 8);
/* use real-world maximum ranges to cut off pure noise */
if ((temp > SONAR_MIN*SONAR_SCALE) && (temp < SONAR_MAX*SONAR_SCALE))
{
/* it is in normal sensor range, take it */
last_measure_time = measure_time;
measure_time = get_boot_time_us();
sonar_measure_time_interrupt = measure_time;
dt = ((float)(measure_time - last_measure_time)) / 1000000.0f;

valid_data = temp;
// the mode filter turned out to be more problematic
// than using the raw value of the sonar
//insert_sonar_value_and_get_mode_value(valid_data / SONAR_SCALE);
sonar_mode = valid_data / SONAR_SCALE;
new_value = 1;
sonar_valid = true;
} else {
sonar_valid = false;
}
}
data_valid = 0;
data_counter = 0;
}

data_valid = 0;
}
else
{
data_valid = 0;
}
}
}
Expand Down Expand Up @@ -254,7 +258,7 @@ void sonar_config(void)

USART_InitTypeDef USART_InitStructure;

USART_InitStructure.USART_BaudRate = 9600;
USART_InitStructure.USART_BaudRate = 115200; /** 9600 sonar */
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
Expand Down