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; } } } }