/**
* Initialise the module. Called before the start function
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AttitudeInitialize(void)
{
AttitudeActualInitialize();
AirspeedActualInitialize();
AirspeedSensorInitialize();
AttitudeSettingsInitialize();
PositionActualInitialize();
VelocityActualInitialize();
RevoSettingsInitialize();
RevoCalibrationInitialize();
EKFConfigurationInitialize();
EKFStateVarianceInitialize();
FlightStatusInitialize();
// Initialize this here while we aren't setting the homelocation in GPS
HomeLocationInitialize();
// Initialize quaternion
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
attitude.q1 = 1.0f;
attitude.q2 = 0.0f;
attitude.q3 = 0.0f;
attitude.q4 = 0.0f;
AttitudeActualSet(&attitude);
// Cannot trust the values to init right above if BL runs
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosBias.x = 0.0f;
gyrosBias.y = 0.0f;
gyrosBias.z = 0.0f;
GyrosBiasSet(&gyrosBias);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
RevoSettingsConnectCallback(&settingsUpdatedCb);
RevoCalibrationConnectCallback(&settingsUpdatedCb);
HomeLocationConnectCallback(&settingsUpdatedCb);
EKFConfigurationConnectCallback(&settingsUpdatedCb);
FlightStatusConnectCallback(&settingsUpdatedCb);
return 0;
}
int32_t NotifyInitialize(void)
{
uint8_t ws281xOutStatus;
HwSettingsWS2811LED_OutGet(&ws281xOutStatus);
// Todo: Until further applications exists for WS2811 notify enabled status is tied to ws281x output configuration
bool enabled = ws281xOutStatus != HWSETTINGS_WS2811LED_OUT_DISABLED;
if (enabled) {
alarmStatus = (AlarmStatus_t *)pios_malloc(sizeof(AlarmStatus_t) * alarmsMapSize);
for (uint8_t i = 0; i < alarmsMapSize; i++) {
alarmStatus[i].lastAlarm = SYSTEMALARMS_ALARM_OK;
alarmStatus[i].lastAlarmTime = 0;
}
FlightStatusConnectCallback(&updatedCb);
static UAVObjEvent ev;
memset(&ev, 0, sizeof(UAVObjEvent));
EventPeriodicCallbackCreate(&ev, onTimerCb, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
updatedCb(0);
}
return 0;
}
int32_t LoggingStart(void)
{
DebugLogSettingsConnectCallback(SettingsUpdatedCb);
DebugLogControlConnectCallback(ControlUpdatedCb);
FlightStatusConnectCallback(FlightStatusUpdatedCb);
SettingsUpdatedCb(DebugLogSettingsHandle());
UAVObjEvent ev = {
.obj = DebugLogSettingsHandle(),
.instId = 0,
.event = EV_UPDATED_PERIODIC,
.lowPriority = true,
};
EventPeriodicCallbackCreate(&ev, StatusUpdatedCb, 1000);
// invoke a periodic dispatcher callback - the event struct is a dummy, it could be filled with anything!
StatusUpdatedCb(&ev);
return 0;
}
MODULE_INITCALL(LoggingInitialize, LoggingStart);
static void StatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
PIOS_DEBUGLOG_Info(&status.Flight, &status.Entry, &status.FreeSlots, &status.UsedSlots);
DebugLogStatusSet(&status);
}
static void FlightStatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
FlightStatusGet(&flightstatus);
if (settings.LoggingEnabled == DEBUGLOGSETTINGS_LOGGINGENABLED_ONLYWHENARMED) {
if (flightstatus.Armed != FLIGHTSTATUS_ARMED_ARMED) {
PIOS_DEBUGLOG_Printf("FlightStatus Disarmed: On board Logging disabled.");
PIOS_DEBUGLOG_Enable(0);
} else {
PIOS_DEBUGLOG_Enable(1);
PIOS_DEBUGLOG_Printf("FlightStatus Armed: On board logging enabled.");
}
}
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
DebugLogSettingsGet(&settings);
if (settings.LoggingEnabled == DEBUGLOGSETTINGS_LOGGINGENABLED_ALWAYS) {
PIOS_DEBUGLOG_Enable(1);
PIOS_DEBUGLOG_Printf("On board logging enabled.");
} else if (settings.LoggingEnabled == DEBUGLOGSETTINGS_LOGGINGENABLED_DISABLED) {
PIOS_DEBUGLOG_Printf("On board logging disabled.");
PIOS_DEBUGLOG_Enable(0);
} else {
FlightStatusUpdatedCb(NULL);
}
}
static void ControlUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
DebugLogControlGet(&control);
if (control.Operation == DEBUGLOGCONTROL_OPERATION_RETRIEVE) {
memset(entry, 0, sizeof(DebugLogEntryData));
if (PIOS_DEBUGLOG_Read(entry, control.Flight, control.Entry) != 0) {
// reading from log failed, mark as non existent in output
entry->Flight = control.Flight;
entry->Entry = control.Entry;
entry->Type = DEBUGLOGENTRY_TYPE_EMPTY;
}
DebugLogEntrySet(entry);
} else if (control.Operation == DEBUGLOGCONTROL_OPERATION_FORMATFLASH) {
uint8_t armed;
FlightStatusArmedGet(&armed);
if (armed == FLIGHTSTATUS_ARMED_DISARMED) {
PIOS_DEBUGLOG_Format();
}
}
StatusUpdatedCb(ev);
}
/**
* System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
*/
static void systemTask(void *parameters)
{
/* create all modules thread */
MODULE_TASKCREATE_ALL;
if (PIOS_heap_malloc_failed_p()) {
/* We failed to malloc during task creation,
* system behaviour is undefined. Reset and let
* the BootFault code recover for us.
*/
PIOS_SYS_Reset();
}
#if defined(PIOS_INCLUDE_IAP)
/* Record a successful boot */
PIOS_IAP_WriteBootCount(0);
#endif
// Initialize vars
idleCounter = 0;
idleCounterClear = 0;
// Listen for SettingPersistance object updates, connect a callback function
ObjectPersistenceConnectQueue(objectPersistenceQueue);
#if (defined(COPTERCONTROL) || defined(REVOLUTION) || defined(SIM_OSX)) && ! (defined(SIM_POSIX))
// Run this initially to make sure the configuration is checked
configuration_check();
// Whenever the configuration changes, make sure it is safe to fly
if (StabilizationSettingsHandle())
StabilizationSettingsConnectCallback(configurationUpdatedCb);
if (SystemSettingsHandle())
SystemSettingsConnectCallback(configurationUpdatedCb);
if (ManualControlSettingsHandle())
ManualControlSettingsConnectCallback(configurationUpdatedCb);
if (FlightStatusHandle())
FlightStatusConnectCallback(configurationUpdatedCb);
#endif
#if (defined(REVOLUTION) || defined(SIM_OSX)) && ! (defined(SIM_POSIX))
if (StateEstimationHandle())
StateEstimationConnectCallback(configurationUpdatedCb);
#endif
// Main system loop
while (1) {
// Update the system statistics
updateStats();
// Update the system alarms
updateSystemAlarms();
#if defined(WDG_STATS_DIAGNOSTICS)
updateWDGstats();
#endif
#if defined(DIAG_TASKS)
// Update the task status object
TaskMonitorUpdateAll();
#endif
// Flash the heartbeat LED
#if defined(PIOS_LED_HEARTBEAT)
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
DEBUG_MSG("+ 0x%08x\r\n", 0xDEADBEEF);
#endif /* PIOS_LED_HEARTBEAT */
// Turn on the error LED if an alarm is set
if (indicateError()) {
#if defined (PIOS_LED_ALARM)
PIOS_LED_On(PIOS_LED_ALARM);
#endif /* PIOS_LED_ALARM */
} else {
#if defined (PIOS_LED_ALARM)
PIOS_LED_Off(PIOS_LED_ALARM);
#endif /* PIOS_LED_ALARM */
}
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
UAVObjEvent ev;
int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ?
SYSTEM_UPDATE_PERIOD_MS / (LED_BLINK_RATE_HZ * 2) :
SYSTEM_UPDATE_PERIOD_MS;
if (PIOS_Queue_Receive(objectPersistenceQueue, &ev, delayTime) == true) {
// If object persistence is updated call the callback
objectUpdatedCb(&ev);
}
}
}
