int gps_GetData (GPS_Data *gpsdata)
{
if (!gps_init)
ublox5Init();
//DEBUG
if (!gps_parse())
{
//printf("no response from gps\n\r");
gps_gga.newdata = 0;
*gpsdata = gps_gga;
return 0;
} else {
gps_gga.newdata = 1;
*gpsdata = gps_gga; //Copy current data set over to response
return 1;
}
}
/**
* @brief Receive communication packets and handle them
*
* This function decodes packets on the protocol level and also handles
* their value by calling the appropriate functions.
*/
void communication_receive(void)
{
mavlink_message_t msg;
mavlink_status_t status =
{ 0 };
status.packet_rx_drop_count = 0;
// COMMUNICATION WITH ONBOARD COMPUTER
while (uart0_char_available())
{
uint8_t c = uart0_get_char();
if (global_data.state.uart0mode == UART_MODE_MAVLINK)
{
// Try to get a new message
if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status))
{
// Handle message
handle_mavlink_message(MAVLINK_COMM_0, &msg);
}
}
else if (global_data.state.uart0mode == UART_MODE_BYTE_FORWARD)
{
uart1_transmit(c);
}
// And get the next one
}
// Update global packet drops counter
global_data.comm.uart0_rx_drop_count += status.packet_rx_drop_count;
global_data.comm.uart0_rx_success_count += status.packet_rx_success_count;
status.packet_rx_drop_count = 0;
// COMMUNICATION WITH EXTERNAL COMPUTER
while (uart1_char_available())
{
uint8_t c = uart1_get_char();
// Check if this link is used for MAVLink or GPS
if (global_data.state.uart1mode == UART_MODE_MAVLINK)
{
//uart0_transmit((unsigned char)c);
// Try to get a new message
if (mavlink_parse_char(MAVLINK_COMM_1, c, &msg, &status))
{
// Handle message
handle_mavlink_message(MAVLINK_COMM_1, &msg);
}
}
else if (global_data.state.uart1mode == UART_MODE_GPS)
{
if (global_data.state.gps_mode == 10)
{
static uint8_t gps_i = 0;
static char gps_chars[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
if (c == '$' || gps_i == MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN
- 1)
{
gps_i = 0;
char gps_chars_buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
strncpy(gps_chars_buf, gps_chars,
MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN);
debug_message_buffer(gps_chars_buf);
}
gps_chars[gps_i++] = c;
}
if (gps_parse(c))
{
// New GPS data received
//debug_message_buffer("RECEIVED NEW GPS DATA");
parse_gps_msg();
if (gps_lat == 0)
{
global_data.state.gps_ok = 0;
//debug_message_buffer("GPS Signal Lost");
}
else
{
global_data.state.gps_ok = 1;
mavlink_msg_gps_raw_send(
global_data.param[PARAM_SEND_DEBUGCHAN],
sys_time_clock_get_unix_time(), gps_mode, gps_lat
/ 1e7f, gps_lon / 1e7f, gps_alt / 100.0f,
0.0f, 0.0f, gps_gspeed / 100.0f, gps_course / 10.0f);
}
// // Output satellite info
// for (int i = 0; i < gps_nb_channels; i++)
// {
// mavlink_msg_gps_status_send(global_data.param[PARAM_SEND_DEBUGCHAN], gps_numSV, gps_svinfos[i].svid, gps_satellite_used(gps_svinfos[i].qi), gps_svinfos[i].elev, ((gps_svinfos[i].azim/360.0f)*255.0f), gps_svinfos[i].cno);
// }
}
}
else if (global_data.state.uart1mode == UART_MODE_BYTE_FORWARD)
{
uart0_transmit(c);
led_toggle(LED_YELLOW);
}
// And get the next one
}
// Update global packet drops counter
global_data.comm.uart0_rx_drop_count += status.packet_rx_drop_count;
global_data.comm.uart0_rx_success_count += status.packet_rx_success_count;
status.packet_rx_drop_count = 0;
}
void gps_step()
{
while (!gps_uart.isRxBufferEmpty())
gps_parse(&gps_uart);
}
