/**
* 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_magnetometer(Picoc *pc)
{
const char *definition = "typedef struct {"
"float x;"
"float y;"
"float z;"
"} MagnetometerData;";
PicocParse(pc, "mylib", definition, strlen(definition), TRUE, TRUE, FALSE, FALSE);
if (MagnetometerHandle() == NULL)
ProgramFailNoParser(pc, "no magnetometer");
}
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++;
}
}
void MagnetometerzGet( float *Newz )
{
UAVObjGetDataField(MagnetometerHandle(), (void*)Newz, offsetof( MagnetometerData, z), sizeof(float));
}
void MagnetometeryGet( float *Newy )
{
UAVObjGetDataField(MagnetometerHandle(), (void*)Newy, offsetof( MagnetometerData, y), sizeof(float));
}
void MagnetometerxGet( float *Newx )
{
UAVObjGetDataField(MagnetometerHandle(), (void*)Newx, offsetof( MagnetometerData, x), sizeof(float));
}
