/** * Register objects for the default logging profile */ static void register_default_profile() { // For the default profile, we limit things to 100Hz (for now) uint16_t min_period = MAX(get_minimum_logging_period(), 10); // Objects for which we log all changes (use 100Hz to limit max data rate) UAVObjConnectCallbackThrottled(FlightStatusHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 10); UAVObjConnectCallbackThrottled(SystemAlarmsHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 10); if (WaypointActiveHandle()) { UAVObjConnectCallbackThrottled(WaypointActiveHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 10); } if (SystemIdentHandle()){ UAVObjConnectCallbackThrottled(SystemIdentHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 10); } // Log fast UAVObjConnectCallbackThrottled(AccelsHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, min_period); UAVObjConnectCallbackThrottled(GyrosHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, min_period); // Log a bit slower UAVObjConnectCallbackThrottled(AttitudeActualHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 5 * min_period); UAVObjConnectCallbackThrottled(MagnetometerHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 5 * min_period); UAVObjConnectCallbackThrottled(ManualControlCommandHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 5 * min_period); UAVObjConnectCallbackThrottled(ActuatorDesiredHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 5 * min_period); UAVObjConnectCallbackThrottled(StabilizationDesiredHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 5 * min_period); // Log slow if (FlightBatteryStateHandle()) { UAVObjConnectCallbackThrottled(FlightBatteryStateHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 10 * min_period); } if (BaroAltitudeHandle()) { UAVObjConnectCallbackThrottled(BaroAltitudeHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 10 * min_period); } if (AirspeedActualHandle()) { UAVObjConnectCallbackThrottled(AirspeedActualHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 10 * min_period); } if (GPSPositionHandle()) { UAVObjConnectCallbackThrottled(GPSPositionHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 10 * min_period); } if (PositionActualHandle()) { UAVObjConnectCallbackThrottled(PositionActualHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 10 * min_period); } if (VelocityActualHandle()) { UAVObjConnectCallbackThrottled(VelocityActualHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 10 * min_period); } // Log very slow if (GPSTimeHandle()) { UAVObjConnectCallbackThrottled(GPSTimeHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 50 * min_period); } // Log very very slow if (GPSSatellitesHandle()) { UAVObjConnectCallbackThrottled(GPSSatellitesHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 500 * min_period); } }
/* this is called when the header file is included */ void PlatformLibrarySetup_attitudeactual(Picoc *pc) { #ifndef NO_FP const char *definition = "typedef struct {" "float Roll;" "float Pitch;" "float Yaw;" "} AttitudeActualData;"; PicocParse(pc, "mylib", definition, strlen(definition), TRUE, TRUE, FALSE, FALSE); #endif if (AttitudeActualHandle() == NULL) ProgramFailNoParser(pc, "no attitudeactual"); }
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++; } }
/* this is called when the header file is included */ void PlatformLibrarySetup_attitudeactual(Picoc *pc) { if (AttitudeActualHandle() == NULL) ProgramFailNoParser(pc, "no attitudeactual"); }
void AttitudeActualYawGet( float *NewYaw ) { UAVObjGetDataField(AttitudeActualHandle(), (void*)NewYaw, offsetof( AttitudeActualData, Yaw), sizeof(float)); }
void AttitudeActualPitchGet( float *NewPitch ) { UAVObjGetDataField(AttitudeActualHandle(), (void*)NewPitch, offsetof( AttitudeActualData, Pitch), sizeof(float)); }
void AttitudeActualRollGet( float *NewRoll ) { UAVObjGetDataField(AttitudeActualHandle(), (void*)NewRoll, offsetof( AttitudeActualData, Roll), sizeof(float)); }
void AttitudeActualq4Get( float *Newq4 ) { UAVObjGetDataField(AttitudeActualHandle(), (void*)Newq4, offsetof( AttitudeActualData, q4), sizeof(float)); }
void AttitudeActualq3Set( float *Newq3 ) { UAVObjSetDataField(AttitudeActualHandle(), (void*)Newq3, offsetof( AttitudeActualData, q3), sizeof(float)); }