/** * @brief Initialize DCMI DMA and enable image capturing */ void enable_image_capture(void) { dcmi_clock_init(); dcmi_hw_init(); dcmi_dma_init(global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]); mt9v034_context_configuration(); dcmi_dma_enable(); }
void handle_mavlink_message(mavlink_channel_t chan, mavlink_message_t* msg) { /* all messages from usart2 are directly forwarded */ if(chan == MAVLINK_COMM_1) { uint8_t buf[MAVLINK_MAX_PACKET_LEN]; uint32_t len; // Copy to USART 3 len = mavlink_msg_to_send_buffer(buf, msg); mavlink_send_uart_bytes(MAVLINK_COMM_0, buf, len); if(global_data.param[PARAM_USB_SEND_FORWARD]) mavlink_send_uart_bytes(MAVLINK_COMM_2, buf, len); return; } /* forwarded received messages from usb and usart3 to usart2 */ uint8_t buf[MAVLINK_MAX_PACKET_LEN]; uint32_t len; /* copy to usart2 */ len = mavlink_msg_to_send_buffer(buf, msg); for (int i = 0; i < len; i++) { usart2_tx_ringbuffer_push(buf, len); } /* handling messages */ switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { mavlink_param_request_read_t set; mavlink_msg_param_request_read_decode(msg, &set); /* Check if this message is for this system */ if ((uint8_t) set.target_system == (uint8_t) global_data.param[PARAM_SYSTEM_ID] && (uint8_t) set.target_component == (uint8_t) global_data.param[PARAM_COMPONENT_ID]) { char* key = (char*) set.param_id; if (set.param_id[0] == '\0') { /* Choose parameter based on index */ if (set.param_index < ONBOARD_PARAM_COUNT) { /* Report back value */ mavlink_msg_param_value_send(chan, global_data.param_name[set.param_index], global_data.param[set.param_index], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, set.param_index); } } else { for (int i = 0; i < ONBOARD_PARAM_COUNT; i++) { bool match = true; for (int j = 0; j < ONBOARD_PARAM_NAME_LENGTH; j++) { /* Compare */ if (((char) (global_data.param_name[i][j])) != (char) (key[j])) { match = false; } /* End matching if null termination is reached */ if (((char) global_data.param_name[i][j]) == '\0') { break; } } /* Check if matched */ if (match) { /* Report back value */ mavlink_msg_param_value_send(chan, global_data.param_name[i], global_data.param[i], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, m_parameter_i); } } } } } break; case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { /* Start sending parameters */ m_parameter_i = 0; } break; case MAVLINK_MSG_ID_PARAM_SET: { mavlink_param_set_t set; mavlink_msg_param_set_decode(msg, &set); /* Check if this message is for this system */ if ((uint8_t) set.target_system == (uint8_t) global_data.param[PARAM_SYSTEM_ID] && (uint8_t) set.target_component == (uint8_t) global_data.param[PARAM_COMPONENT_ID]) { char* key = (char*) set.param_id; for (int i = 0; i < ONBOARD_PARAM_COUNT; i++) { bool match = true; for (int j = 0; j < ONBOARD_PARAM_NAME_LENGTH; j++) { /* Compare */ if (((char) (global_data.param_name[i][j])) != (char) (key[j])) { match = false; } /* End matching if null termination is reached */ if (((char) global_data.param_name[i][j]) == '\0') { break; } } /* Check if matched */ if (match) { /* Only write and emit changes if there is actually a difference * AND only write if new value is NOT "not-a-number" * AND is NOT infinity */ if (global_data.param[i] != set.param_value && !isnan(set.param_value) && !isinf(set.param_value) && global_data.param_access[i]) { global_data.param[i] = set.param_value; /* handle sensor position */ if(i == PARAM_SENSOR_POSITION) { set_sensor_position_settings((uint8_t) set.param_value); mt9v034_context_configuration(); dma_reconfigure(); buffer_reset(); } /* handle low light mode and noise correction */ else if(i == PARAM_IMAGE_LOW_LIGHT || i == PARAM_IMAGE_ROW_NOISE_CORR) { mt9v034_context_configuration(); dma_reconfigure(); buffer_reset(); } /* handle calibration on/off */ else if(i == PARAM_CALIBRATION_ON) { mt9v034_set_context(); dma_reconfigure(); buffer_reset(); if(global_data.param[PARAM_CALIBRATION_ON]) debug_string_message_buffer("Calibration Mode On"); else debug_string_message_buffer("Calibration Mode Off"); } /* handle sensor position */ else if(i == PARAM_GYRO_SENSITIVITY_DPS) { l3gd20_config(); } else { debug_int_message_buffer("Parameter received, param id =", i); } /* report back new value */ mavlink_msg_param_value_send(MAVLINK_COMM_0, global_data.param_name[i], global_data.param[i], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, m_parameter_i); mavlink_msg_param_value_send(MAVLINK_COMM_2, global_data.param_name[i], global_data.param[i], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, m_parameter_i); } else { /* send back current value because it is not accepted or not write access*/ mavlink_msg_param_value_send(MAVLINK_COMM_0, global_data.param_name[i], global_data.param[i], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, m_parameter_i); mavlink_msg_param_value_send(MAVLINK_COMM_2, global_data.param_name[i], global_data.param[i], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, m_parameter_i); } } } } } break; case MAVLINK_MSG_ID_PING: { mavlink_ping_t ping; mavlink_msg_ping_decode(msg, &ping); if (ping.target_system == 0 && ping.target_component == 0) { /* Respond to ping */ uint64_t r_timestamp = get_boot_time_ms() * 1000; mavlink_msg_ping_send(chan, ping.seq, msg->sysid, msg->compid, r_timestamp); } } break; default: break; } }