Skip to content
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

Jump to bottom

Improve flow quality #90

Open
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 all commits
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
4 changes: 2 additions & 2 deletions makefiles/baremetal/toolchain_gnu-arm-eabi.mk
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ OBJDUMP = $(CROSSDEV)objdump

# Check if the right version of the toolchain is available
#
CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.4 4.9.3
CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.2 4.8.4 4.9.3
CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion)

ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED)))
Expand Down Expand Up @@ -276,4 +276,4 @@ define SYM_TO_BIN
@$(ECHO) "BIN: $2"
@$(MKDIR) -p $(dir $2)
$(Q) $(OBJCOPY) -O binary $1 $2
endef
endef
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
11 changes: 10 additions & 1 deletion src/include/settings.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,15 @@
#define ONBOARD_PARAM_NAME_LENGTH 15
#define BOTTOM_FLOW_IMAGE_HEIGHT 64
#define BOTTOM_FLOW_IMAGE_WIDTH 64
#define BOTTOM_FLOW_SEARCH_WINDOW_SIZE 4
#define BOTTOM_FLOW_SEARCH_WINDOW_SIZE 8

/* Translate normalized pixels to have their mean around 127. */
#define WHITENING_MEAN 127.0f

/* Rescale normalized pixels so 32 is one standard deviation,
* giving a range of four sigmas.
*/
#define WHITENING_STDDEV 32.0f

/******************************************************************
* ALL TYPE DEFINITIONS
Expand Down Expand Up @@ -117,6 +125,7 @@ enum global_param_id_t
PARAM_MAX_FLOW_PIXEL,
PARAM_IMAGE_LOW_LIGHT,
PARAM_IMAGE_ROW_NOISE_CORR,
PARAM_IMAGE_WHITEN,
PARAM_IMAGE_TEST_PATTERN,
PARAM_GYRO_SENSITIVITY_DPS,
PARAM_GYRO_COMPENSATION_THRESHOLD,
Expand Down
114 changes: 62 additions & 52 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,55 @@ 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]);

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

/**
* @brief Whiten image by mean shift and scaling by standard deviation
*
* @param source Source image
* @param dest Destination image (may be the same as source)
* @param image_size Size of source and dest images. Must be <= 256.
*/
void whitened_image(uint8_t *source, uint8_t *dest, uint16_t image_size) {
uint32_t sum = 0;
for (uint16_t pixel = 0; pixel < image_size; pixel++)
sum += source[pixel];
uint8_t mean = sum / image_size;
uint32_t rss = 0;
for (uint16_t pixel = 0; pixel < image_size; pixel++) {
int16_t v = source[pixel] - mean;
rss += v*v;
}
else
{
for (uint16_t pixel = 0; pixel < image_size; pixel++)
(*current_image)[pixel] = (uint8_t)(dcmi_image_buffer_8bit_3[pixel]);
float stddev = sqrtf(rss/(image_size - 1)) / WHITENING_STDDEV;

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

Expand All @@ -304,16 +324,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 +337,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
68 changes: 33 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

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

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

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 @@ -427,6 +428,11 @@ int main(void)
/* copy recent image to faster ram */
dma_copy_image_buffers(&current_image, &previous_image, image_size, 1);

if (FLOAT_AS_BOOL(global_data.param[PARAM_IMAGE_WHITEN])) {
/* whiten image for improved stability */
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 +505,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 +522,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 +575,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
Loading