void SystemSettingsAirframeTypeGet( uint8_t *NewAirframeType )
{
UAVObjGetDataField(SystemSettingsHandle(), (void*)NewAirframeType, offsetof( SystemSettingsData, AirframeType), sizeof(uint8_t));
}
void SystemSettingsThrustControlGet(uint8_t *NewThrustControl)
{
UAVObjGetDataField(SystemSettingsHandle(), (void *)NewThrustControl, offsetof(SystemSettingsData, ThrustControl), sizeof(uint8_t));
}
void SystemSettingsGUIConfigDataGet( uint32_t *NewGUIConfigData )
{
UAVObjGetDataField(SystemSettingsHandle(), (void*)NewGUIConfigData, offsetof( SystemSettingsData, GUIConfigData), 4*sizeof(uint32_t));
}
void SystemSettingsVehicleNameGet( uint8_t *NewVehicleName )
{
UAVObjGetDataField(SystemSettingsHandle(), (void *)NewVehicleName, offsetof(SystemSettingsData, VehicleName), 20*sizeof(uint8_t));
}
void SystemSettingsAirSpeedMinGet(float *NewAirSpeedMin)
{
UAVObjGetDataField(SystemSettingsHandle(), (void *)NewAirSpeedMin, offsetof(SystemSettingsData, AirSpeedMin), sizeof(float));
}
/**
* 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);
}
}
}
/**
* Run a preflight check over the hardware configuration
* and currently active modules
*/
int32_t configuration_check()
{
SystemAlarmsConfigErrorOptions error_code = SYSTEMALARMS_CONFIGERROR_NONE;
// Get board type
const struct pios_board_info * bdinfo = &pios_board_info_blob;
bool coptercontrol = bdinfo->board_type == 0x04;
// For when modules are not running we should explicitly check the objects are
// valid
if (ManualControlSettingsHandle() == NULL ||
SystemSettingsHandle() == NULL) {
AlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, SYSTEMALARMS_ALARM_CRITICAL);
return 0;
}
// Classify airframe type
bool multirotor = true;
uint8_t airframe_type;
SystemSettingsAirframeTypeGet(&airframe_type);
switch(airframe_type) {
case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX:
case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP:
case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO:
case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP:
case SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX:
case SYSTEMSETTINGS_AIRFRAMETYPE_TRI:
multirotor = true;
break;
default:
multirotor = false;
}
// For each available flight mode position sanity check the available
// modes
uint8_t num_modes;
uint8_t modes[MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM];
ManualControlSettingsFlightModeNumberGet(&num_modes);
ManualControlSettingsFlightModePositionGet(modes);
for(uint32_t i = 0; i < num_modes; i++) {
switch(modes[i]) {
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL:
if (multirotor) {
error_code = SYSTEMALARMS_CONFIGERROR_STABILIZATION;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ACRO:
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ACROPLUS:
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LEVELING:
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MWRATE:
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_HORIZON:
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AXISLOCK:
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VIRTUALBAR:
// always ok
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
error_code = (error_code == SYSTEMALARMS_CONFIGERROR_NONE) ? check_stabilization_settings(1, multirotor) : error_code;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
error_code = (error_code == SYSTEMALARMS_CONFIGERROR_NONE) ? check_stabilization_settings(2, multirotor) : error_code;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
error_code = (error_code == SYSTEMALARMS_CONFIGERROR_NONE) ? check_stabilization_settings(3, multirotor) : error_code;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_AUTOTUNE))
error_code = SYSTEMALARMS_CONFIGERROR_AUTOTUNE;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD:
if (coptercontrol)
error_code = SYSTEMALARMS_CONFIGERROR_ALTITUDEHOLD;
else {
if ( !TaskMonitorQueryRunning(TASKINFO_RUNNING_ALTITUDEHOLD) )
error_code = SYSTEMALARMS_CONFIGERROR_ALTITUDEHOLD;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOHOME:
if (coptercontrol) {
error_code = SYSTEMALARMS_CONFIGERROR_PATHPLANNER;
}
else {
if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
error_code = SYSTEMALARMS_CONFIGERROR_PATHPLANNER;
} else {
error_code = check_safe_autonomous();
}
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_TABLETCONTROL:
if (coptercontrol) {
error_code = SYSTEMALARMS_CONFIGERROR_PATHPLANNER;
}
else {
if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER) ||
!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHPLANNER)) {
error_code = SYSTEMALARMS_CONFIGERROR_PATHPLANNER;
} else {
error_code = check_safe_autonomous();
}
}
break;
default:
// Uncovered modes are automatically an error
error_code = SYSTEMALARMS_CONFIGERROR_UNDEFINED;
}
}
// Check the stabilization rates are within what the sensors can track
error_code = (error_code == SYSTEMALARMS_CONFIGERROR_NONE) ? check_stabilization_rates() : error_code;
// Only check safe to arm if no other errors exist
error_code = (error_code == SYSTEMALARMS_CONFIGERROR_NONE) ? check_safe_to_arm() : error_code;
set_config_error(error_code);
return 0;
}
void SystemSettingsAirframeCategorySpecificConfigurationGet( uint32_t *NewAirframeCategorySpecificConfiguration )
{
UAVObjGetDataField(SystemSettingsHandle(), (void*)NewAirframeCategorySpecificConfiguration, offsetof( SystemSettingsData, AirframeCategorySpecificConfiguration), 4*sizeof(uint32_t));
}
