/**
* @brief Configures Gyro
*/
void gyro_config()
{
/* spi first */
spi_config();
/* gyro */
l3gd20_config();
}
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;
}
}
