/**
* @brief Indicate error condition with LED3
* @param None
* @retval None
*/
void IndicateError(void)
{
/* Switch LED3 on and off in case of error */
LEDOn(LED3);
Delay(BLINK_DELAY);
LEDOff(LED3);
}
void MinuteView(void)
{
unsigned int OnTime = 10*minute;
unsigned int OffTime = 120/minute;
for(LEDPointer=1, bitpointer=1;LEDPointer <= 6;LEDPointer++)
{
if(minute & bitpointer)
{
LEDOn();
}
delay(OnTime);
if(!(minute & bitpointer))
{
LEDOff();
}
delay(OffTime);
// All LEDs Off
P1DIR &= (~LED_MASK);
P1OUT &= (~LED_MASK);
bitpointer<<=1;
if (bitpointer==64) bitpointer=1;
}
}
/**
@brief Sends a photo via telemetry
@warning Executing this function takes ages. (About 0,5 to 4 seconds, depending on the resolution.)
*/
void sendPhoto(uint16_t image_size_send, uint8_t ** current_image, uint8_t ** previous_image){
uint16_t image_width_send;
uint16_t image_height_send;
image_width_send = global_data.param[PARAM_IMAGE_WIDTH];
image_height_send = global_data.param[PARAM_IMAGE_HEIGHT];
//delay(250);
mavlink_msg_data_transmission_handshake_send(
MAVLINK_COMM_2,
MAVLINK_DATA_STREAM_IMG_RAW8U,
image_size_send,
image_width_send,
image_height_send,
image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1,
MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN,
100);
//send_image_now = false;
uint16_t frame = 0;
//delay(250);
dma_copy_image_buffers(current_image, previous_image, image_size_send, 1);
for (frame = 0; frame < image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1; frame++)
{
LEDOn(LED_ERR);
mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, ((uint8_t *) current_image)[frame * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]);
LEDOff(LED_ERR);
//delay(250);
//send_image_now = false;
}
}
void GoToSleep()
{
gFlags.light_sleep = !( gFlags.has_x32 || dfStatus.lsloff || gFlags.noclock );
ScreenOff();
LEDOff();
gFlags.firing = 0;
BatReadTimer = 50;
RTCSleep();
DevicesOnOff( 1 );
CLK_SysTickDelay( 250 );
CLK_SysTickDelay( 250 );
CLK_SysTickDelay( 250 );
if ( dfStatus.off || PE0 || KeyPressTime == 1100 )
{
SYS_UnlockReg();
WDT_Close();
FlushAndSleep();
PreheatDelay = 0;
}
WDT_Open( WDT_TIMEOUT_2POW14, WDT_RESET_DELAY_18CLK, TRUE, FALSE );
SYS_LockReg();
gFlags.refresh_battery = 1;
DevicesOnOff( 0 );
RTCWakeUp();
InitDisplay();
}
void loop()
{
while (Serial.available() > 0) {
int mode = Serial.parseInt();
Serial.println(mode);
switch (mode) {
case 1:
red();
break;
case 2:
green();
break;
case 3:
blue();
break;
case 4:
yellow();
break;
case 5:
pink();
break;
default:
LEDOff();
break;
}
}
}
/**
Init pin mode
**/
void setup()
{
pinMode(r, OUTPUT);
pinMode(g, OUTPUT);
pinMode(b, OUTPUT);
LEDOff();
Serial.begin(9600); // initialize serial communications at 9600 bps
}
/**
* @brief Blink with LED1
* @param num - blinks number
* @param del - delay
* @retval None
*/
void BlinkLED1(uint32_t num, uint32_t del)
{
uint32_t cnt;
for ( cnt = 0; cnt < num; cnt++)
{
LEDOn(LED1);
Delay(del);
LEDOff(LED1);
Delay(del);
}
}
void HourView(void)
{
j = 5000/hour;
for(i=0;i<j;i++)
{
unsigned int OnTime = 10*hour;
unsigned int OffTime = 120/hour;
for(LEDPointer = 1;LEDPointer <= hour;LEDPointer++)
{
LEDOn();
delay(OnTime);
LEDOff();
delay(OffTime);
}
}
}
/**
* @fn void Alarm(unsigned int * pBuffer)
*
* @brief Alarm implementation
*
* @param pBuffer an unsigned integer pointer
*/
void Alarm(unsigned int * pBuffer)
{
unsigned int r = 0;
unsigned char i;
for (i=1;i<MAX_NODES;i++) {
if (Nodes[i].ID) { // Is it a valid network member
r |= Nodes[i].Data[2] & 0x8000; // 'OR' all red LED's (alarm)
}
}
Nodes[0].Data[2] = (Nodes[0].Data[2] & (~0x8000)) | r; // Update our local copy for a red LED Alarm
if (r) { // Write to the HW the red LED (redundant)
LEDOn(__BSP_LEDRED1);
} else {
LEDOff(__BSP_LEDRED1);
}
}
void send_video_fn(void) {
/* update the rate */
timer_update(send_video_fn, global_data.param[PARAM_VIDEO_RATE]);
if (previous_image == NULL) return;
/* transmit raw 8-bit image */
if (global_data.param[PARAM_USB_SEND_VIDEO] && !global_data.param[PARAM_VIDEO_ONLY])
{
mavlink_send_image(previous_image);
LEDToggle(LED_COM);
}
else if (!global_data.param[PARAM_USB_SEND_VIDEO])
{
LEDOff(LED_COM);
}
}
void compassLEDs(float ftiltheading){
LEDOff();
if(337.5 < ftiltheading || ftiltheading <= 22.5 ) {
// NORTH
setLed(NORTH, 1);
}
else if(22.5 < ftiltheading && ftiltheading <= 67.5) {
// NORTH - WEST
setLed(NORTH, 1);
setLed(WEST, 1);
}
else if(67.5 < ftiltheading && ftiltheading <= 112.5) {
// WEST
setLed(WEST, 1);
}
else if(112.5 < ftiltheading && ftiltheading <= 157.5) {
// SOUTH- WEST
setLed(SOUTH, 1);
setLed(WEST, 1);
}
else if(157.5 < ftiltheading && ftiltheading <= 205.5) {
// SOUTH
setLed(SOUTH, 1);
}
else if(205.5 < ftiltheading && ftiltheading <= 247.5) {
// SOUTH -EAST
setLed(SOUTH, 1);
setLed(EAST, 1);
}
else if(247.5 < ftiltheading && ftiltheading <= 292.5) {
// EAST
setLed(EAST, 1);
}
else if(292.5 < ftiltheading && ftiltheading <= 337.5) {
// NORTH - EAST
setLed(NORTH, 1);
setLed(EAST, 1);
}
else{
/* not valid value */
}
}
void AllLEDOff() {
uint8_t i;
for (i = 0; i < 8; i++)
LEDOff(i);
}
int main(void) {
// Setup pins
DDRB = 0x00; // All pins inputs
DDRC = 0x07; // PC0,1,2 as outputs for the LEDs
DDRD = 0x0b; // PD0 as output for LED, PD1 for TXD, PD3 for buzzer
PORTD = 0xe0; // Pullups for switches
PORTB = 0x3f; // Pullups for switches
// LEDs
LEDOff();
// ADC
ADMUX = 0x4f; // start off with mux on internal input
ADCSRA = 0x80; // ADC EN
DIDR0 = 0x38; // disable digital buffers on ADC pins
// Timer
TCCR0A = 0x00; // Normal mode
TCCR0B = 0x03; // prescaler at 64 results in 125kHz ticks
// UART
cli(); // disable global interrupts
stickInit(); // initialise stick input
spektrumInit();
// Get startup mode
trainerPlugged = TRAINERPIN;
bindSwitch = BINDPIN;
transmitMode = 0;
if(trainerPlugged == 0) transmitMode = eTM_trainer_master; // fixme or trainer slave, if PPM signal present
if(bindSwitch == 0) transmitMode = eTM_bind;
// Timer loop!
static uint8_t fastScaler = 255;
static uint8_t slowScaler = 255;
static uint16_t secondScaler = 0xffff;
// Buzzer
TCCR2B = 0x03; // prescaler at 64
stopNote();
noteBuffer[0] = NOTE6G;
noteBuffer[1] = NOTE6GS;
noteBuffer[2] = NOTESTOP;
noteCounter = 0;
noteInterruptable = 0;
while(1) {
while(TCNT0 < FASTLOOPCOUNT)
{
if(TCNT0 < FASTLOOPCOUNT && TCNT0 >= FASTLOOPCOUNT-LED0Duty/3) LED0On(); // red LED too bright, reduce duty
if(TCNT0 < FASTLOOPCOUNT && TCNT0 >= FASTLOOPCOUNT-LED1Duty) LED1On();
if(TCNT0 < FASTLOOPCOUNT && TCNT0 >= FASTLOOPCOUNT-LED2Duty) LED2On();
if(TCNT0 < FASTLOOPCOUNT && TCNT0 >= FASTLOOPCOUNT-LED3Duty) LED3On();
}
TCNT0 = 0; // reduce jitter by resetting soon (SPEKTRUM does not like jitter!)
LEDOff();
// should be going at about 625Hz
getDigital();
stickGetRawADC(rawstickvalues);
if(++fastScaler >= OVERSAMPLE) //should be 22ms for DSMX (Nano Board can not handle it faster!!)
{
fastScaler = 0;
// this loop runs slower than 50Hz
fastLoop();
/*throVoltage = 0;
ruddVoltage = 0;
elevVoltage = 0;
aileVoltage = 0;*/
rawstickvalues[0]=0;
rawstickvalues[1]=0;
rawstickvalues[2]=0;
rawstickvalues[3]=0;
if(++slowScaler >= 6)
{ // should be going at about 10Hz
slowScaler = 0;
slowLoop();
}
}
// we are here every 2ms
if(++secondScaler >=500)
{
// every second
secondScaler=0;
IdleSeconds++;
if(auxSwitch && mixToggle)
{
FlySeconds++;
if(FlySeconds == FLYSECONDS_MAX)
oneTone(NOTE7B);
if(FlySeconds == FLYSECONDS_MAX+10)
twoTone(NOTE7C);
if(IdleSeconds >= FLYSECONDS_MAX*2)
{
twoTone(NOTE7C);
}
}
}
}
}
int main(void)
{
#warning change clock hse_value to 25m
SYSTIM_Init();
LEDInit(LED_ACT);
LEDInit(LED_COM);
LEDInit(LED_ERR);
LEDOn(LED_ACT);
LEDOff(LED_COM);
LEDOff(LED_ERR);
SYSTIM_DelayTms(1000);
LEDOff(LED_ACT);
// USBD_Init(&USB_OTG_dev,
// USB_OTG_FS_CORE_ID,
// &USR_desc,
// &USBD_CDC_cb,
// &USR_cb);
usart_init();
UART_DMAInit();
if (!CheckXCLK())
{
XCLK_ON();
SYSTIM_DelayTms(100);
}
#ifdef USE_MT9V034
MT9V034_Init();
#endif
#ifdef USE_MT9D111
MT9D111_Init();
#endif
#warning already enable i2c when init camera
//mpu6050_enable();
memset(Cam_Capture,'1',FULL_IMAGE_SIZE);
Cam_Capture[FULL_IMAGE_SIZE] = 's';
Cam_Capture[FULL_IMAGE_SIZE + 1] = 't';
Cam_Capture[FULL_IMAGE_SIZE + 2] = 'p';
DCMI_CaptureCmd(ENABLE);
while(1)
{
int i=0;
#ifdef TEST_MPU6050
bool res;
res = I2C_ReadBytes(I2C2,data,MPU_ADDR, MPU_DATA, 14);
if (!res)
{
LEDOn(LED_ERR);
}
ax = (double)(data[1] + (data[0]<<8));
ay = (double)(data[3] + (data[2]<<8));
az = (double)(data[5] + (data[4]<<8));
wx = (double)(data[9] + (data[8]<<8));
wy = (double)(data[11] + (data[10]<<8));
wz = (double)(data[13] + (data[12]<<8));
SYSTIM_DelayTms(100);
#endif
if (DCMI_Flag)
{
DCMI_Flag = 0;
LEDToggle(LED_ACT);
// SendImageDMA((uint32_t*)&Cam_Capture[0], FULL_IMAGE_SIZE);
// for (i=0;i<FULL_IMAGE_SIZE;i++)
// {
// Cam_Buffer[i] = Cam_Capture[i];
// }
VCP_DataTx(Cam_Capture,FULL_IMAGE_SIZE+3);
SYSTIM_DelayTms(5000);
DCMI_CaptureCmd(ENABLE);
}
}
}
/**
* @brief Main function.
*/
int main(void)
{
/* load settings and parameters */
global_data_reset_param_defaults();
global_data_reset();
/* init led */
LEDInit(LED_ACT);
LEDInit(LED_COM);
LEDInit(LED_ERR);
LEDOff(LED_ACT);
LEDOff(LED_COM);
LEDOff(LED_ERR);
/* enable FPU on Cortex-M4F core */
SCB_CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); /* set CP10 Full Access and set CP11 Full Access */
/* init clock */
if (SysTick_Config(SystemCoreClock / 1000))
{
/* capture clock error */
LEDOn(LED_ERR);
while (1);
}
/* init usb */
USBD_Init( &USB_OTG_dev,
USB_OTG_FS_CORE_ID,
&USR_desc,
&USBD_CDC_cb,
&USR_cb);
/* init mavlink */
communication_init();
/* enable image capturing */
enable_image_capture();
/* gyro config */
gyro_config();
/* init and clear fast image buffers */
for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++)
{
image_buffer_8bit_1[i] = 0;
image_buffer_8bit_2[i] = 0;
}
uint8_t * current_image = image_buffer_8bit_1;
uint8_t * previous_image = image_buffer_8bit_2;
/* usart config*/
usart_init();
/* i2c config*/
i2c_init();
/* sonar config*/
float sonar_distance_filtered = 0.0f; // distance in meter
float sonar_distance_raw = 0.0f; // distance in meter
bool distance_valid = false;
sonar_config();
/* reset/start timers */
timer[TIMER_SONAR] = SONAR_TIMER_COUNT;
timer[TIMER_SYSTEM_STATE] = SYSTEM_STATE_COUNT;
timer[TIMER_RECEIVE] = SYSTEM_STATE_COUNT / 2;
timer[TIMER_PARAMS] = PARAMS_COUNT;
timer[TIMER_IMAGE] = global_data.param[PARAM_VIDEO_RATE];
/* variables */
uint32_t counter = 0;
uint8_t qual = 0;
/* bottom flow variables */
float pixel_flow_x = 0.0f;
float pixel_flow_y = 0.0f;
float pixel_flow_x_sum = 0.0f;
float pixel_flow_y_sum = 0.0f;
float velocity_x_sum = 0.0f;
float velocity_y_sum = 0.0f;
float velocity_x_lp = 0.0f;
float velocity_y_lp = 0.0f;
int valid_frame_count = 0;
int pixel_flow_count = 0;
/* main loop */
while (1)
{
/* reset flow buffers if needed */
if(buffer_reset_needed)
{
buffer_reset_needed = 0;
for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++)
{
image_buffer_8bit_1[i] = 0;
image_buffer_8bit_2[i] = 0;
}
delay(500);
continue;
}
/* calibration routine */
if(global_data.param[PARAM_CALIBRATION_ON])
{
while(global_data.param[PARAM_CALIBRATION_ON])
{
dcmi_restart_calibration_routine();
/* waiting for first quarter of image */
while(get_frame_counter() < 2){}
dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1);
/* waiting for second quarter of image */
while(get_frame_counter() < 3){}
dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1);
/* waiting for all image parts */
while(get_frame_counter() < 4){}
send_calibration_image(&previous_image, ¤t_image);
if (global_data.param[PARAM_SYSTEM_SEND_STATE])
communication_system_state_send();
communication_receive_usb();
debug_message_send_one();
communication_parameter_send();
LEDToggle(LED_COM);
}
dcmi_restart_calibration_routine();
LEDOff(LED_COM);
}
uint16_t image_size = global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT];
/* new gyroscope data */
float x_rate_sensor, y_rate_sensor, z_rate_sensor;
gyro_read(&x_rate_sensor, &y_rate_sensor, &z_rate_sensor);
/* gyroscope coordinate transformation */
float x_rate = y_rate_sensor; // change x and y rates
float y_rate = - x_rate_sensor;
float z_rate = z_rate_sensor; // z is correct
/* 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
/* debug */
float x_rate_pixel = x_rate * (get_time_between_images() / 1000.0f) * focal_length_px;
float y_rate_pixel = y_rate * (get_time_between_images() / 1000.0f) * focal_length_px;
//FIXME for the old sensor PX4FLOW v1.2 uncomment this!!!!
// x_rate = x_rate_raw_sensor; // change x and y rates
// y_rate = y_rate_raw_sensor;
/* get sonar data */
sonar_read(&sonar_distance_filtered, &sonar_distance_raw);
/* compute optical flow */
if(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM)
{
/* copy recent image to faster ram */
dma_copy_image_buffers(¤t_image, &previous_image, image_size, 1);
/* compute optical flow */
qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y);
if (sonar_distance_filtered > 5.0f || sonar_distance_filtered == 0.0f)
{
/* distances above 5m are considered invalid */
sonar_distance_filtered = 0.0f;
distance_valid = false;
}
else
{
distance_valid = true;
}
/*
* real point P (X,Y,Z), image plane projection p (x,y,z), focal-length f, distance-to-scene Z
* x / f = X / Z
* y / f = Y / Z
*/
float flow_compx = pixel_flow_x / focal_length_px / (get_time_between_images() / 1000.0f);
float flow_compy = pixel_flow_y / focal_length_px / (get_time_between_images() / 1000.0f);
/* integrate velocity and output values only if distance is valid */
if (distance_valid)
{
/* calc velocity (negative of flow values scaled with distance) */
float new_velocity_x = - flow_compx * sonar_distance_filtered;
float new_velocity_y = - flow_compy * sonar_distance_filtered;
if (qual > 0)
{
velocity_x_sum += new_velocity_x;
velocity_y_sum += new_velocity_y;
valid_frame_count++;
/* lowpass velocity output */
velocity_x_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_x +
(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
velocity_y_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_y +
(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
}
else
{
/* taking flow as zero */
velocity_x_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
velocity_y_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
}
}
else
{
/* taking flow as zero */
velocity_x_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
velocity_y_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
}
pixel_flow_x_sum += pixel_flow_x;
pixel_flow_y_sum += pixel_flow_y;
pixel_flow_count++;
}
counter++;
/* TODO for debugging */
//mavlink_msg_named_value_float_send(MAVLINK_COMM_2, boot_time_ms, "blabla", blabla);
if(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM)
{
/* send bottom flow if activated */
if (counter % 2 == 0)
{
float flow_comp_m_x = 0.0f;
float flow_comp_m_y = 0.0f;
float ground_distance = 0.0f;
if(global_data.param[PARAM_BOTTOM_FLOW_LP_FILTERED])
{
flow_comp_m_x = velocity_x_lp;
flow_comp_m_y = velocity_y_lp;
}
else
{
flow_comp_m_x = velocity_x_sum/valid_frame_count;
flow_comp_m_y = velocity_y_sum/valid_frame_count;
}
if(global_data.param[PARAM_SONAR_FILTERED])
ground_distance = sonar_distance_filtered;
else
ground_distance = sonar_distance_raw;
if (valid_frame_count > 0)
{
// send flow
mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_ms() * 1000, global_data.param[PARAM_SENSOR_ID],
pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
flow_comp_m_x, flow_comp_m_y, qual, ground_distance);
if (global_data.param[PARAM_USB_SEND_FLOW])
mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_ms() * 1000, global_data.param[PARAM_SENSOR_ID],
pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
flow_comp_m_x, flow_comp_m_y, qual, ground_distance);
update_TX_buffer(pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, flow_comp_m_x, flow_comp_m_y, qual,
ground_distance, x_rate, y_rate, z_rate);
}
else
{
// send distance
mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_ms() * 1000, global_data.param[PARAM_SENSOR_ID],
pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
0.0f, 0.0f, 0, ground_distance);
if (global_data.param[PARAM_USB_SEND_FLOW])
mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_ms() * 1000, global_data.param[PARAM_SENSOR_ID],
pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
0.0f, 0.0f, 0, ground_distance);
update_TX_buffer(pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, 0.0f, 0.0f, 0, ground_distance, x_rate, y_rate,
z_rate);
}
if(global_data.param[PARAM_USB_SEND_GYRO])
{
mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_ms() * 1000, x_rate, y_rate, z_rate);
}
velocity_x_sum = 0.0f;
velocity_y_sum = 0.0f;
pixel_flow_x_sum = 0.0f;
pixel_flow_y_sum = 0.0f;
valid_frame_count = 0;
pixel_flow_count = 0;
}
}
/* forward flow from other sensors */
if (counter % 2)
{
communication_receive_forward();
}
/* send system state, receive commands */
if (send_system_state_now)
{
/* every second */
if (global_data.param[PARAM_SYSTEM_SEND_STATE])
{
communication_system_state_send();
}
send_system_state_now = false;
}
/* receive commands */
if (receive_now)
{
/* test every second */
communication_receive();
communication_receive_usb();
receive_now = false;
}
/* sending debug msgs and requested parameters */
if (send_params_now)
{
debug_message_send_one();
communication_parameter_send();
send_params_now = false;
}
/* transmit raw 8-bit image */
if (global_data.param[PARAM_USB_SEND_VIDEO] && send_image_now)
{
/* get size of image to send */
uint16_t image_size_send;
uint16_t image_width_send;
uint16_t image_height_send;
image_size_send = image_size;
image_width_send = global_data.param[PARAM_IMAGE_WIDTH];
image_height_send = global_data.param[PARAM_IMAGE_HEIGHT];
if (global_data.param[PARAM_VIDEO_USB_MODE] == CAM_VIDEO)
{
mavlink_msg_data_transmission_handshake_send(
MAVLINK_COMM_2,
MAVLINK_DATA_STREAM_IMG_RAW8U,
image_size_send,
image_width_send,
image_height_send,
image_size_send / 253 + 1,
253,
100);
LEDToggle(LED_COM);
uint16_t frame;
for (frame = 0; frame < image_size_send / 253 + 1; frame++)
{
mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) current_image)[frame * 253]);
}
}
else if (global_data.param[PARAM_VIDEO_USB_MODE] == FLOW_VIDEO)
{
mavlink_msg_data_transmission_handshake_send(
MAVLINK_COMM_2,
MAVLINK_DATA_STREAM_IMG_RAW8U,
image_size_send,
image_width_send,
image_height_send,
image_size_send / 253 + 1,
253,
100);
LEDToggle(LED_COM);
uint16_t frame;
for (frame = 0; frame < image_size / 253 + 1; frame++)
{
mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) previous_image)[frame * 253]);
}
}
send_image_now = false;
}
else if (!global_data.param[PARAM_USB_SEND_VIDEO])
{
LEDOff(LED_COM);
}
}
}
//****************************************************************************
//
// This command allows setting, clearing or toggling the Status LED.
//
// The first argument should be one of the following:
// on - Turn on the LED.
// off - Turn off the LEd.
// toggle - Toggle the current LED status.
// activity - Set the LED mode to monitor serial activity.
//
//****************************************************************************
int
Cmd_led(int argc, char *argv[])
{
//
// These values only check the second character since all parameters are
// different in that character.
//
if(argv[1][1] == 'n')
{
//
// Turn on the LED.
//
LEDOn();
//
// Switch off activity mode.
//
HWREGBITW(&g_ulFlags, FLAG_LED_ACTIVITY) = 0;
}
else if(argv[1][1] == 'f')
{
//
// Turn off the LED.
//
LEDOff();
//
// Switch off activity mode.
//
HWREGBITW(&g_ulFlags, FLAG_LED_ACTIVITY) = 0;
}
else if(argv[1][1] == 'o')
{
//
// Toggle the LED.
//
LEDToggle();
//
// Switch off activity mode.
//
HWREGBITW(&g_ulFlags, FLAG_LED_ACTIVITY) = 0;
}
else if(argv[1][1] == 'c')
{
//
// If this is the "activity" value then set the activity mode.
//
HWREGBITW(&g_ulFlags, FLAG_LED_ACTIVITY) = 1;
}
else
{
//
// The command format was not correct so print out some help.
//
CommandPrint("\nled <on|off|toggle|activity>\n");
CommandPrint(" on - Turn on the LED.\n");
CommandPrint(" off - Turn off the LED.\n");
CommandPrint(" toggle - Toggle the LED state.\n");
CommandPrint(" activity - LED state will toggle on UART activity.\n");
}
return(0);
}
//****************************************************************************
//
// Handles CDC driver notifications related to control and setup of the
// device.
//
// \param pvCBData is the client-supplied callback pointer for this channel.
// \param ulEvent identifies the event we are being notified about.
// \param ulMsgValue is an event-specific value.
// \param pvMsgData is an event-specific pointer.
//
// This function is called by the CDC driver to perform control-related
// operations on behalf of the USB host. These functions include setting
// and querying the serial communication parameters, setting handshake line
// states and sending break conditions.
//
// \return The return value is event-specific.
//
//****************************************************************************
unsigned long
SerialHandler(void *pvCBData, unsigned long ulEvent, unsigned long ulMsgValue,
void *pvMsgData)
{
tLineCoding *psLineCoding;
//
// Which event are we being asked to process?
//
switch(ulEvent)
{
//
// We are connected to a host and communication is now possible.
//
case USB_EVENT_CONNECTED:
{
//
// Flush our buffers.
//
USBBufferFlush(&g_sTxBuffer);
USBBufferFlush(&g_sRxBuffer);
//
// Turn on the user LED.
//
LEDOn();
break;
}
case USB_EVENT_DISCONNECTED:
{
//
// Turn off the user LED.
//
LEDOff();
break;
}
//
// Return the current serial communication parameters.
//
case USBD_CDC_EVENT_GET_LINE_CODING:
{
//
// Create a pointer to the line coding information.
//
psLineCoding = (tLineCoding *)pvMsgData;
//
// Copy the current line coding information into the structure.
//
psLineCoding->ulRate = g_sLineCoding.ulRate;
psLineCoding->ucStop = g_sLineCoding.ucStop;
psLineCoding->ucParity = g_sLineCoding.ucParity;
psLineCoding->ucDatabits = g_sLineCoding.ucDatabits;
break;
}
//
// Set the current serial communication parameters.
//
case USBD_CDC_EVENT_SET_LINE_CODING:
{
//
// Create a pointer to the line coding information.
//
psLineCoding = (tLineCoding *)pvMsgData;
//
// Copy the line coding information into the current line coding
// structure.
//
g_sLineCoding.ulRate = psLineCoding->ulRate;
g_sLineCoding.ucStop = psLineCoding->ucStop;
g_sLineCoding.ucParity = psLineCoding->ucParity;
g_sLineCoding.ucDatabits = psLineCoding->ucDatabits;
break;
}
//
// Set the current serial communication parameters.
//
case USBD_CDC_EVENT_SET_CONTROL_LINE_STATE:
{
break;
}
//
// All other events are ignored.
//
default:
{
break;
}
}
return(0);
}
/**
* @brief Main function.
*/
int main(void)
{
__enable_irq();
snapshot_buffer = BuildCameraImageBuffer(snapshot_buffer_mem);
/* load settings and parameters */
global_data_reset_param_defaults();
global_data_reset();
/* init led */
LEDInit(LED_ACT);
LEDInit(LED_COM);
LEDInit(LED_ERR);
LEDOff(LED_ACT);
LEDOff(LED_COM);
LEDOff(LED_ERR);
/* enable FPU on Cortex-M4F core */
SCB_CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); /* set CP10 Full Access and set CP11 Full Access */
/* init timers */
timer_init();
/* init usb */
USBD_Init( &USB_OTG_dev,
USB_OTG_FS_CORE_ID,
&USR_desc,
&USBD_CDC_cb,
&USR_cb);
/* init mavlink */
communication_init();
/* initialize camera: */
img_stream_param.size.x = FLOW_IMAGE_SIZE;
img_stream_param.size.y = FLOW_IMAGE_SIZE;
img_stream_param.binning = 4;
{
camera_image_buffer buffers[5] = {
BuildCameraImageBuffer(image_buffer_8bit_1),
BuildCameraImageBuffer(image_buffer_8bit_2),
BuildCameraImageBuffer(image_buffer_8bit_3),
BuildCameraImageBuffer(image_buffer_8bit_4),
BuildCameraImageBuffer(image_buffer_8bit_5)
};
camera_init(&cam_ctx, mt9v034_get_sensor_interface(), dcmi_get_transport_interface(),
mt9v034_get_clks_per_row(64, 4) * 1, mt9v034_get_clks_per_row(64, 4) * 64, 2.0,
&img_stream_param, buffers, 5);
}
/* gyro config */
gyro_config();
/* usart config*/
usart_init();
/* i2c config*/
i2c_init();
/* sonar config*/
float sonar_distance_filtered = 0.0f; // distance in meter
float sonar_distance_raw = 0.0f; // distance in meter
bool distance_valid = false;
sonar_config();
/* reset/start timers */
timer_register(sonar_update_fn, SONAR_POLL_MS);
timer_register(system_state_send_fn, SYSTEM_STATE_MS);
timer_register(system_receive_fn, SYSTEM_STATE_MS / 2);
timer_register(send_params_fn, PARAMS_MS);
timer_register(send_video_fn, global_data.param[PARAM_VIDEO_RATE]);
timer_register(take_snapshot_fn, 500);
//timer_register(switch_params_fn, 2000);
/* variables */
uint32_t counter = 0;
result_accumulator_ctx mavlink_accumulator;
result_accumulator_init(&mavlink_accumulator);
uint32_t fps_timing_start = get_boot_time_us();
uint16_t fps_counter = 0;
uint16_t fps_skipped_counter = 0;
uint32_t last_frame_index = 0;
/* main loop */
while (1)
{
/* check timers */
timer_check();
if (snap_capture_done) {
snap_capture_done = false;
camera_snapshot_acknowledge(&cam_ctx);
snap_ready = true;
if (snap_capture_success) {
/* send the snapshot! */
LEDToggle(LED_COM);
mavlink_send_image(&snapshot_buffer);
}
}
/* calculate focal_length in pixel */
const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 0.006f); //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled
/* new gyroscope data */
float x_rate_sensor, y_rate_sensor, z_rate_sensor;
int16_t gyro_temp;
gyro_read(&x_rate_sensor, &y_rate_sensor, &z_rate_sensor,&gyro_temp);
/* gyroscope coordinate transformation to flow sensor coordinates */
float x_rate = y_rate_sensor; // change x and y rates
float y_rate = - x_rate_sensor;
float z_rate = z_rate_sensor; // z is correct
/* get sonar data */
distance_valid = sonar_read(&sonar_distance_filtered, &sonar_distance_raw);
/* reset to zero for invalid distances */
if (!distance_valid) {
sonar_distance_filtered = 0.0f;
sonar_distance_raw = 0.0f;
}
bool use_klt = global_data.param[PARAM_ALGORITHM_CHOICE] != 0;
uint32_t start_computations = 0;
/* get recent images */
camera_image_buffer *frames[2];
camera_img_stream_get_buffers(&cam_ctx, frames, 2, true);
start_computations = get_boot_time_us();
int frame_delta = ((int32_t)frames[0]->frame_number - (int32_t)last_frame_index);
last_frame_index = frames[0]->frame_number;
fps_skipped_counter += frame_delta - 1;
flow_klt_image *klt_images[2] = {NULL, NULL};
{
/* make sure that the new images get the correct treatment */
/* this algorithm will still work if both images are new */
int i;
bool used_klt_image[2] = {false, false};
for (i = 0; i < 2; ++i) {
if (frames[i]->frame_number != frames[i]->meta) {
// the image is new. apply pre-processing:
/* filter the new image */
if (global_data.param[PARAM_ALGORITHM_IMAGE_FILTER]) {
filter_image(frames[i]->buffer, frames[i]->param.p.size.x);
}
/* update meta data to mark it as an up-to date image: */
frames[i]->meta = frames[i]->frame_number;
} else {
// the image has the preprocessing already applied.
if (use_klt) {
int j;
/* find the klt image that matches: */
for (j = 0; j < 2; ++j) {
if (flow_klt_images[j].meta == frames[i]->frame_number) {
used_klt_image[j] = true;
klt_images[i] = &flow_klt_images[j];
}
}
}
}
}
if (use_klt) {
/* only for KLT: */
/* preprocess the images if they are not yet preprocessed */
for (i = 0; i < 2; ++i) {
if (klt_images[i] == NULL) {
// need processing. find unused KLT image:
int j;
for (j = 0; j < 2; ++j) {
if (!used_klt_image[j]) {
used_klt_image[j] = true;
klt_images[i] = &flow_klt_images[j];
break;
}
}
klt_preprocess_image(frames[i]->buffer, klt_images[i]);
}
}
}
}
float frame_dt = (frames[0]->timestamp - frames[1]->timestamp) * 0.000001f;
/* compute gyro rate in pixels and change to image coordinates */
float x_rate_px = - y_rate * (focal_length_px * frame_dt);
float y_rate_px = x_rate * (focal_length_px * frame_dt);
float z_rate_fr = - z_rate * frame_dt;
/* compute optical flow in pixels */
flow_raw_result flow_rslt[32];
uint16_t flow_rslt_count = 0;
if (!use_klt) {
flow_rslt_count = compute_flow(frames[1]->buffer, frames[0]->buffer, x_rate_px, y_rate_px, z_rate_fr, flow_rslt, 32);
} else {
flow_rslt_count = compute_klt(klt_images[1], klt_images[0], x_rate_px, y_rate_px, z_rate_fr, flow_rslt, 32);
}
/* calculate flow value from the raw results */
float pixel_flow_x;
float pixel_flow_y;
float outlier_threshold = global_data.param[PARAM_ALGORITHM_OUTLIER_THR_RATIO];
float min_outlier_threshold = 0;
if(global_data.param[PARAM_ALGORITHM_CHOICE] == 0)
{
min_outlier_threshold = global_data.param[PARAM_ALGORITHM_OUTLIER_THR_BLOCK];
}else
{
min_outlier_threshold = global_data.param[PARAM_ALGORITHM_OUTLIER_THR_KLT];
}
uint8_t qual = flow_extract_result(flow_rslt, flow_rslt_count, &pixel_flow_x, &pixel_flow_y,
outlier_threshold, min_outlier_threshold);
/* create flow image if needed (previous_image is not needed anymore)
* -> can be used for debugging purpose
*/
previous_image = frames[1];
if (global_data.param[PARAM_USB_SEND_VIDEO])
{
uint16_t frame_size = global_data.param[PARAM_IMAGE_WIDTH];
uint8_t *prev_img = previous_image->buffer;
for (int i = 0; i < flow_rslt_count; i++) {
if (flow_rslt[i].quality > 0) {
prev_img[flow_rslt[i].at_y * frame_size + flow_rslt[i].at_x] = 255;
int ofs = (int)floor(flow_rslt[i].at_y + flow_rslt[i].y * 2 + 0.5f) * frame_size + (int)floor(flow_rslt[i].at_x + flow_rslt[i].x * 2 + 0.5f);
if (ofs >= 0 && ofs < frame_size * frame_size) {
prev_img[ofs] = 200;
}
}
}
}
/* return the image buffers */
camera_img_stream_return_buffers(&cam_ctx, frames, 2);
/* decide which distance to use */
float ground_distance = 0.0f;
if(global_data.param[PARAM_SONAR_FILTERED])
{
ground_distance = sonar_distance_filtered;
}
else
{
ground_distance = sonar_distance_raw;
}
/* update I2C transmit buffer */
update_TX_buffer(frame_dt,
x_rate, y_rate, z_rate, gyro_temp,
qual, pixel_flow_x, pixel_flow_y, 1.0f / focal_length_px,
distance_valid, ground_distance, get_time_delta_us(get_sonar_measure_time()));
/* accumulate the results */
result_accumulator_feed(&mavlink_accumulator, frame_dt,
x_rate, y_rate, z_rate, gyro_temp,
qual, pixel_flow_x, pixel_flow_y, 1.0f / focal_length_px,
distance_valid, ground_distance, get_time_delta_us(get_sonar_measure_time()));
uint32_t computaiton_time_us = get_time_delta_us(start_computations);
counter++;
fps_counter++;
/* serial mavlink + usb mavlink output throttled */
if (counter % (uint32_t)global_data.param[PARAM_FLOW_SERIAL_THROTTLE_FACTOR] == 0)//throttling factor
{
float fps = 0;
float fps_skip = 0;
if (fps_counter + fps_skipped_counter > 100) {
uint32_t dt = get_time_delta_us(fps_timing_start);
fps_timing_start += dt;
fps = (float)fps_counter / ((float)dt * 1e-6f);
fps_skip = (float)fps_skipped_counter / ((float)dt * 1e-6f);
fps_counter = 0;
fps_skipped_counter = 0;
mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "TIMING", get_boot_time_us(), computaiton_time_us, fps, fps_skip);
}
mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "EXPOSURE", get_boot_time_us(),
frames[0]->param.exposure, frames[0]->param.analog_gain, cam_ctx.last_brightness);
/* calculate the output values */
result_accumulator_output_flow output_flow;
result_accumulator_output_flow_rad output_flow_rad;
int min_valid_ratio = global_data.param[PARAM_ALGORITHM_MIN_VALID_RATIO];
result_accumulator_calculate_output_flow(&mavlink_accumulator, min_valid_ratio, &output_flow);
result_accumulator_calculate_output_flow_rad(&mavlink_accumulator, min_valid_ratio, &output_flow_rad);
// send flow
mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
output_flow.flow_x, output_flow.flow_y,
output_flow.flow_comp_m_x, output_flow.flow_comp_m_y,
output_flow.quality, output_flow.ground_distance);
mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
output_flow_rad.integration_time,
output_flow_rad.integrated_x, output_flow_rad.integrated_y,
output_flow_rad.integrated_xgyro, output_flow_rad.integrated_ygyro, output_flow_rad.integrated_zgyro,
output_flow_rad.temperature, output_flow_rad.quality,
output_flow_rad.time_delta_distance_us,output_flow_rad.ground_distance);
if (global_data.param[PARAM_USB_SEND_FLOW] && (output_flow.quality > 0 || global_data.param[PARAM_USB_SEND_QUAL_0]))
{
mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
output_flow.flow_x, output_flow.flow_y,
output_flow.flow_comp_m_x, output_flow.flow_comp_m_y,
output_flow.quality, output_flow.ground_distance);
mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
output_flow_rad.integration_time,
output_flow_rad.integrated_x, output_flow_rad.integrated_y,
output_flow_rad.integrated_xgyro, output_flow_rad.integrated_ygyro, output_flow_rad.integrated_zgyro,
output_flow_rad.temperature, output_flow_rad.quality,
output_flow_rad.time_delta_distance_us,output_flow_rad.ground_distance);
}
if(global_data.param[PARAM_USB_SEND_GYRO])
{
mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_us(), x_rate, y_rate, z_rate);
}
result_accumulator_reset(&mavlink_accumulator);
}
/* forward flow from other sensors */
if (counter % 2)
{
communication_receive_forward();
}
}
}
void main(void)
#endif
{
RST_CLK_PCLKcmd(RST_CLK_PCLK_PORTD, ENABLE);
/* Configure all unused PORT pins to low power consumption */
PORT_StructInit(&PORT_InitStructure);
PORT_InitStructure.PORT_Pin = (PORT_Pin_All & ~(PORT_Pin_10 | PORT_Pin_11 |
PORT_Pin_12 | PORT_Pin_13 |
PORT_Pin_14));
PORT_Init(MDR_PORTD, &PORT_InitStructure);
/* Configure PORTD pins 10..14 for output to switch LEDs on/off */
PORT_InitStructure.PORT_Pin = (PORT_Pin_10 | PORT_Pin_11 | PORT_Pin_12 |
PORT_Pin_13 | PORT_Pin_14);
PORT_InitStructure.PORT_OE = PORT_OE_OUT;
PORT_InitStructure.PORT_FUNC = PORT_FUNC_PORT;
PORT_InitStructure.PORT_MODE = PORT_MODE_DIGITAL;
PORT_InitStructure.PORT_SPEED = PORT_SPEED_SLOW;
PORT_Init(MDR_PORTD, &PORT_InitStructure);
/* Consequently turn all three used LEDs on and off */
LEDOn(LED1);
Delay(4*BLINK_DELAY);
LEDOff(LED1);
Delay(4*BLINK_DELAY);
LEDOn(LED2);
Delay(4*BLINK_DELAY);
LEDOff(LED2);
Delay(4*BLINK_DELAY);
LEDOn(LED3);
Delay(4*BLINK_DELAY);
LEDOff(LED3);
Delay(4*BLINK_DELAY);
/* Infinite loop that demonstrates different input clock sources using */
while (1)
{
/* Set RST_CLK to default */
RST_CLK_DeInit();
RST_CLK_PCLKcmd(RST_CLK_PCLK_PORTD, ENABLE);
/* 1. CPU_CLK = HSI clock */
/* Enable HSI clock source */
RST_CLK_HSIcmd(ENABLE);
/* Switch LED2 on and wait for HSI ready status */
LEDOn(LED2);
Delay(BLINK_DELAY);
if (RST_CLK_HSIstatus() == SUCCESS) /* Good HSI clock */
{
/* Switch LED2 off */
LEDOff(LED2);
/* Select HSI clock on the CPU clock MUX */
RST_CLK_CPUclkSelection(RST_CLK_CPUclkHSI);
/* LED1 blinking with HSI clock as input clock source */
BlinkLED1(BLINK_NUM, BLINK_DELAY);
}
else /* HSI timeout */
{
IndicateError();
}
/* 2. CPU_CLK = HSI/2 clock */
/* Enable HSI clock source */
RST_CLK_HSIcmd(ENABLE);
/* Disable CPU_PLL */
RST_CLK_CPU_PLLcmd(DISABLE);
/* Select HSI/2 clock as CPU_PLL input clock source */
RST_CLK_CPU_PLLconfig(RST_CLK_CPU_PLLsrcHSIdiv2, 1);
/* Switch LED2 on and wait for HSI ready status */
LEDOn(LED2);
Delay(BLINK_DELAY);
if (RST_CLK_HSIstatus() == SUCCESS) /* Good HSI clock */
{
/* Switch LED2 off */
LEDOff(LED2);
/* Set CPU_C3_prescaler to 1 */
RST_CLK_CPUclkPrescaler(RST_CLK_CPUclkDIV1);
/* Switch CPU_C2_SEL to CPU_C1 clock instead of CPU_PLL output */
RST_CLK_CPU_PLLuse(DISABLE);
/* LED1 blinking with HSI/2 clock as input clock source */
BlinkLED1(BLINK_NUM, BLINK_DELAY);
}
else /* HSI timeout */
{
IndicateError();
}
/* 3. CPU_CLK = 7*HSE/2 clock */
/* Enable HSE clock oscillator */
RST_CLK_HSEconfig(RST_CLK_HSE_ON);
/* Switch LED2 on and wait for HSE ready status */
LEDOn(LED2);
Delay(BLINK_DELAY);
if (RST_CLK_HSEstatus() == SUCCESS) /* Good HSE clock */
{
/* Select HSE clock as CPU_PLL input clock source */
/* Set PLL multiplier to 7 */
RST_CLK_CPU_PLLconfig(RST_CLK_CPU_PLLsrcHSEdiv1, 7);
/* Enable CPU_PLL */
RST_CLK_CPU_PLLcmd(ENABLE);
/* Switch LED2 on and wait for PLL ready status */
if (RST_CLK_HSEstatus() == SUCCESS) /* Good CPU PLL */
{
/* Switch LED2 off */
LEDOff(LED2);
/* Set CPU_C3_prescaler to 2 */
RST_CLK_CPUclkPrescaler(RST_CLK_CPUclkDIV2);
/* Set CPU_C2_SEL to CPU_PLL output instead of CPU_C1 clock */
RST_CLK_CPU_PLLuse(ENABLE);
/* Select CPU_C3 clock on the CPU clock MUX */
RST_CLK_CPUclkSelection(RST_CLK_CPUclkCPU_C3);
/* LED1 blinking with 7*HSE/2 clock as input clock source */
BlinkLED1(BLINK_NUM, BLINK_DELAY);
}
else /* CPU_PLL timeout */
{
IndicateError();
}
}
else /* HSE timeout */
{
IndicateError();
}
/* 4. CPU_CLK = LSI clock */
/* Enable LSI clock source */
RST_CLK_LSIcmd(ENABLE);
/* Switch LED2 on and wait for LSI ready status */
LEDOn(LED2);
Delay(BLINK_DELAY);
if (RST_CLK_LSIstatus() == SUCCESS) /* Good LSI clock */
{
/* Switch LED2 off */
LEDOff(LED2);
/* Select LSI clock on the CPU clock MUX */
RST_CLK_CPUclkSelection(RST_CLK_CPUclkLSI);
/* LED1 blinking with LSI clock as input clock source */
BlinkLED1(BLINK_NUM, BLINK_DELAY);
}
else /* LSI timeout */
{
IndicateError();
}
}
}
/**
* @brief Main function.
*/
int main(void)
{
__enable_irq();
/* load settings and parameters */
global_data_reset_param_defaults();
global_data_reset();
PROBE_INIT();
/* init led */
LEDInit(LED_ACT);
LEDInit(LED_COM);
LEDInit(LED_ERR);
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);
/* enable FPU on Cortex-M4F core */
SCB_CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); /* set CP10 Full Access and set CP11 Full Access */
/* init clock */
if (SysTick_Config(SystemCoreClock / 100000))/*set timer to trigger interrupt every 10 microsecond */
{
/* capture clock error */
LEDOn(LED_ERR);
while (1);
}
/* init usb */
USBD_Init( &USB_OTG_dev,
USB_OTG_FS_CORE_ID,
&USR_desc,
&USBD_CDC_cb,
&USR_cb);
/* init mavlink */
communication_init();
/* enable image capturing */
enable_image_capture();
/* gyro config */
gyro_config();
/* init and clear fast image buffers */
for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++)
{
image_buffer_8bit_1[i] = 0;
image_buffer_8bit_2[i] = 0;
}
uint8_t * current_image = image_buffer_8bit_1;
uint8_t * previous_image = image_buffer_8bit_2;
/* usart config*/
usart_init();
/* i2c config*/
i2c_init();
/* sonar config*/
float sonar_distance_filtered = 0.0f; // distance in meter
float sonar_distance_raw = 0.0f; // distance in meter
bool distance_valid = false;
sonar_config();
/* reset/start timers */
timer[TIMER_SONAR] = SONAR_TIMER_COUNT;
timer[TIMER_SYSTEM_STATE] = SYSTEM_STATE_COUNT;
timer[TIMER_RECEIVE] = SYSTEM_STATE_COUNT / 2;
timer[TIMER_PARAMS] = PARAMS_COUNT;
timer[TIMER_IMAGE] = global_data.param[PARAM_VIDEO_RATE];
/* variables */
uint32_t counter = 0;
uint8_t qual = 0;
/* bottom flow variables */
float pixel_flow_x = 0.0f;
float pixel_flow_y = 0.0f;
float pixel_flow_x_sum = 0.0f;
float pixel_flow_y_sum = 0.0f;
float velocity_x_sum = 0.0f;
float velocity_y_sum = 0.0f;
float velocity_x_lp = 0.0f;
float velocity_y_lp = 0.0f;
int valid_frame_count = 0;
int pixel_flow_count = 0;
static float accumulated_flow_x = 0;
static float accumulated_flow_y = 0;
static float accumulated_gyro_x = 0;
static float accumulated_gyro_y = 0;
static float accumulated_gyro_z = 0;
static uint16_t accumulated_framecount = 0;
static uint16_t accumulated_quality = 0;
static uint32_t integration_timespan = 0;
static uint32_t lasttime = 0;
uint32_t time_since_last_sonar_update= 0;
uint32_t time_last_pub= 0;
uavcan_start();
/* main loop */
while (1)
{
PROBE_1(false);
uavcan_run();
PROBE_1(true);
/* reset flow buffers if needed */
if(buffer_reset_needed)
{
buffer_reset_needed = 0;
for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++)
{
image_buffer_8bit_1[i] = 0;
image_buffer_8bit_2[i] = 0;
}
delay(500);
continue;
}
/* calibration routine */
if(FLOAT_AS_BOOL(global_data.param[PARAM_VIDEO_ONLY]))
{
while(FLOAT_AS_BOOL(global_data.param[PARAM_VIDEO_ONLY]))
{
dcmi_restart_calibration_routine();
/* waiting for first quarter of image */
while(get_frame_counter() < 2){}
dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1);
/* waiting for second quarter of image */
while(get_frame_counter() < 3){}
dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1);
/* waiting for all image parts */
while(get_frame_counter() < 4){}
send_calibration_image(&previous_image, ¤t_image);
if (FLOAT_AS_BOOL(global_data.param[PARAM_SYSTEM_SEND_STATE]))
communication_system_state_send();
communication_receive_usb();
debug_message_send_one();
communication_parameter_send();
LEDToggle(LED_COM);
}
dcmi_restart_calibration_routine();
LEDOff(LED_COM);
}
uint16_t image_size = global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT];
/* new gyroscope data */
float x_rate_sensor, y_rate_sensor, z_rate_sensor;
int16_t gyro_temp;
gyro_read(&x_rate_sensor, &y_rate_sensor, &z_rate_sensor,&gyro_temp);
/* gyroscope coordinate transformation */
float x_rate = y_rate_sensor; // change x and y rates
float y_rate = - x_rate_sensor;
float z_rate = z_rate_sensor; // z is correct
/* 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
/* get sonar data */
distance_valid = sonar_read(&sonar_distance_filtered, &sonar_distance_raw);
/* reset to zero for invalid distances */
if (!distance_valid) {
sonar_distance_filtered = 0.0f;
sonar_distance_raw = 0.0f;
}
/* compute optical flow */
if (FLOAT_EQ_INT(global_data.param[PARAM_SENSOR_POSITION], BOTTOM))
{
/* copy recent image to faster ram */
dma_copy_image_buffers(¤t_image, &previous_image, image_size, 1);
/* compute optical flow */
qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y);
/*
* real point P (X,Y,Z), image plane projection p (x,y,z), focal-length f, distance-to-scene Z
* x / f = X / Z
* y / f = Y / Z
*/
float flow_compx = pixel_flow_x / focal_length_px / (get_time_between_images() / 1000000.0f);
float flow_compy = pixel_flow_y / focal_length_px / (get_time_between_images() / 1000000.0f);
if (qual > 0)
{
valid_frame_count++;
uint32_t deltatime = (get_boot_time_us() - lasttime);
integration_timespan += deltatime;
accumulated_flow_x += pixel_flow_y / focal_length_px * 1.0f; //rad axis swapped to align x flow around y axis
accumulated_flow_y += pixel_flow_x / focal_length_px * -1.0f;//rad
accumulated_gyro_x += x_rate * deltatime / 1000000.0f; //rad
accumulated_gyro_y += y_rate * deltatime / 1000000.0f; //rad
accumulated_gyro_z += z_rate * deltatime / 1000000.0f; //rad
accumulated_framecount++;
accumulated_quality += qual;
}
/* integrate velocity and output values only if distance is valid */
if (distance_valid)
{
/* calc velocity (negative of flow values scaled with distance) */
float new_velocity_x = - flow_compx * sonar_distance_filtered;
float new_velocity_y = - flow_compy * sonar_distance_filtered;
time_since_last_sonar_update = (get_boot_time_us()- get_sonar_measure_time());
if (qual > 0)
{
velocity_x_sum += new_velocity_x;
velocity_y_sum += new_velocity_y;
/* lowpass velocity output */
velocity_x_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_x +
(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
velocity_y_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_y +
(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
}
else
{
/* taking flow as zero */
velocity_x_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
velocity_y_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
}
}
else
{
/* taking flow as zero */
velocity_x_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
velocity_y_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
}
//update lasttime
lasttime = get_boot_time_us();
pixel_flow_x_sum += pixel_flow_x;
pixel_flow_y_sum += pixel_flow_y;
pixel_flow_count++;
}
counter++;
if (FLOAT_EQ_INT(global_data.param[PARAM_SENSOR_POSITION], BOTTOM))
{
/* send bottom flow if activated */
float ground_distance = 0.0f;
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_timestamp_export(i2c_data);
uavcan_assign(range_data.time_stamp_utc, i2c_data.time_stamp_utc);
//update I2C transmitbuffer
if(valid_frame_count>0)
{
update_TX_buffer(pixel_flow_x, pixel_flow_y, velocity_x_sum/valid_frame_count, velocity_y_sum/valid_frame_count, qual,
ground_distance, x_rate, y_rate, z_rate, gyro_temp, uavcan_use_export(i2c_data));
}
else
{
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_3(false);
uavcan_publish(flow, 40, i2c_data);
PROBE_3(true);
//serial mavlink + usb mavlink output throttled
uint32_t now = get_boot_time_us();
uint32_t time_since_last_pub = now - time_last_pub;
if (time_since_last_pub > (1.0e6f/global_data.param[PARAM_BOTTOM_FLOW_PUB_RATE]))
{
time_last_pub = now;
float flow_comp_m_x = 0.0f;
float flow_comp_m_y = 0.0f;
if(FLOAT_AS_BOOL(global_data.param[PARAM_BOTTOM_FLOW_LP_FILTERED]))
{
flow_comp_m_x = velocity_x_lp;
flow_comp_m_y = velocity_y_lp;
}
else
{
if(valid_frame_count>0)
{
flow_comp_m_x = velocity_x_sum/valid_frame_count;
flow_comp_m_y = velocity_y_sum/valid_frame_count;
}
else
{
flow_comp_m_x = 0.0f;
flow_comp_m_y = 0.0f;
}
}
// send flow
mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
flow_comp_m_x, flow_comp_m_y, qual, ground_distance);
mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
integration_timespan, accumulated_flow_x, accumulated_flow_y,
accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z,
gyro_temp, accumulated_quality/accumulated_framecount,
time_since_last_sonar_update,ground_distance);
/* send approximate local position estimate without heading */
if (FLOAT_AS_BOOL(global_data.param[PARAM_SYSTEM_SEND_LPOS]))
{
/* rough local position estimate for unit testing */
lpos.x += ground_distance*accumulated_flow_x;
lpos.y += ground_distance*accumulated_flow_y;
lpos.z = -ground_distance;
lpos.vx = ground_distance*accumulated_flow_x/integration_timespan;
lpos.vy = ground_distance*accumulated_flow_y/integration_timespan;
lpos.vz = 0; // no direct measurement, just ignore
} else {
/* toggling param allows user reset */
lpos.x = 0;
lpos.y = 0;
lpos.z = 0;
lpos.vx = 0;
lpos.vy = 0;
lpos.vz = 0;
}
if (FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_FLOW]))
{
mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
flow_comp_m_x, flow_comp_m_y, qual, ground_distance);
mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
integration_timespan, accumulated_flow_x, accumulated_flow_y,
accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z,
gyro_temp, accumulated_quality/accumulated_framecount,
time_since_last_sonar_update,ground_distance);
}
if(FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_GYRO]))
{
mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_us(), x_rate, y_rate, z_rate);
}
integration_timespan = 0;
accumulated_flow_x = 0;
accumulated_flow_y = 0;
accumulated_framecount = 0;
accumulated_quality = 0;
accumulated_gyro_x = 0;
accumulated_gyro_y = 0;
accumulated_gyro_z = 0;
velocity_x_sum = 0.0f;
velocity_y_sum = 0.0f;
pixel_flow_x_sum = 0.0f;
pixel_flow_y_sum = 0.0f;
valid_frame_count = 0;
pixel_flow_count = 0;
}
}
/* forward flow from other sensors */
if (counter % 2)
{
communication_receive_forward();
}
/* send system state, receive commands */
if (send_system_state_now)
{
/* every second */
if (FLOAT_AS_BOOL(global_data.param[PARAM_SYSTEM_SEND_STATE]))
{
communication_system_state_send();
}
send_system_state_now = false;
}
/* receive commands */
if (receive_now)
{
/* test every second */
communication_receive();
communication_receive_usb();
receive_now = false;
}
/* sending debug msgs and requested parameters */
if (send_params_now)
{
debug_message_send_one();
communication_parameter_send();
send_params_now = false;
}
/* send local position estimate, for testing only, doesn't account for heading */
if (send_lpos_now)
{
if (FLOAT_AS_BOOL(global_data.param[PARAM_SYSTEM_SEND_LPOS]))
{
mavlink_msg_local_position_ned_send(MAVLINK_COMM_2, timer_ms, lpos.x, lpos.y, lpos.z, lpos.vx, lpos.vy, lpos.vz);
}
send_lpos_now = false;
}
/* transmit raw 8-bit image */
if (FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_VIDEO])&& send_image_now)
{
/* get size of image to send */
uint16_t image_size_send;
uint16_t image_width_send;
uint16_t image_height_send;
image_size_send = image_size;
image_width_send = global_data.param[PARAM_IMAGE_WIDTH];
image_height_send = global_data.param[PARAM_IMAGE_HEIGHT];
mavlink_msg_data_transmission_handshake_send(
MAVLINK_COMM_2,
MAVLINK_DATA_STREAM_IMG_RAW8U,
image_size_send,
image_width_send,
image_height_send,
image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1,
MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN,
100);
LEDToggle(LED_COM);
uint16_t frame = 0;
for (frame = 0; frame < image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1; frame++)
{
mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) previous_image)[frame * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]);
}
send_image_now = false;
}
else if (!FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_VIDEO]))
{
LEDOff(LED_COM);
}
}
}
void SDCardReadWriteTextTask(void* parameters)
{
uint8_t outbuf[512];
uint8_t inbuf[512];
int i;
//LED_LowLevel_Init();
//UART_LowLevel_Init();
SD_LowLevel_Init(); //Initialize PINS, vector table and SDIO interface
//UART_SendLine("Ready...Steady\n");
printf("SSD\n\r");
LEDOn(LED2);
//Initialize the SD
SD_Init(); //After return from this function sd is in transfer mode.
//UART_SendLine("GO\n");
printf("SDI\n\r");
LEDOff(LED2);
//Write a single block to the SD
for (i=0;i<512;i++) {
outbuf[i]='a';
}
//Single Block Test
SD_WriteSingleBlock(outbuf, 3000);
SD_ReadSingleBlock(inbuf, 3000);
//UART_SendLine("First Block\n");
//UART_Send(inbuf,512);
printf("\n\r%s", "First Block");
printf("\n\r%i,%i,%i,%i", (int)inbuf[0], (int)inbuf[1], (int)inbuf[2], (int)inbuf[3]);
SD_WriteSingleBlock(outbuf, 3001);
SD_ReadSingleBlock(inbuf, 3001);
//UART_SendLine("Second Block\n");
//UART_Send(inbuf,512);
printf("\n\r%s", "Second Block");
printf("\n\r%i,%i,%i,%i", (int)inbuf[0], (int)inbuf[1], (int)inbuf[2], (int)inbuf[3]);
//Multiple block
SD_StartMultipleBlockWrite(1000);
SD_WriteData(outbuf,500);
SD_WriteData(outbuf+50,50);
SD_WriteData(outbuf,500);
SD_StopMultipleBlockWrite();
SD_ReadSingleBlock(inbuf, 1000);
//UART_SendLine("Mult. Block 1\n");
//UART_Send(inbuf,512);
printf("\n\r%s", "Mult. Block 1");
printf("\n\r%i,%i,%i,%i", (int)inbuf[0], (int)inbuf[1], (int)inbuf[2], (int)inbuf[3]);
SD_WaitTransmissionEnd();
SD_ReadSingleBlock(inbuf, 1001);
//UART_SendLine("Mult. Block 2\n");
//UART_Send(inbuf,512);
printf("\n\r%s", "Mult. Block 2");
printf("\n\r%i,%i,%i,%i", (int)inbuf[0], (int)inbuf[1], (int)inbuf[2], (int)inbuf[3]);
// LEDOn(LED6);
portTickType lastExecutionTime = xTaskGetTickCount();
for (;;)
{
DelayUntilTime( &lastExecutionTime, SDCardReadWriteTextTaskT);
}
}
void main(void)
{
/******************************************************************************/
// Init
/******************************************************************************/
WDTCTL = WDTPW + WDTHOLD; // watchdog timer off
configureClocks();
init_PWM_TimerA();
hour = 6;
hour24 = 6;
minute = 1; // Set to one because with 0 no LED is on which is confusing.
second = 0;
state = SET_HOUR;
capsense=0;
LEDState=OFF;
timer=0;
DEMOPointer = 1;
P1REN = BIT6 + BIT7; // P1REN = pullup/down enable
P1OUT = BIT6 + BIT7; // P1OUT = define pullup
P1IFG = 0;
P1IES = BIT6 + BIT7; // set high to low transition for P1.6&7 interrupt
P1IE = BIT6 + BIT7; // enable P1.6&7 interrupt
P1REN |= BIT4 + BIT5; // P1REN = pullup/down enable
P1OUT |= BIT4 + BIT5; // P1OUT = pullup
P2REN |= BIT0; // P2REN = pullup/down enable
P2OUT |= BIT0; // P2OUT = pullup
P2OUT |= BIT3;
P2DIR |= BIT3;
P3REN = 0xFF; // Tie all P3 ports which are not available on N package
P3OUT = 0xFF;
WDTCTL = WDTPW + WDTTMSEL + WDTCNTCL + WDTSSEL; // watchdog counter mode, ACLK, /32768
IFG1 &= ~WDTIFG; // Clear WDT interrupt flag
IE1 |= WDTIE; // WDT interrupt enable
measure_count(); // Establish baseline capacitance
base_cnt = meas_cnt;
for(i=15; i>0; i--) // Repeat and avg base measurement
{
measure_count();
base_cnt = (meas_cnt+base_cnt)/2;
}
/******************************************************************************/
// Mainloop
/******************************************************************************/
__enable_interrupt();
while(1)
{
switch(state)
{
case NORMAL:
if (capsense)
{
CapTouch();
}
if (LEDState==ON)
{
HourView();
}
if (LEDState==ON && ((hour24>=7) && (hour24<18)))
{
MoonOff();
SunOn();
}
if (LEDState==ON && ((hour24>=18) || (hour24<7)))
{
SunOff();
MoonOn();
}
break;
case SET_HOUR:
capsense=OFF;
MoonOff();
SunOff();
HourView();
break;
case SET_MINUTE:
capsense=OFF;
MoonOff();
SunOff();
MinuteView();
break;
case DEMO:
if (DEMOPointer==1)
{
MoonOn();
SunOff();
}
if (DEMOPointer==7)
{
MoonOff();
SunOn();
}
j = 5000/DEMOPointer;
for(i=0;i<j;i++)
{
unsigned int OnTime = 10*DEMOPointer;
unsigned int OffTime = 120/DEMOPointer;
for(LEDPointer = 1;LEDPointer <= DEMOPointer;LEDPointer++)
{
LEDOn();
delay(OnTime);
LEDOff();
delay(OffTime);
}
}
DEMOPointer++;
if (DEMOPointer==13)
{
DEMOPointer=1;
}
break;
}
}
} // main()
/**
* @brief Main function.
*/
int main(void)
{
/* load settings and parameters */
global_data_reset_param_defaults();
global_data_reset();
/* init led */
LEDInit(LED_ACT);
LEDInit(LED_COM);
LEDInit(LED_ERR);
LEDOff(LED_ACT);
LEDOff(LED_COM);
LEDOff(LED_ERR);
/* enable FPU on Cortex-M4F core */
SCB_CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); /* set CP10 Full Access and set CP11 Full Access */
/* init clock */
if (SysTick_Config(SystemCoreClock / 100000))/*set timer to trigger interrupt every 10 microsecond */
{
/* capture clock error */
LEDOn(LED_ERR);
while (1);
}
/* init usb */
USBD_Init( &USB_OTG_dev,
USB_OTG_FS_CORE_ID,
&USR_desc,
&USBD_CDC_cb,
&USR_cb);
/* init mavlink */
communication_init();
/* enable image capturing */
enable_image_capture();
/* gyro config */
gyro_config();
/* init and clear fast image buffers */
for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++)
{
image_buffer_8bit_1[i] = 0;
image_buffer_8bit_2[i] = 0;
}
uint8_t * current_image = image_buffer_8bit_1;
uint8_t * previous_image = image_buffer_8bit_2;
uint8_t current_image1[64][64];
uint8_t current_image2[64][64];
uint8_t current_image3[64][64];
uint8_t current_image4[64][64];
uint8_t previous_image1[64][64];
uint8_t previous_image2[64][64];
uint8_t previous_image3[64][64];
uint8_t previous_image4[64][64];
for(int i=0;i<64;++i)
{
for(int j=0;j<64;++j)
{
current_image1[i][j]=0;
previous_image1[i][j]=0;
current_image2[i][j]=0;
previous_image2[i][j]=0;
current_image3[i][j]=0;
previous_image3[i][j]=0;
current_image4[i][j]=0;
previous_image4[i][j]=0;
}
}
uint8_t ourImage[OurSize];
for(int i=0;i<OurSize;++i)
{
ourImage[i]=0;
}
/* usart config*/
usart_init();
/* i2c config*/
i2c_init();
/* sonar config*/
float sonar_distance_filtered = 0.0f; // distance in meter
float sonar_distance_raw = 0.0f; // distance in meter
bool distance_valid = false;
sonar_config();
/* reset/start timers */
timer[TIMER_SONAR] = SONAR_TIMER_COUNT;
timer[TIMER_SYSTEM_STATE] = SYSTEM_STATE_COUNT;
timer[TIMER_RECEIVE] = SYSTEM_STATE_COUNT / 2;
timer[TIMER_PARAMS] = PARAMS_COUNT;
timer[TIMER_IMAGE] = global_data.param[PARAM_VIDEO_RATE];
/* variables */
uint32_t counter = 0;
uint8_t qual = 0;
/* bottom flow variables */
float pixel_flow_x = 0.0f;
float pixel_flow_y = 0.0f;
float pixel_flow_x_sum = 0.0f;
float pixel_flow_y_sum = 0.0f;
float velocity_x_sum = 0.0f;
float velocity_y_sum = 0.0f;
float velocity_x_lp = 0.0f;
float velocity_y_lp = 0.0f;
int valid_frame_count = 0;
int pixel_flow_count = 0;
float pixel_flow_x_lp = 0.0f;
float pixel_flow_y_lp = 0.0f;
static float accumulated_flow_x = 0;
static float accumulated_flow_y = 0;
static float accumulated_gyro_x = 0;
static float accumulated_gyro_y = 0;
static float accumulated_gyro_z = 0;
static uint16_t accumulated_framecount = 0;
static uint16_t accumulated_quality = 0;
static uint32_t integration_timespan = 0;
static uint32_t lasttime = 0;
uint32_t time_since_last_sonar_update= 0;
static float last_vel_x = 0;
static float last_vel_y = 0;
float vel_x = 0, vel_y = 0;
//Change
int count=1;
//int gyroCount=1;
uint8_t * tmp_image1 = *previous_image;
uint8_t * tmp_image2 = *current_image;
float x_temprate=0;
float y_temprate=0;
float z_temprate=0;
int defCount=3;
for (uint16_t pixel = 0; pixel < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; pixel++)
{
*(tmp_image1+pixel)=0;
*(tmp_image2+pixel)=0;
}
uint16_t image_sum1=0;
uint16_t image_sum2=0;
uint16_t image_sum3=0;
uint16_t image_sum4=0;
//EndChange
/* main loop */
while (1)
{
uint16_t image_size = global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT];
/* reset flow buffers if needed */
if(buffer_reset_needed)
{
buffer_reset_needed = 0;
for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++)
{
image_buffer_8bit_1[i] = 0;
image_buffer_8bit_2[i] = 0;
}
delay(500);
continue;
}
/* calibration routine */
if(global_data.param[PARAM_VIDEO_ONLY])
{
while(global_data.param[PARAM_VIDEO_ONLY])
{
dcmi_restart_calibration_routine();
/* waiting for first quarter of image */
while(get_frame_counter() < 2){}
dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1);
/* waiting for second quarter of image */
while(get_frame_counter() < 3){}
dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1);
/* waiting for all image parts */
while(get_frame_counter() < 4){}
send_calibration_image(&previous_image, ¤t_image);
if (global_data.param[PARAM_SYSTEM_SEND_STATE])
communication_system_state_send();
communication_receive_usb();
debug_message_send_one();
communication_parameter_send();
LEDToggle(LED_COM);
}
dcmi_restart_calibration_routine();
LEDOff(LED_COM);
}
//StartChange
if(count<=defCount)
{
if(count==1)
{
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
image_sum1+=*(previous_image+pixel);
}
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
*(tmp_image1+pixel)=(uint8_t)(*(previous_image+pixel));
}
}
else
{
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
image_sum2+=*(previous_image+pixel);
}
if(image_sum1<image_sum2)
{
image_sum1=image_sum2;
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
*(tmp_image1+pixel)=(uint8_t)(*(previous_image+pixel));
}
}
image_sum2=0;
}
}
if(count>defCount&&count<=2*defCount)
{
// uint16_t image_sum1=0;
// uint16_t image_sum2=0;
if(count==defCount+1)
{
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
image_sum3+=*(current_image+pixel);
}
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
*(tmp_image2+pixel)=(uint8_t)(*(previous_image+pixel));
}
}
else
{
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
image_sum4+=*(current_image+pixel);
}
if(image_sum3<image_sum4)
{
image_sum3=image_sum4;
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
*(tmp_image2+pixel)=(uint8_t)(*(previous_image+pixel));
}
}
image_sum4=0;
}
}
// if(count<=defCount)
// {
// for (uint16_t pixel = 0; pixel < image_size; pixel++)
// {
// *(tmp_image1+pixel)+=(uint8_t)(*(previous_image+pixel)/defCount);
// }
// }
// if(count>defCount&&count<=2*defCount)
// {
// for (uint16_t pixel = 0; pixel < image_size; pixel++)
// {
// *(tmp_image2+pixel)+=(uint8_t)(*(current_image+pixel)/defCount);
// }
//
// }
count++;
if(count==2*defCount+1)
{
/* new gyroscope data */
float x_rate_sensor, y_rate_sensor, z_rate_sensor;
int16_t gyro_temp;
gyro_read(&x_rate_sensor, &y_rate_sensor, &z_rate_sensor,&gyro_temp);
/* gyroscope coordinate transformation */
x_temprate += y_rate_sensor/(2*defCount); // change x and y rates
y_temprate += - x_rate_sensor/(2*defCount);
z_temprate += z_rate_sensor/(2*defCount); // z is correct
/* 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
/* get sonar data */
distance_valid = sonar_read(&sonar_distance_filtered, &sonar_distance_raw);
/* reset to zero for invalid distances */
if (!distance_valid) {
sonar_distance_filtered = 0.0f;
sonar_distance_raw = 0.0f;
}
float x_rate = x_temprate; // change x and y rates
float y_rate = - y_temprate;
float z_rate = z_temprate; // z is correct
x_temprate =0.0f;
y_temprate =0.0f;
z_temprate =0.0f;
// /* 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
//
// /* get sonar data */
// distance_valid = sonar_read(&sonar_distance_filtered, &sonar_distance_raw);
*previous_image=tmp_image1;
*current_image=tmp_image2;
count=1;
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
*(tmp_image1+pixel)=0;
*(tmp_image2+pixel)=0;
}
image_sum1=0;
image_sum2=0;
image_sum3=0;
image_sum4=0;
/* compute optical flow */
if(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM)
{
/* copy recent image to faster ram */
dma_copy_image_buffers(¤t_image, &previous_image, image_size, 1);
for(int i=0;i<64;++i)
{
for(int j=0;j<64;++j)
{
previous_image1[i][j]=*(previous_image+64*i+j);
current_image1[i][j]=*(current_image+64*i+j);
}
}
/* compute optical flow */
qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y);
/*
* real point P (X,Y,Z), image plane projection p (x,y,z), focal-length f, distance-to-scene Z
* x / f = X / Z
* y / f = Y / Z
*/
float flow_compx = pixel_flow_x / focal_length_px / (get_time_between_images() / 1000000.0f);
float flow_compy = pixel_flow_y / focal_length_px / (get_time_between_images() / 1000000.0f);
/* integrate velocity and output values only if distance is valid */
if (distance_valid)
{
/* calc velocity (negative of flow values scaled with distance) */
float new_velocity_x = - flow_compx * sonar_distance_filtered;
float new_velocity_y = - flow_compy * sonar_distance_filtered;
time_since_last_sonar_update = (get_boot_time_us()- get_sonar_measure_time());
if (qual > 0)
{
velocity_x_sum += new_velocity_x;
velocity_y_sum += new_velocity_y;
valid_frame_count++;
uint32_t deltatime = (get_boot_time_us() - lasttime);
integration_timespan += deltatime;
accumulated_flow_x += pixel_flow_y / focal_length_px * 1.0f; //rad axis swapped to align x flow around y axis
accumulated_flow_y += pixel_flow_x / focal_length_px * -1.0f;//rad
accumulated_gyro_x += x_rate * deltatime / 1000000.0f; //rad
accumulated_gyro_y += y_rate * deltatime / 1000000.0f; //rad
accumulated_gyro_z += z_rate * deltatime / 1000000.0f; //rad
accumulated_framecount++;
accumulated_quality += qual;
/* lowpass velocity output */
velocity_x_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_x +
(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
velocity_y_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_y +
(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
}
else
{
/* taking flow as zero */
velocity_x_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
velocity_y_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
}
}
else
{
/* taking flow as zero */
velocity_x_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
velocity_y_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
}
//update lasttime
lasttime = get_boot_time_us();
pixel_flow_x_sum += pixel_flow_x;
pixel_flow_y_sum += pixel_flow_y;
pixel_flow_count++;
}
counter++;
if(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM)
{
/* send bottom flow if activated */
float ground_distance = 0.0f;
if(global_data.param[PARAM_SONAR_FILTERED])
{
ground_distance = sonar_distance_filtered;
}
else
{
ground_distance = sonar_distance_raw;
}
if(global_data.param[PARAM_BOTTOM_FLOW_LP_FILTERED])
{
/* lowpass velocity output */
pixel_flow_x_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * pixel_flow_x +
(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * pixel_flow_x_lp;
pixel_flow_y_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * pixel_flow_y +
(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * pixel_flow_y_lp;
}
else
{
pixel_flow_x_lp = pixel_flow_x;
pixel_flow_y_lp = pixel_flow_y;
}
//update I2C transmitbuffer
if(valid_frame_count>0)
{
update_TX_buffer(pixel_flow_x_lp, pixel_flow_y_lp, velocity_x_sum/valid_frame_count, velocity_y_sum/valid_frame_count, qual,
ground_distance, x_rate, y_rate, z_rate, gyro_temp);
last_vel_x = velocity_x_sum/valid_frame_count;
last_vel_y = velocity_y_sum/valid_frame_count;
vel_x = last_vel_x;
vel_y = last_vel_y;
}
else
{
/*
update_TX_buffer(pixel_flow_x_lp, pixel_flow_y_lp, last_vel_x, last_vel_y, qual,
ground_distance, x_rate, y_rate, z_rate, gyro_temp);
vel_x = 0;
vel_y = 0;
*/
}
//serial mavlink + usb mavlink output throttled
if (counter % (uint32_t)global_data.param[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] == 0)//throttling factor
{
float flow_comp_m_x = 0.0f;
float flow_comp_m_y = 0.0f;
if(global_data.param[PARAM_BOTTOM_FLOW_LP_FILTERED])
{
flow_comp_m_x = velocity_x_lp;
flow_comp_m_y = velocity_y_lp;
}
else
{
if(valid_frame_count>0)
{
flow_comp_m_x = velocity_x_sum/valid_frame_count;
flow_comp_m_y = velocity_y_sum/valid_frame_count;
}
else
{
flow_comp_m_x = 0.0f;
flow_comp_m_y = 0.0f;
}
}
// send flow
mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
flow_comp_m_x, flow_comp_m_y, qual, ground_distance);
mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
integration_timespan, accumulated_flow_x, accumulated_flow_y,
accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z,
gyro_temp, accumulated_quality/accumulated_framecount,
time_since_last_sonar_update,ground_distance);
/*
mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
integration_timespan, accumulated_flow_x, accumulated_flow_y,
vel_x, vel_y, accumulated_gyro_z,
gyro_temp, accumulated_quality/accumulated_framecount,
time_since_last_sonar_update,ground_distance);
*/
/*
mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
integration_timespan, accumulated_flow_x, accumulated_flow_y,
last_vel_x, last_vel_y, accumulated_gyro_z,
gyro_temp, accumulated_quality/accumulated_framecount,
time_since_last_sonar_update,ground_distance);
*/
/* send approximate local position estimate without heading */
if (global_data.param[PARAM_SYSTEM_SEND_LPOS])
{
/* rough local position estimate for unit testing */
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 */
lpos.vx = 0;
lpos.vy = 0;
lpos.vz = 0;
} else {
/* toggling param allows user reset */
lpos.x = 0;
lpos.y = 0;
lpos.z = 0;
lpos.vx = 0;
lpos.vy = 0;
lpos.vz = 0;
}
if (global_data.param[PARAM_USB_SEND_FLOW])
{
mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
flow_comp_m_x, flow_comp_m_y, qual, ground_distance);
mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
integration_timespan, accumulated_flow_x, accumulated_flow_y,
accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z,
gyro_temp, accumulated_quality/accumulated_framecount,
time_since_last_sonar_update,ground_distance);
}
if(global_data.param[PARAM_USB_SEND_GYRO])
{
mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_us(), x_rate, y_rate, z_rate);
}
integration_timespan = 0;
accumulated_flow_x = 0;
accumulated_flow_y = 0;
accumulated_framecount = 0;
accumulated_quality = 0;
accumulated_gyro_x = 0;
accumulated_gyro_y = 0;
accumulated_gyro_z = 0;
velocity_x_sum = 0.0f;
velocity_y_sum = 0.0f;
pixel_flow_x_sum = 0.0f;
pixel_flow_y_sum = 0.0f;
valid_frame_count = 0;
pixel_flow_count = 0;
}
}
/* forward flow from other sensors */
if (counter % 2)
{
communication_receive_forward();
}
/* send system state, receive commands */
if (send_system_state_now)
{
/* every second */
if (global_data.param[PARAM_SYSTEM_SEND_STATE])
{
communication_system_state_send();
}
send_system_state_now = false;
}
/* receive commands */
if (receive_now)
{
/* test every second */
communication_receive();
communication_receive_usb();
receive_now = false;
}
/* sending debug msgs and requested parameters */
if (send_params_now)
{
debug_message_send_one();
communication_parameter_send();
send_params_now = false;
}
/* send local position estimate, for testing only, doesn't account for heading */
if (send_lpos_now)
{
if (global_data.param[PARAM_SYSTEM_SEND_LPOS])
{
mavlink_msg_local_position_ned_send(MAVLINK_COMM_2, timer_ms, lpos.x, lpos.y, lpos.z, lpos.vx, lpos.vy, lpos.vz);
}
send_lpos_now = false;
}
/* transmit raw 8-bit image */
if (global_data.param[PARAM_USB_SEND_VIDEO] && send_image_now)
{
/* get size of image to send */
uint16_t image_size_send;
uint16_t image_width_send;
uint16_t image_height_send;
image_size_send = image_size;
image_width_send = global_data.param[PARAM_IMAGE_WIDTH];
image_height_send = global_data.param[PARAM_IMAGE_HEIGHT];
mavlink_msg_data_transmission_handshake_send(
MAVLINK_COMM_2,
MAVLINK_DATA_STREAM_IMG_RAW8U,
image_size_send,
image_width_send,
image_height_send,
image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1,
MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN,
100);
LEDToggle(LED_COM);
uint16_t frame = 0;
for (frame = 0; frame < image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1; frame++)
{
mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) current_image)[frame * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]);
}
send_image_now = false;
}
else if (!global_data.param[PARAM_USB_SEND_VIDEO])
{
LEDOff(LED_COM);
}
}
//EndChange
}
}
