static void uavoRelayTask(void *parameters)
{
UAVObjEvent ev;
// Loop forever
while (1) {
PIOS_Thread_Sleep(50);
// Wait for queue message
if (PIOS_Queue_Receive(queue, &ev, 2) == true) {
// Process event. This calls transmitData
UAVTalkSendObject(uavTalkCon, ev.obj, ev.instId, false, 0);
}
// Process incoming data in sufficient chunks that we keep up
uint8_t serial_data[8];
uint16_t bytes_to_process;
bytes_to_process = PIOS_COM_ReceiveBuffer(uavorelay_com_id, serial_data, sizeof(serial_data), 0);
do {
bytes_to_process = PIOS_COM_ReceiveBuffer(uavorelay_com_id, serial_data, sizeof(serial_data), 0);
for (uint8_t i = 0; i < bytes_to_process; i++)
UAVTalkProcessInputStream(uavTalkCon,serial_data[i]);
} while (bytes_to_process > 0);
}
}
/**
* Telemetry transmit task. Processes queue events and periodic updates.
*/
static void telemetryRxTask(void *parameters)
{
uint32_t inputPort;
int32_t len;
// Task loop
while (1) {
#if defined(PIOS_INCLUDE_USB_HID)
// Determine input port (USB takes priority over telemetry port)
if (PIOS_USB_HID_CheckAvailable(0)) {
inputPort = PIOS_COM_TELEM_USB;
} else
#endif /* PIOS_INCLUDE_USB_HID */
{
inputPort = telemetryPort;
}
// Block until data are available
// TODO: Currently we periodically check the buffer for data, update once the PIOS_COM is made blocking
len = PIOS_COM_ReceiveBufferUsed(inputPort);
for (int32_t n = 0; n < len; ++n) {
UAVTalkProcessInputStream(PIOS_COM_ReceiveBuffer(inputPort));
}
vTaskDelay(5); // <- remove when blocking calls are implemented
}
}
/**
* Telemetry transmit task. Processes queue events and periodic updates.
*/
static void telemetryRxTask(void *parameters)
{
uint32_t inputPort;
// Task loop
while (1) {
#if defined(PIOS_INCLUDE_USB_HID)
// Determine input port (USB takes priority over telemetry port)
if (PIOS_USB_HID_CheckAvailable(0)) {
inputPort = PIOS_COM_TELEM_USB;
} else
#endif /* PIOS_INCLUDE_USB_HID */
{
inputPort = telemetryPort;
}
if (inputPort) {
// Block until data are available
uint8_t serial_data[1];
uint16_t bytes_to_process;
bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), 500);
if (bytes_to_process > 0) {
for (uint8_t i = 0; i < bytes_to_process; i++) {
UAVTalkProcessInputStream(serial_data[i]);
}
}
} else {
vTaskDelay(5);
}
}
}
uint8_t processRX() {
while (PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_USB) >= 63) {
if (PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB, mReceive_Buffer, 63, 0) == 63) {
processComand(mReceive_Buffer);
}
}
return TRUE;
}
int16_t SSP_SerialRead(void) {
if(PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_RF)>0)
{
return PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_RF);
}
else
return -1;
}
uint8_t processRX() {
while (PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_USB) >= 63) {
for (int32_t x = 0; x < 63; ++x) {
mReceive_Buffer[x] = PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB);
}
processComand(mReceive_Buffer);
}
return TRUE;
}
static void TaskHIDTest(void *pvParameters)
{
uint8_t byte;
uint8_t line_buffer[128];
uint16_t line_ix = 0;
for(;;)
{
/* HID Loopback Test */
#if 0
if(PIOS_COM_ReceiveBufferUsed(COM_USB_HID) != 0) {
byte = PIOS_COM_ReceiveBuffer(COM_USB_HID);
if(byte == '\r' || byte == '\n' || byte == 0) {
PIOS_COM_SendFormattedString(COM_USB_HID, "RX: %s\r", line_buffer);
PIOS_COM_SendFormattedString(COM_DEBUG_USART, "RX: %s\r", line_buffer);
line_ix = 0;
} else if(line_ix < sizeof(line_buffer)) {
line_buffer[line_ix++] = byte;
line_buffer[line_ix] = 0;
}
}
#endif
/* HID Loopback Test */
if(PIOS_COM_ReceiveBufferUsed(COM_USART2) != 0) {
byte = PIOS_COM_ReceiveBuffer(COM_USART2);
#if 0
if(byte == '\r' || byte == '\n' || byte == 0) {
PIOS_COM_SendFormattedString(COM_DEBUG_USART, "RX: %s\r", line_buffer);
line_ix = 0;
} else if(line_ix < sizeof(line_buffer)) {
line_buffer[line_ix++] = byte;
line_buffer[line_ix] = 0;
}
#endif
PIOS_COM_SendChar(COM_DEBUG_USART, (char)byte);
}
}
}
//! Parse incoming data while paused
static void ubx_cfg_pause_parse(uintptr_t gps_port, uint32_t delay_ticks)
{
struct GPS_RX_STATS gpsRxStats;
GPSPositionData gpsPosition;
uint8_t c;
uint32_t enterTime = PIOS_Thread_Systime();
while ((PIOS_Thread_Systime() - enterTime) < delay_ticks)
{
int32_t received = PIOS_COM_ReceiveBuffer(gps_port, &c, 1, 1);
if (received > 0)
parse_ubx_stream (c, gps_rx_buffer, &gpsPosition, &gpsRxStats);
}
}
uint8_t processRX() {
if (ProgPort == Usb) {
while (PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_USB) >= 63) {
for (int32_t x = 0; x < 63; ++x) {
mReceive_Buffer[x] = PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB);
}
processComand(mReceive_Buffer);
}
} else if (ProgPort == Serial) {
if (fifoBuf_getUsed(&ssp_buffer) >= 63) {
for (int32_t x = 0; x < 63; ++x) {
mReceive_Buffer[x] = fifoBuf_getByte(&ssp_buffer);
}
processComand(mReceive_Buffer);
}
}
return TRUE;
}
static void usb2ComBridgeTask(void * parameters)
{
/* Handle vcp -> usart direction */
volatile uint32_t tx_errors = 0;
while (1) {
uint32_t rx_bytes;
uint8_t usb2com_buf[BRIDGE_BUF_LEN];
rx_bytes = PIOS_COM_ReceiveBuffer(vcp_port, usb2com_buf, BRIDGE_BUF_LEN, 500);
if (rx_bytes > 0) {
/* Bytes available to transfer */
if (PIOS_COM_SendBuffer(usart_port, usb2com_buf, rx_bytes) != rx_bytes) {
/* Error on transmit */
tx_errors++;
}
}
}
}
/* long ReceiveBuffer(unsigned char *,unsigned int,unsigned long): receives buffer from picoc serial port */
void SystemReceiveBuffer(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
{
if ((PIOS_COM_PICOC) && (Param[0]->Val->Pointer != NULL)) {
uint8_t *buffer = Param[0]->Val->Pointer;
uint16_t buf_len = Param[1]->Val->UnsignedInteger;
uint32_t timeout = Param[2]->Val->UnsignedLongInteger;
ReturnValue->Val->LongInteger = 0;
while (buf_len > 0) {
uint16_t rc = PIOS_COM_ReceiveBuffer(PIOS_COM_PICOC, buffer, buf_len, 0);
if (rc > 0) {
buf_len -= rc;
buffer += rc;
ReturnValue->Val->LongInteger += rc;
} else if (timeout-- > 0) {
PIOS_Thread_Sleep(1);
} else {
return;
}
}
} else {
ReturnValue->Val->LongInteger = -1;
}
}
void trans_process(void)
{ // copy data from comm-port RX buffer to RF packet handler TX buffer, and from RF packet handler RX buffer to comm-port TX buffer
// ********************
// decide which comm-port we are using (usart or usb)
bool usb_comms = false; // TRUE if we are using the usb port for comms.
uint32_t comm_port = PIOS_COM_SERIAL; // default to using the usart comm-port
#if defined(PIOS_INCLUDE_USB)
if (PIOS_USB_CheckAvailable(0))
{ // USB comms is up, use the USB comm-port instead of the USART comm-port
usb_comms = true;
comm_port = PIOS_COM_TELEM_USB;
}
#endif
// ********************
// check to see if the local communication port has changed (usart/usb)
if (trans_previous_com_port == 0 && trans_previous_com_port != comm_port)
{ // the local communications port has changed .. remove any data in the buffers
trans_temp_buffer2_wr = 0;
}
else
if (usb_comms)
{ // we're using the USB for comms - keep the USART rx buffer empty
uint8_t c;
while (PIOS_COM_ReceiveBuffer(PIOS_COM_SERIAL, &c, 1, 0) > 0);
}
trans_previous_com_port = comm_port; // remember the current comm-port we are using
// ********************
uint16_t connection_index = 0; // the RF connection we are using
// ********************
// send the data received down the comm-port to the RF packet handler TX buffer
if (saved_settings.mode == MODE_NORMAL || saved_settings.mode == MODE_STREAM_TX)
{
// free space size in the RF packet handler tx buffer
uint16_t ph_num = ph_putData_free(connection_index);
// set the USART RTS handshaking line
if (!usb_comms)
{
if (ph_num < 32 || !ph_connected(connection_index))
SERIAL_RTS_CLEAR; // lower the USART RTS line - we don't have space in the buffer for anymore bytes
else
SERIAL_RTS_SET; // release the USART RTS line - we have space in the buffer for now bytes
}
else
SERIAL_RTS_SET; // release the USART RTS line
// limit number of bytes we will get to the size of the temp buffer
if (ph_num > sizeof(trans_temp_buffer1))
ph_num = sizeof(trans_temp_buffer1);
// copy data received down the comm-port into our temp buffer
register uint16_t bytes_saved = 0;
bytes_saved = PIOS_COM_ReceiveBuffer(comm_port, trans_temp_buffer1, ph_num, 0);
// put the received comm-port data bytes into the RF packet handler TX buffer
if (bytes_saved > 0)
{
trans_rx_timer = 0;
ph_putData(connection_index, trans_temp_buffer1, bytes_saved);
}
}
else
{ // empty the comm-ports rx buffer
uint8_t c;
while (PIOS_COM_ReceiveBuffer(comm_port, &c, 1, 0) > 0);
}
// ********************
// send the data received via the RF link out the comm-port
if (saved_settings.mode == MODE_NORMAL || saved_settings.mode == MODE_STREAM_RX)
{
if (trans_temp_buffer2_wr < sizeof(trans_temp_buffer2))
{
// get number of data bytes received via the RF link
uint16_t ph_num = ph_getData_used(connection_index);
// limit to how much space we have in the temp buffer
if (ph_num > sizeof(trans_temp_buffer2) - trans_temp_buffer2_wr)
ph_num = sizeof(trans_temp_buffer2) - trans_temp_buffer2_wr;
if (ph_num > 0)
{ // fetch the data bytes received via the RF link and save into our temp buffer
ph_num = ph_getData(connection_index, trans_temp_buffer2 + trans_temp_buffer2_wr, ph_num);
trans_temp_buffer2_wr += ph_num;
}
}
#if (defined(PIOS_COM_DEBUG) && (PIOS_COM_DEBUG == PIOS_COM_SERIAL))
if (!usb_comms)
{ // the serial-port is being used for debugging - don't send data down it
trans_temp_buffer2_wr = 0;
trans_tx_timer = 0;
return;
}
#endif
if (trans_temp_buffer2_wr > 0)
{ // we have data in our temp buffer that needs sending out the comm-port
if (usb_comms || (!usb_comms && GPIO_IN(SERIAL_CTS_PIN)))
{ // we are OK to send the data out the comm-port
// send the data out the comm-port
int32_t res = PIOS_COM_SendBufferNonBlocking(comm_port, trans_temp_buffer2, trans_temp_buffer2_wr); // this one doesn't work properly with USB :(
if (res >= 0)
{ // data was sent out the comm-port OK .. remove the sent data from the temp buffer
trans_temp_buffer2_wr = 0;
trans_tx_timer = 0;
}
else
{ // failed to send the data out the comm-port
#if defined(TRANS_DEBUG)
DEBUG_PRINTF("PIOS_COM_SendBuffer %d %d\r\n", trans_temp_buffer2_wr, res);
#endif
if (trans_tx_timer >= 5000)
trans_temp_buffer2_wr = 0; // seems we can't send our data for at least the last 5 seconds - delete it
}
}
}
}
else
{ // empty the buffer
trans_temp_buffer2_wr = 0;
trans_tx_timer = 0;
}
// ********************
}
static void loggingTask(void *parameters)
{
UAVObjEvent ev;
bool armed = false;
uint32_t now = PIOS_Thread_Systime();
#if defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC)
bool write_open = false;
bool read_open = false;
int32_t read_sector = 0;
uint8_t read_data[LOGGINGSTATS_FILESECTOR_NUMELEM];
#endif
// Get settings automatically for now on
LoggingSettingsConnectCopy(&settings);
LoggingStatsGet(&loggingData);
loggingData.BytesLogged = 0;
#if defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC)
if (destination_spi_flash)
{
loggingData.MinFileId = PIOS_STREAMFS_MinFileId(streamfs_id);
loggingData.MaxFileId = PIOS_STREAMFS_MaxFileId(streamfs_id);
}
#endif
if (settings.LogBehavior == LOGGINGSETTINGS_LOGBEHAVIOR_LOGONSTART) {
loggingData.Operation = LOGGINGSTATS_OPERATION_INITIALIZING;
} else {
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
}
LoggingStatsSet(&loggingData);
// Loop forever
while (1)
{
LoggingStatsGet(&loggingData);
// Check for change in armed state if logging on armed
if (settings.LogBehavior == LOGGINGSETTINGS_LOGBEHAVIOR_LOGONARM) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !armed) {
// Start logging because just armed
loggingData.Operation = LOGGINGSTATS_OPERATION_INITIALIZING;
armed = true;
LoggingStatsSet(&loggingData);
} else if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && armed) {
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
armed = false;
LoggingStatsSet(&loggingData);
}
}
switch (loggingData.Operation) {
case LOGGINGSTATS_OPERATION_FORMAT:
// Format the file system
#if defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC)
if (destination_spi_flash){
if (read_open || write_open) {
PIOS_STREAMFS_Close(streamfs_id);
read_open = false;
write_open = false;
}
PIOS_STREAMFS_Format(streamfs_id);
loggingData.MinFileId = PIOS_STREAMFS_MinFileId(streamfs_id);
loggingData.MaxFileId = PIOS_STREAMFS_MaxFileId(streamfs_id);
}
#endif /* defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC) */
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
LoggingStatsSet(&loggingData);
break;
case LOGGINGSTATS_OPERATION_INITIALIZING:
// Unregister all objects
UAVObjIterate(&unregister_object);
// Register objects to be logged
switch (settings.Profile) {
case LOGGINGSETTINGS_PROFILE_DEFAULT:
register_default_profile();
break;
case LOGGINGSETTINGS_PROFILE_CUSTOM:
UAVObjIterate(®ister_object);
break;
}
#if defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC)
if (destination_spi_flash){
// Close the file if it is open for reading
if (read_open) {
PIOS_STREAMFS_Close(streamfs_id);
read_open = false;
}
// Open the file if it is not open for writing
if (!write_open) {
if (PIOS_STREAMFS_OpenWrite(streamfs_id) != 0) {
loggingData.Operation = LOGGINGSTATS_OPERATION_ERROR;
continue;
} else {
write_open = true;
}
loggingData.MinFileId = PIOS_STREAMFS_MinFileId(streamfs_id);
loggingData.MaxFileId = PIOS_STREAMFS_MaxFileId(streamfs_id);
LoggingStatsSet(&loggingData);
}
}
else {
read_open = false;
write_open = true;
}
#endif /* defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC) */
// Write information at start of the log file
writeHeader();
// Log settings
if (settings.LogSettingsOnStart == LOGGINGSETTINGS_LOGSETTINGSONSTART_TRUE){
UAVObjIterate(&logSettings);
}
// Empty the queue
while(PIOS_Queue_Receive(logging_queue, &ev, 0))
LoggingStatsBytesLoggedSet(&written_bytes);
loggingData.Operation = LOGGINGSTATS_OPERATION_LOGGING;
LoggingStatsSet(&loggingData);
break;
case LOGGINGSTATS_OPERATION_LOGGING:
{
// Sleep between writing
PIOS_Thread_Sleep_Until(&now, LOGGING_PERIOD_MS);
// Log the objects registred to the shared queue
for (int i=0; i<LOGGING_QUEUE_SIZE; i++) {
if (PIOS_Queue_Receive(logging_queue, &ev, 0) == true) {
UAVTalkSendObjectTimestamped(uavTalkCon, ev.obj, ev.instId, false, 0);
}
else {
break;
}
}
LoggingStatsBytesLoggedSet(&written_bytes);
now = PIOS_Thread_Systime();
}
break;
case LOGGINGSTATS_OPERATION_DOWNLOAD:
#if defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC)
if (destination_spi_flash) {
if (!read_open) {
// Start reading
if (PIOS_STREAMFS_OpenRead(streamfs_id, loggingData.FileRequest) != 0) {
loggingData.Operation = LOGGINGSTATS_OPERATION_ERROR;
} else {
read_open = true;
read_sector = -1;
}
}
if (read_open && read_sector == loggingData.FileSectorNum) {
// Request received for same sector. Reupdate.
memcpy(loggingData.FileSector, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM);
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
} else if (read_open && (read_sector + 1) == loggingData.FileSectorNum) {
int32_t bytes_read = PIOS_COM_ReceiveBuffer(logging_com_id, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM, 1);
if (bytes_read < 0 || bytes_read > LOGGINGSTATS_FILESECTOR_NUMELEM) {
// close on error
loggingData.Operation = LOGGINGSTATS_OPERATION_ERROR;
PIOS_STREAMFS_Close(streamfs_id);
read_open = false;
} else if (bytes_read < LOGGINGSTATS_FILESECTOR_NUMELEM) {
// Check it has really run out of bytes by reading again
int32_t bytes_read2 = PIOS_COM_ReceiveBuffer(logging_com_id, &read_data[bytes_read], LOGGINGSTATS_FILESECTOR_NUMELEM - bytes_read, 1);
memcpy(loggingData.FileSector, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM);
if ((bytes_read + bytes_read2) < LOGGINGSTATS_FILESECTOR_NUMELEM) {
// indicate end of file
loggingData.Operation = LOGGINGSTATS_OPERATION_COMPLETE;
PIOS_STREAMFS_Close(streamfs_id);
read_open = false;
} else {
// Indicate sent
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
}
} else {
// Indicate sent
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
memcpy(loggingData.FileSector, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM);
}
read_sector = loggingData.FileSectorNum;
}
LoggingStatsSet(&loggingData);
}
#endif /* defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC) */
// fall-through to default case
default:
// Makes sure that we are not hogging the processor
PIOS_Thread_Sleep(10);
#if defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC)
if (destination_spi_flash) {
// Close the file if necessary
if (write_open) {
PIOS_STREAMFS_Close(streamfs_id);
loggingData.MinFileId = PIOS_STREAMFS_MinFileId(streamfs_id);
loggingData.MaxFileId = PIOS_STREAMFS_MaxFileId(streamfs_id);
LoggingStatsSet(&loggingData);
write_open = false;
}
}
#endif /* defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC) */
}
}
}
static void gpsTask(void *parameters)
{
portTickType xDelay = 100 / portTICK_RATE_MS;
uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;;
GPSPositionData GpsData;
#ifdef ENABLE_GPS_BINARY_GTOP
GTOP_BIN_init();
#else
uint8_t rx_count = 0;
bool start_flag = false;
bool found_cr = false;
int32_t gpsRxOverflow = 0;
#endif
#ifdef FULL_COLD_RESTART
// tell the GPS to do a FULL COLD restart
PIOS_COM_SendStringNonBlocking(gpsPort, "$PMTK104*37\r\n");
timeOfLastCommandMs = timeNowMs;
while (timeNowMs - timeOfLastCommandMs < 300) // delay for 300ms to let the GPS sort itself out
{
vTaskDelay(xDelay); // Block task until next update
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;;
}
#endif
#ifdef DISABLE_GPS_TRESHOLD
PIOS_COM_SendStringNonBlocking(gpsPort, "$PMTK397,0*23\r\n");
#endif
#ifdef ENABLE_GPS_BINARY_GTOP
// switch to GTOP binary mode
PIOS_COM_SendStringNonBlocking(gpsPort ,"$PGCMD,21,1*6F\r\n");
#endif
#ifdef ENABLE_GPS_ONESENTENCE_GTOP
// switch to single sentence mode
PIOS_COM_SendStringNonBlocking(gpsPort, "$PGCMD,21,2*6C\r\n");
#endif
#ifdef ENABLE_GPS_NMEA
// switch to NMEA mode
PIOS_COM_SendStringNonBlocking(gpsPort, "$PGCMD,21,3*6D\r\n");
#endif
numUpdates = 0;
numChecksumErrors = 0;
numParsingErrors = 0;
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
// Loop forever
while (1)
{
#ifdef ENABLE_GPS_BINARY_GTOP
// GTOP BINARY GPS mode
while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0)
{
int res = GTOP_BIN_update_position(PIOS_COM_ReceiveBuffer(gpsPort), &numChecksumErrors, &numParsingErrors);
if (res >= 0)
{
numUpdates++;
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
}
}
#else
// NMEA or SINGLE-SENTENCE GPS mode
// This blocks the task until there is something on the buffer
while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0)
{
char c = PIOS_COM_ReceiveBuffer(gpsPort);
// detect start while acquiring stream
if (!start_flag && (c == '$'))
{
start_flag = true;
found_cr = false;
rx_count = 0;
}
else
if (!start_flag)
continue;
if (rx_count >= sizeof(gps_rx_buffer))
{
// The buffer is already full and we haven't found a valid NMEA sentence.
// Flush the buffer and note the overflow event.
gpsRxOverflow++;
start_flag = false;
found_cr = false;
rx_count = 0;
}
else
{
gps_rx_buffer[rx_count] = c;
rx_count++;
}
// look for ending '\r\n' sequence
if (!found_cr && (c == '\r') )
found_cr = true;
else
if (found_cr && (c != '\n') )
found_cr = false; // false end flag
else
if (found_cr && (c == '\n') )
{
// The NMEA functions require a zero-terminated string
// As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n
gps_rx_buffer[rx_count-2] = 0;
// prepare to parse next sentence
start_flag = false;
found_cr = false;
rx_count = 0;
// Our rxBuffer must look like this now:
// [0] = '$'
// ... = zero or more bytes of sentence payload
// [end_pos - 1] = '\r'
// [end_pos] = '\n'
//
// Prepare to consume the sentence from the buffer
// Validate the checksum over the sentence
if (!NMEA_checksum(&gps_rx_buffer[1]))
{ // Invalid checksum. May indicate dropped characters on Rx.
PIOS_DEBUG_PinHigh(2);
++numChecksumErrors;
PIOS_DEBUG_PinLow(2);
}
else
{ // Valid checksum, use this packet to update the GPS position
if (!NMEA_update_position(&gps_rx_buffer[1])) {
PIOS_DEBUG_PinHigh(2);
++numParsingErrors;
PIOS_DEBUG_PinLow(2);
}
else
++numUpdates;
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
}
}
}
#endif
// Check for GPS timeout
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS)
{ // we have not received any valid GPS sentences for a while.
// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
GPSPositionGet(&GpsData);
GpsData.Status = GPSPOSITION_STATUS_NOGPS;
GPSPositionSet(&GpsData);
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
if ((timeNowMs - timeOfLastCommandMs) >= GPS_COMMAND_RESEND_TIMEOUT_MS)
{ // resend the command .. just incase the gps has only just been plugged in or the gps did not get our last command
timeOfLastCommandMs = timeNowMs;
#ifdef ENABLE_GPS_BINARY_GTOP
GTOP_BIN_init();
// switch to binary mode
PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,1*6F\r\n");
#endif
#ifdef ENABLE_GPS_ONESENTENCE_GTOP
// switch to single sentence mode
PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,2*6C\r\n");
#endif
#ifdef ENABLE_GPS_NMEA
// switch to NMEA mode
PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,3*6D\r\n");
#endif
#ifdef DISABLE_GPS_TRESHOLD
PIOS_COM_SendStringNonBlocking(gpsPort,"$PMTK397,0*23\r\n");
#endif
}
}
else
{ // we appear to be receiving GPS sentences OK, we've had an update
HomeLocationData home;
HomeLocationGet(&home);
GPSPositionGet(&GpsData);
if ((GpsData.Status == GPSPOSITION_STATUS_FIX3D) && (home.Set == HOMELOCATION_SET_FALSE))
setHomeLocation(&GpsData);
//criteria for GPS-OK taken from this post...
//http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
if ((GpsData.PDOP < 3.5) && (GpsData.Satellites >= 7))
AlarmsClear(SYSTEMALARMS_ALARM_GPS);
else
if (GpsData.Status == GPSPOSITION_STATUS_FIX3D)
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
else
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
}
// Block task until next update
vTaskDelay(xDelay);
}
}
static void loggingTask(void *parameters)
{
bool armed = false;
bool write_open = false;
bool read_open = false;
int32_t read_sector = 0;
uint8_t read_data[LOGGINGSTATS_FILESECTOR_NUMELEM];
//PIOS_STREAMFS_Format(streamfs_id);
LoggingStatsData loggingData;
LoggingStatsGet(&loggingData);
loggingData.BytesLogged = 0;
loggingData.MinFileId = PIOS_STREAMFS_MinFileId(streamfs_id);
loggingData.MaxFileId = PIOS_STREAMFS_MaxFileId(streamfs_id);
LoggingSettingsData settings;
LoggingSettingsGet(&settings);
if (settings.LogBehavior == LOGGINGSETTINGS_LOGBEHAVIOR_LOGONSTART) {
if (PIOS_STREAMFS_OpenWrite(streamfs_id) != 0) {
loggingData.Operation = LOGGINGSTATS_OPERATION_ERROR;
write_open = false;
} else {
loggingData.Operation = LOGGINGSTATS_OPERATION_LOGGING;
write_open = true;
}
} else {
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
}
LoggingStatsSet(&loggingData);
int i = 0;
// Loop forever
while (1) {
// Do not update anything at more than 40 Hz
PIOS_Thread_Sleep(20);
LoggingStatsGet(&loggingData);
// Check for change in armed state if logging on armed
LoggingSettingsGet(&settings);
if (settings.LogBehavior == LOGGINGSETTINGS_LOGBEHAVIOR_LOGONARM) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !armed) {
// Start logging because just armed
loggingData.Operation = LOGGINGSTATS_OPERATION_LOGGING;
armed = true;
LoggingStatsSet(&loggingData);
} else if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && armed) {
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
armed = false;
LoggingStatsSet(&loggingData);
}
}
// If currently downloading a log, close the file
if (loggingData.Operation == LOGGINGSTATS_OPERATION_LOGGING && read_open) {
PIOS_STREAMFS_Close(streamfs_id);
read_open = false;
}
if (loggingData.Operation == LOGGINGSTATS_OPERATION_LOGGING && !write_open) {
if (PIOS_STREAMFS_OpenWrite(streamfs_id) != 0) {
loggingData.Operation = LOGGINGSTATS_OPERATION_ERROR;
} else {
write_open = true;
}
loggingData.MinFileId = PIOS_STREAMFS_MinFileId(streamfs_id);
loggingData.MaxFileId = PIOS_STREAMFS_MaxFileId(streamfs_id);
LoggingStatsSet(&loggingData);
} else if (loggingData.Operation != LOGGINGSTATS_OPERATION_LOGGING && write_open) {
PIOS_STREAMFS_Close(streamfs_id);
loggingData.MinFileId = PIOS_STREAMFS_MinFileId(streamfs_id);
loggingData.MaxFileId = PIOS_STREAMFS_MaxFileId(streamfs_id);
LoggingStatsSet(&loggingData);
write_open = false;
}
switch (loggingData.Operation) {
case LOGGINGSTATS_OPERATION_LOGGING:
if (!write_open)
continue;
UAVTalkSendObjectTimestamped(uavTalkCon, AttitudeActualHandle(), 0, false, 0);
UAVTalkSendObjectTimestamped(uavTalkCon, AccelsHandle(), 0, false, 0);
UAVTalkSendObjectTimestamped(uavTalkCon, GyrosHandle(), 0, false, 0);
UAVTalkSendObjectTimestamped(uavTalkCon, MagnetometerHandle(), 0, false, 0);
if ((i % 10) == 0) {
UAVTalkSendObjectTimestamped(uavTalkCon, BaroAltitudeHandle(), 0, false, 0);
UAVTalkSendObjectTimestamped(uavTalkCon, GPSPositionHandle(), 0, false, 0);
}
if ((i % 50) == 1) {
UAVTalkSendObjectTimestamped(uavTalkCon, GPSTimeHandle(), 0, false, 0);
}
LoggingStatsBytesLoggedSet(&written_bytes);
break;
case LOGGINGSTATS_OPERATION_DOWNLOAD:
if (!read_open) {
// Start reading
if (PIOS_STREAMFS_OpenRead(streamfs_id, loggingData.FileRequest) != 0) {
loggingData.Operation = LOGGINGSTATS_OPERATION_ERROR;
} else {
read_open = true;
read_sector = -1;
}
}
if (read_open && read_sector == loggingData.FileSectorNum) {
// Request received for same sector. Reupdate.
memcpy(loggingData.FileSector, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM);
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
} else if (read_open && (read_sector + 1) == loggingData.FileSectorNum) {
int32_t bytes_read = PIOS_COM_ReceiveBuffer(logging_com_id, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM, 1);
if (bytes_read < 0 || bytes_read > LOGGINGSTATS_FILESECTOR_NUMELEM) {
// close on error
loggingData.Operation = LOGGINGSTATS_OPERATION_ERROR;
PIOS_STREAMFS_Close(streamfs_id);
read_open = false;
} else if (bytes_read < LOGGINGSTATS_FILESECTOR_NUMELEM) {
// Check it has really run out of bytes by reading again
int32_t bytes_read2 = PIOS_COM_ReceiveBuffer(logging_com_id, &read_data[bytes_read], LOGGINGSTATS_FILESECTOR_NUMELEM - bytes_read, 1);
memcpy(loggingData.FileSector, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM);
if ((bytes_read + bytes_read2) < LOGGINGSTATS_FILESECTOR_NUMELEM) {
// indicate end of file
loggingData.Operation = LOGGINGSTATS_OPERATION_COMPLETE;
PIOS_STREAMFS_Close(streamfs_id);
read_open = false;
} else {
// Indicate sent
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
}
} else {
// Indicate sent
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
memcpy(loggingData.FileSector, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM);
}
read_sector = loggingData.FileSectorNum;
}
LoggingStatsSet(&loggingData);
}
i++;
}
}
static void gpsTask(void *parameters)
{
GPSPositionData gpsposition;
uint32_t timeOfLastUpdateMs = 0;
uint32_t timeOfConfigAttemptMs = 0;
uint8_t gpsProtocol;
#ifdef PIOS_GPS_PROVIDES_AIRSPEED
gps_airspeed_initialize();
#endif
GPSPositionGet(&gpsposition);
// Wait for power to stabilize before talking to external devices
PIOS_Thread_Sleep(1000);
// Loop forever
while (1) {
uint32_t xDelay = GPS_COM_TIMEOUT_MS;
uint32_t loopTimeMs = PIOS_Thread_Systime();
// XXX TODO: also on modulesettings change..
if (!timeOfConfigAttemptMs) {
ModuleSettingsGPSDataProtocolGet(&gpsProtocol);
gpsConfigure(gpsProtocol);
timeOfConfigAttemptMs = PIOS_Thread_Systime();
continue;
}
uint8_t c;
// This blocks the task until there is something on the buffer
while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0)
{
int res;
switch (gpsProtocol) {
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
case MODULESETTINGS_GPSDATAPROTOCOL_NMEA:
res = parse_nmea_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats);
break;
#endif
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
case MODULESETTINGS_GPSDATAPROTOCOL_UBX:
res = parse_ubx_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats);
break;
#endif
default:
res = NO_PARSER; // this should not happen
break;
}
if (res == PARSER_COMPLETE) {
timeOfLastUpdateMs = loopTimeMs;
}
xDelay = 0; // For now on, don't block / wait,
// but consume what we can from the fifo
}
// Check for GPS timeout
if ((loopTimeMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) {
// we have not received any valid GPS sentences for a while.
// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
uint8_t status = GPSPOSITION_STATUS_NOGPS;
GPSPositionStatusSet(&status);
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
/* Don't reinitialize too often. */
if ((loopTimeMs - timeOfConfigAttemptMs) >= GPS_TIMEOUT_MS) {
timeOfConfigAttemptMs = 0; // reinit next loop
}
} else {
// we appear to be receiving GPS sentences OK, we've had an update
//criteria for GPS-OK taken from this post
if (gpsposition.PDOP < 3.5f &&
gpsposition.Satellites >= 7 &&
(gpsposition.Status == GPSPOSITION_STATUS_FIX3D ||
gpsposition.Status == GPSPOSITION_STATUS_DIFF3D)) {
AlarmsClear(SYSTEMALARMS_ALARM_GPS);
} else if (gpsposition.Status == GPSPOSITION_STATUS_FIX3D ||
gpsposition.Status == GPSPOSITION_STATUS_DIFF3D)
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
else
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
}
}
}
static void gpsTask(void *parameters)
{
portTickType xDelay = MS2TICKS(GPS_COM_TIMEOUT_MS);
uint32_t timeNowMs = TICKS2MS(xTaskGetTickCount());
GPSPositionData gpsposition;
uint8_t gpsProtocol;
ModuleSettingsGPSDataProtocolGet(&gpsProtocol);
#if defined(PIOS_GPS_PROVIDES_AIRSPEED)
gps_airspeed_initialize();
#endif
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
#if !defined(PIOS_GPS_MINIMAL)
switch (gpsProtocol) {
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
case MODULESETTINGS_GPSDATAPROTOCOL_UBX:
{
uint8_t gpsAutoConfigure;
ModuleSettingsGPSAutoConfigureGet(&gpsAutoConfigure);
if (gpsAutoConfigure == MODULESETTINGS_GPSAUTOCONFIGURE_TRUE) {
// Wait for power to stabilize before talking to external devices
vTaskDelay(MS2TICKS(1000));
// Runs through a number of possible GPS baud rates to
// configure the ublox baud rate. This uses a NMEA string
// so could work for either UBX or NMEA actually. This is
// somewhat redundant with updateSettings below, but that
// is only called on startup and is not an issue.
ModuleSettingsGPSSpeedOptions baud_rate;
ModuleSettingsGPSSpeedGet(&baud_rate);
ubx_cfg_set_baudrate(gpsPort, baud_rate);
vTaskDelay(MS2TICKS(1000));
ubx_cfg_send_configuration(gpsPort, gps_rx_buffer);
}
}
break;
#endif
}
#endif /* PIOS_GPS_MINIMAL */
GPSPositionGet(&gpsposition);
// Loop forever
while (1)
{
uint8_t c;
// This blocks the task until there is something on the buffer
while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0)
{
int res;
switch (gpsProtocol) {
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
case MODULESETTINGS_GPSDATAPROTOCOL_NMEA:
res = parse_nmea_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats);
break;
#endif
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
case MODULESETTINGS_GPSDATAPROTOCOL_UBX:
res = parse_ubx_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats);
break;
#endif
default:
res = NO_PARSER; // this should not happen
break;
}
if (res == PARSER_COMPLETE) {
timeNowMs = TICKS2MS(xTaskGetTickCount());
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
}
}
// Check for GPS timeout
timeNowMs = TICKS2MS(xTaskGetTickCount());
if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) {
// we have not received any valid GPS sentences for a while.
// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
uint8_t status = GPSPOSITION_STATUS_NOGPS;
GPSPositionStatusSet(&status);
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
} else {
// we appear to be receiving GPS sentences OK, we've had an update
//criteria for GPS-OK taken from this post
if (gpsposition.PDOP < 3.5f &&
gpsposition.Satellites >= 7 &&
(gpsposition.Status == GPSPOSITION_STATUS_FIX3D ||
gpsposition.Status == GPSPOSITION_STATUS_DIFF3D)) {
AlarmsClear(SYSTEMALARMS_ALARM_GPS);
} else if (gpsposition.Status == GPSPOSITION_STATUS_FIX3D ||
gpsposition.Status == GPSPOSITION_STATUS_DIFF3D)
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
else
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
}
}
}
/**
* Telemetry transmit task. Processes queue events and periodic updates.
*/
static void telemetryRxTask(void *parameters)
{
uint32_t inputPort;
uint8_t c;
// Task loop
while (1) {
#if defined(PIOS_INCLUDE_USB_HID)
// Determine input port (USB takes priority over telemetry port)
if (PIOS_USB_HID_CheckAvailable(0)) {
inputPort = PIOS_COM_TELEM_USB;
} else
#endif /* PIOS_INCLUDE_USB_HID */
{
inputPort = telemetryPort;
}
mavlink_channel_t mavlink_chan = MAVLINK_COMM_0;
// Block until a byte is available
PIOS_COM_ReceiveBuffer(inputPort, &c, 1, portMAX_DELAY);
// And process it
if (mavlink_parse_char(mavlink_chan, c, &rx_msg, &rx_status))
{
// Handle packet with waypoint component
mavlink_wpm_message_handler(&rx_msg);
// Handle packet with parameter component
mavlink_pm_message_handler(mavlink_chan, &rx_msg);
switch (rx_msg.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
// Check if this is the gcs
mavlink_heartbeat_t beat;
mavlink_msg_heartbeat_decode(&rx_msg, &beat);
if (beat.type == MAV_TYPE_GCS)
{
// Got heartbeat from the GCS, we're good!
lastOperatorHeartbeat = xTaskGetTickCount() * portTICK_RATE_MS;
}
}
break;
case MAVLINK_MSG_ID_SET_MODE:
{
mavlink_set_mode_t mode;
mavlink_msg_set_mode_decode(&rx_msg, &mode);
// Check if this system should change the mode
if (mode.target_system == mavlink_system.sysid)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
switch (mode.base_mode)
{
case MAV_MODE_MANUAL_ARMED:
{
flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
}
break;
case MAV_MODE_MANUAL_DISARMED:
{
flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
}
break;
case MAV_MODE_PREFLIGHT:
{
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
}
break;
case MAV_MODE_STABILIZE_ARMED:
{
flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED1;
flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
}
break;
case MAV_MODE_GUIDED_ARMED:
{
flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED2;
flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
}
break;
case MAV_MODE_AUTO_ARMED:
{
flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED3;
flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
}
break;
}
bool newHilEnabled = (mode.base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL);
if (newHilEnabled != hilEnabled)
{
if (newHilEnabled)
{
// READ-ONLY flag write to ActuatorCommand
UAVObjMetadata meta;
UAVObjHandle handle = ActuatorCommandHandle();
UAVObjGetMetadata(handle, &meta);
meta.access = ACCESS_READONLY;
UAVObjSetMetadata(handle, &meta);
mavlink_missionlib_send_gcs_string("ENABLING HIL SIMULATION");
mavlink_missionlib_send_gcs_string("+++++++++++++++++++++++");
mavlink_missionlib_send_gcs_string("BLOCKING ALL ACTUATORS");
}
else
{
// READ-ONLY flag write to ActuatorCommand
UAVObjMetadata meta;
UAVObjHandle handle = ActuatorCommandHandle();
UAVObjGetMetadata(handle, &meta);
meta.access = ACCESS_READWRITE;
UAVObjSetMetadata(handle, &meta);
mavlink_missionlib_send_gcs_string("DISABLING HIL SIMULATION");
mavlink_missionlib_send_gcs_string("+++++++++++++++++++++++");
mavlink_missionlib_send_gcs_string("ACTIVATING ALL ACTUATORS");
}
}
hilEnabled = newHilEnabled;
FlightStatusSet(&flightStatus);
// Check HIL
bool hilEnabled = (mode.base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL);
enableHil(hilEnabled);
}
}
break;
case MAVLINK_MSG_ID_HIL_STATE:
{
if (hilEnabled)
{
mavlink_hil_state_t hil;
mavlink_msg_hil_state_decode(&rx_msg, &hil);
// Write GPSPosition
GPSPositionData gps;
GPSPositionGet(&gps);
gps.Altitude = hil.alt/10;
gps.Latitude = hil.lat/10;
gps.Longitude = hil.lon/10;
GPSPositionSet(&gps);
// Write PositionActual
PositionActualData pos;
PositionActualGet(&pos);
// FIXME WRITE POSITION HERE
PositionActualSet(&pos);
// Write AttitudeActual
AttitudeActualData att;
AttitudeActualGet(&att);
att.Roll = hil.roll;
att.Pitch = hil.pitch;
att.Yaw = hil.yaw;
// FIXME
//att.RollSpeed = hil.rollspeed;
//att.PitchSpeed = hil.pitchspeed;
//att.YawSpeed = hil.yawspeed;
// Convert to quaternion formulation
RPY2Quaternion(&attitudeActual.Roll, &attitudeActual.q1);
// Write AttitudeActual
AttitudeActualSet(&att);
// Write AttitudeRaw
AttitudeRawData raw;
AttitudeRawGet(&raw);
raw.gyros[0] = hil.rollspeed;
raw.gyros[1] = hil.pitchspeed;
raw.gyros[2] = hil.yawspeed;
raw.accels[0] = hil.xacc;
raw.accels[1] = hil.yacc;
raw.accels[2] = hil.zacc;
// raw.magnetometers[0] = hil.xmag;
// raw.magnetometers[0] = hil.ymag;
// raw.magnetometers[0] = hil.zmag;
AttitudeRawSet(&raw);
}
}
break;
case MAVLINK_MSG_ID_COMMAND_LONG:
{
// FIXME Implement
}
break;
}
}
}
}
