Skip to content

Improve flow quality #90

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
lericson wants to merge 7 commits into
base: master
Choose a base branch
from
Open

Improve flow quality #90

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
6feb336
Improve flow quality
lericson Jun 15, 2017
87bee29
Use floats for whitening
lericson Jun 15, 2017
11a9616
Add 4.8.2 as allowed compiler
lericson Jun 15, 2017
1263206
Disable automatic gain control, set gain to 1
lericson Jun 19, 2017
30eb3d5
Add whitening parameter
lericson Jun 19, 2017
f0771ee
Optimize types of whitening variables
lericson Jun 19, 2017
dd01d8d
Increase manual gain, desired brightness
lericson Jun 19, 2017
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
5 changes: 5 additions & 0 deletions src/drivers/boards/px4flow-v1/px4flow_init.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,11 @@
/****************************************************************************
* Public Functions
****************************************************************************/
static int errn;
int *__errno _PARAMS ((void))
{
return &errn;
}

/****************************************************************************
* Name: board_initialize
Expand Down
5 changes: 5 additions & 0 deletions src/include/dcmi.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,11 @@ void dma_copy_image_buffers(uint8_t ** current_image, uint8_t ** previous_image,
*/
void send_calibration_image(uint8_t ** image_buffer_fast_1, uint8_t ** image_buffer_fast_2);

/**
* @brief Whiten image assuming pixels are iid Gaussian
*/
void whitened_image(uint8_t *source, uint8_t *dest, uint16_t image_size);

/**
* @brief Initialize DCMI DMA and enable image capturing
*/
Expand Down
2 changes: 2 additions & 0 deletions src/include/mt9v034.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -103,6 +103,8 @@

#define MTV_HDR_ENABLE_REG 0x0F
#define MTV_ADC_RES_CTRL_REG 0x1C
#define MTV_BLACK_LEVEL_CTRL_REG 0x47
#define MTV_BLACK_LEVEL_CALIBRATION_REG 0x48
#define MTV_ROW_NOISE_CORR_CTRL_REG 0x70
#define MTV_TEST_PATTERN_REG 0x7F
#define MTV_TILED_DIGITAL_GAIN_REG 0x80
Expand Down
108 changes: 55 additions & 53 deletions src/modules/flow/dcmi.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -251,35 +251,47 @@ void dma_copy_image_buffers(uint8_t ** current_image, uint8_t ** previous_image,
*current_image = *previous_image;
*previous_image = tmp_image;

TODO(NB dma_copy_image_buffers is calling uavcan_run());

/* wait for new image if needed */
while(image_counter < image_step) {
PROBE_1(false);
uavcan_run();
PROBE_1(true);
}
while(image_counter < image_step);

image_counter = 0;

/* time between images */
time_between_images = time_between_next_images;

uint8_t *source = NULL;

/* copy image */
if (dcmi_image_buffer_unused == 1)
{
for (uint16_t pixel = 0; pixel < image_size; pixel++)
(*current_image)[pixel] = (uint8_t)(dcmi_image_buffer_8bit_1[pixel]);
if (dcmi_image_buffer_unused == 1) {
source = dcmi_image_buffer_8bit_1;
} else if (dcmi_image_buffer_unused == 2) {
source = dcmi_image_buffer_8bit_2;
} else {
source = dcmi_image_buffer_8bit_3;
}
else if (dcmi_image_buffer_unused == 2)
{
for (uint16_t pixel = 0; pixel < image_size; pixel++)
(*current_image)[pixel] = (uint8_t)(dcmi_image_buffer_8bit_2[pixel]);
}
else
{
for (uint16_t pixel = 0; pixel < image_size; pixel++)
(*current_image)[pixel] = (uint8_t)(dcmi_image_buffer_8bit_3[pixel]);

for (uint16_t pixel = 0; pixel < image_size; pixel++)
(*current_image)[pixel] = source[pixel];
}

void whitened_image(uint8_t *source, uint8_t *dest, uint16_t image_size) {
Copy link
Member

@jgoppert jgoppert Jun 17, 2017 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So I'm wondering how this compares to the auto gain and brightness controls within the camera firmware. Both pieces of software have the same goal, so you might just be eating more cpu here. You might want to check. But if your method is better than the whitening methods coded in the camera firmware, maybe you want to turn auto gain etc. off.

Copy link
Member

@jgoppert jgoppert Jun 17, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

image

http://www.onsemi.com/pub/Collateral/MT9V034-D.PDF

double sum = 0.0;
Copy link
Member

@LorenzMeier LorenzMeier Jun 15, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

double is very expensive on embedded hardware and you don't need this here (unless you need a resolution better than about 0.0000001). Please replace with float.

Copy link
Author

@lericson lericson Jun 15, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good point, will do.

for (uint16_t pixel = 0; pixel < image_size; pixel++)
sum += source[pixel];
double mean = sum / image_size;
double rss = 0.0;
for (uint16_t pixel = 0; pixel < image_size; pixel++)
rss += pow(source[pixel] - mean, 2);
Copy link
Member

@LorenzMeier LorenzMeier Jun 15, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This executes a double instruction and pow instead of square. Please replace with: (source[pixel] - mean) * (source[pixel] - mean). This will be orders of magnitude faster.

double stddev = sqrt(rss/(image_size - 1));
Copy link
Member

@LorenzMeier LorenzMeier Jun 15, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please use sqrtf() to keep this in float and use float variables.

dest[0] = stddev;

for (uint16_t pixel = 0; pixel < image_size; pixel++) {
double v = 127.0 + 32.0*(source[pixel] - mean)/stddev;
if (v < 0.0)
v = 0;
if (v > 255.0)
v = 255;
dest[pixel] = (uint8_t)v;
}
}

Expand All @@ -304,16 +316,11 @@ void send_calibration_image(uint8_t ** image_buffer_fast_1, uint8_t ** image_buf
100);

uint16_t frame = 0;
uint8_t image = 0;
uint8_t frame_buffer[MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN];

for (int i = 0; i < FULL_IMAGE_SIZE * 4; i++)
{

if (i % FULL_IMAGE_SIZE == 0 && i != 0)
{
image++;
}
uint8_t image = i / FULL_IMAGE_SIZE;

if (i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN == 0 && i != 0)
{
Expand All @@ -322,44 +329,39 @@ void send_calibration_image(uint8_t ** image_buffer_fast_1, uint8_t ** image_buf
delay(2);
}

if (image == 0 )
{
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = (uint8_t)(*image_buffer_fast_1)[i % FULL_IMAGE_SIZE];
}
else if (image == 1 )
{
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = (uint8_t)(*image_buffer_fast_2)[i % FULL_IMAGE_SIZE];
}
else if (image == 2)
{
// The whole camera capture is stored in five parts: two buffers given
// as arguments, and three internal DMA buffers.
uint8_t *source;

if (image == 0)
source = *image_buffer_fast_1;
else if (image == 1)
source = *image_buffer_fast_2;
else if (image == 2) {
if (calibration_unused == 1)
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_1[i % FULL_IMAGE_SIZE];
source = dcmi_image_buffer_8bit_1;
else if (calibration_unused == 2)
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_2[i % FULL_IMAGE_SIZE];
source = dcmi_image_buffer_8bit_2;
else
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_3[i % FULL_IMAGE_SIZE];
}
else
{
if (calibration_used)
{
source = dcmi_image_buffer_8bit_3;
} else {
if (calibration_used) {
if (calibration_mem0 == 1)
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_1[i % FULL_IMAGE_SIZE];
source = dcmi_image_buffer_8bit_1;
else if (calibration_mem0 == 2)
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_2[i % FULL_IMAGE_SIZE];
source = dcmi_image_buffer_8bit_2;
else
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_3[i % FULL_IMAGE_SIZE];
}
else
{
source = dcmi_image_buffer_8bit_3;
} else {
if (calibration_mem1 == 1)
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_1[i % FULL_IMAGE_SIZE];
source = dcmi_image_buffer_8bit_1;
else if (calibration_mem1 == 2)
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_2[i % FULL_IMAGE_SIZE];
source = dcmi_image_buffer_8bit_2;
else
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_3[i % FULL_IMAGE_SIZE];
source = dcmi_image_buffer_8bit_3;
}
}
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = source[i % FULL_IMAGE_SIZE];
}

mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, frame_buffer);
Expand Down
64 changes: 29 additions & 35 deletions src/modules/flow/main.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -108,10 +108,10 @@ volatile uint32_t boot_time10_us = 0;
#define TIMER_LPOS 8
#define MS_TIMER_COUNT 100 /* steps in 10 microseconds ticks */
#define LED_TIMER_COUNT 500 /* steps in milliseconds ticks */
#define SONAR_TIMER_COUNT 100 /* steps in milliseconds ticks */
#define SONAR_TIMER_COUNT 9999/* steps in milliseconds ticks */
Copy link

@jlecoeur jlecoeur Jun 19, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Any particular reason you modified this? This gave very slow sonar update when I tested.

Copy link
Author

@lericson lericson Jun 19, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Whops, not intended to be included. I did this since we don't use this data at all, as the sonar is actually really bad.

Copy link
Contributor

@ChristophTobler ChristophTobler Aug 14, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you revert?

#define SYSTEM_STATE_COUNT 1000/* steps in milliseconds ticks */
#define PARAMS_COUNT 100 /* steps in milliseconds ticks */
#define LPOS_TIMER_COUNT 100 /* steps in milliseconds ticks */
#define LPOS_TIMER_COUNT 100 /* steps in milliseconds ticks */

static volatile unsigned timer[NTIMERS];
static volatile unsigned timer_ms = MS_TIMER_COUNT;
Expand Down Expand Up @@ -251,14 +251,14 @@ int main(void)
LEDOff(LED_ACT);
LEDOff(LED_COM);
LEDOff(LED_ERR);
board_led_rgb(255,255,255, 1);
board_led_rgb( 0, 0,255, 0);
board_led_rgb( 0, 0, 0, 0);
board_led_rgb(255, 0, 0, 1);
board_led_rgb(255, 0, 0, 2);
board_led_rgb(255, 0, 0, 3);
board_led_rgb( 0,255, 0, 3);
board_led_rgb( 0, 0,255, 4);
board_led_rgb(255,255,255, 1);
board_led_rgb( 0, 0,255, 0);
board_led_rgb( 0, 0, 0, 0);
board_led_rgb(255, 0, 0, 1);
board_led_rgb(255, 0, 0, 2);
board_led_rgb(255, 0, 0, 3);
board_led_rgb( 0,255, 0, 3);
board_led_rgb( 0, 0,255, 4);

/* enable FPU on Cortex-M4F core */
SCB_CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); /* set CP10 Full Access and set CP11 Full Access */
Expand Down Expand Up @@ -300,8 +300,8 @@ int main(void)
/* usart config*/
usart_init();

/* i2c config*/
i2c_init();
/* i2c config*/
i2c_init();

/* sonar config*/
float sonar_distance_filtered = 0.0f; // distance in meter
Expand Down Expand Up @@ -347,9 +347,10 @@ int main(void)
/* main loop */
while (1)
{
PROBE_1(false);
uavcan_run();
PROBE_1(true);
PROBE_1(false);
uavcan_run();
PROBE_1(true);

/* reset flow buffers if needed */
if(buffer_reset_needed)
{
Expand Down Expand Up @@ -426,6 +427,7 @@ int main(void)
{
/* copy recent image to faster ram */
dma_copy_image_buffers(&current_image, &previous_image, image_size, 1);
whitened_image(current_image, current_image, image_size);

/* compute optical flow */
qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y);
Expand Down Expand Up @@ -499,20 +501,12 @@ int main(void)

float ground_distance = 0.0f;

ground_distance = FLOAT_AS_BOOL(global_data.param[PARAM_SONAR_FILTERED]) ? sonar_distance_filtered : sonar_distance_raw;

if(FLOAT_AS_BOOL(global_data.param[PARAM_SONAR_FILTERED]))
{
ground_distance = sonar_distance_filtered;
}
else
{
ground_distance = sonar_distance_raw;
}

uavcan_define_export(i2c_data, legacy_12c_data_t, ccm);
uavcan_define_export(range_data, range_data_t, ccm);
uavcan_define_export(i2c_data, legacy_12c_data_t, ccm);
uavcan_define_export(range_data, range_data_t, ccm);
uavcan_timestamp_export(i2c_data);
uavcan_assign(range_data.time_stamp_utc, i2c_data.time_stamp_utc);
uavcan_assign(range_data.time_stamp_utc, i2c_data.time_stamp_utc);
//update I2C transmitbuffer
if(valid_frame_count>0)
{
Expand All @@ -524,15 +518,15 @@ int main(void)
update_TX_buffer(pixel_flow_x, pixel_flow_y, 0.0f, 0.0f, qual,
ground_distance, x_rate, y_rate, z_rate, gyro_temp, uavcan_use_export(i2c_data));
}
PROBE_2(false);
uavcan_publish(range, 40, range_data);
PROBE_2(true);
PROBE_2(false);
uavcan_publish(range, 40, range_data);
PROBE_2(true);

PROBE_3(false);
uavcan_publish(flow, 40, i2c_data);
PROBE_3(true);
PROBE_3(false);
uavcan_publish(flow, 40, i2c_data);
PROBE_3(true);

//serial mavlink + usb mavlink output throttled
//serial mavlink + usb mavlink output throttled
if (counter % (uint32_t)global_data.param[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] == 0)//throttling factor
{

Expand Down Expand Up @@ -577,7 +571,7 @@ int main(void)
lpos.x += ground_distance*accumulated_flow_x;
lpos.y += ground_distance*accumulated_flow_y;
lpos.z = -ground_distance;
/* velocity not directly measured and not important for testing */
/* velocity not directly measured and not important for testing */
lpos.vx = 0;
lpos.vy = 0;
lpos.vz = 0;
Expand Down
9 changes: 8 additions & 1 deletion src/modules/flow/mt9v034.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,7 +125,9 @@ void mt9v034_context_configuration(void)
uint16_t aec_low_pass = 0x01; // default VALID RANGE: 0-2
uint16_t agc_update_freq = 0x02; // default Number of frames to skip between changes in AGC VALID RANGE: 0-15
uint16_t agc_low_pass = 0x02; // default VALID RANGE: 0-2

uint16_t analog_gain_control = 16;
uint16_t black_level_control = 0x01;
uint16_t black_level_calibration = 0x50;

if (FLOAT_AS_BOOL(global_data.param[PARAM_IMAGE_LOW_LIGHT]))
{
Expand Down Expand Up @@ -195,6 +197,7 @@ void mt9v034_context_configuration(void)
mt9v034_WriteReg16(MTV_COARSE_SW_2_REG_A, coarse_sw2);
mt9v034_WriteReg16(MTV_COARSE_SW_CTRL_REG_A, shutter_width_ctrl);
mt9v034_WriteReg16(MTV_COARSE_SW_TOTAL_REG_A, total_shutter_width);
mt9v034_WriteReg16(MTV_ANALOG_GAIN_CTRL_REG_A, analog_gain_control);


/* Context B */
Expand All @@ -209,6 +212,7 @@ void mt9v034_context_configuration(void)
mt9v034_WriteReg16(MTV_COARSE_SW_2_REG_B, coarse_sw2);
mt9v034_WriteReg16(MTV_COARSE_SW_CTRL_REG_B, shutter_width_ctrl);
mt9v034_WriteReg16(MTV_COARSE_SW_TOTAL_REG_B, total_shutter_width);
mt9v034_WriteReg16(MTV_ANALOG_GAIN_CTRL_REG_B, analog_gain_control);

/* General Settings */
mt9v034_WriteReg16(MTV_ROW_NOISE_CORR_CTRL_REG, row_noise_correction);
Expand All @@ -228,6 +232,9 @@ void mt9v034_context_configuration(void)
mt9v034_WriteReg16(MTV_AGC_UPDATE_REG,agc_update_freq);
mt9v034_WriteReg16(MTV_AGC_LOWPASS_REG,agc_low_pass);

mt9v034_WriteReg16(MTV_BLACK_LEVEL_CTRL_REG,black_level_control);
mt9v034_WriteReg16(MTV_BLACK_LEVEL_CALIBRATION_REG,black_level_calibration);

/* Reset */
mt9v034_WriteReg16(MTV_SOFT_RESET_REG, 0x01);
}
Expand Down