diff --git a/flight/Modules/CameraStab/camerastab.c b/flight/Modules/CameraStab/camerastab.c index e2894a767..e8321543b 100644 --- a/flight/Modules/CameraStab/camerastab.c +++ b/flight/Modules/CameraStab/camerastab.c @@ -51,6 +51,7 @@ #include "attitudeactual.h" #include "camerastabsettings.h" #include "cameradesired.h" +#include "hwsettings.h" // // Configuration @@ -61,6 +62,8 @@ // Private variables +static uint8_t camerastabEnabled = 0; + // Private functions static void attitudeUpdated(UAVObjEvent* ev); static float bound(float val); @@ -72,16 +75,33 @@ static float bound(float val); int32_t CameraStabInitialize(void) { static UAVObjEvent ev; - ev.obj = AttitudeActualHandle(); - ev.instId = 0; - ev.event = 0; - CameraStabSettingsInitialize(); - CameraDesiredInitialize(); + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesGet(optionalModules); + if (optionalModules[HWSETTINGS_OPTIONALMODULES_CAMERASTAB]==HWSETTINGS_OPTIONALMODULES_ENABLED) { + camerastabEnabled=1; + } else { + camerastabEnabled=0; + } - EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS); + if (camerastabEnabled) { - return 0; + AttitudeActualInitialize(); + + ev.obj = AttitudeActualHandle(); + ev.instId = 0; + ev.event = 0; + + CameraStabSettingsInitialize(); + CameraDesiredInitialize(); + + EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS); + + return 0; + } + + return -1; } /* stub: module has no module thread */ @@ -90,6 +110,8 @@ int32_t CameraStabStart(void) return 0; } +MODULE_INITCALL(CameraStabInitialize, CameraStabStart) + static void attitudeUpdated(UAVObjEvent* ev) { if (ev->obj != AttitudeActualHandle()) diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index 0830d5762..3d844d5fc 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -78,6 +78,7 @@ static float GravityAccel(float latitude, float longitude, float altitude); // Private variables static uint32_t gpsPort; +static uint8_t gpsEnabled = 0; static xTaskHandle gpsTaskHandle; @@ -98,14 +99,16 @@ static uint32_t numParsingErrors; int32_t GPSStart(void) { - if (gpsPort) { - // Start gps task - xTaskCreate(gpsTask, (signed char *)"GPS", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &gpsTaskHandle); - TaskMonitorAdd(TASKINFO_RUNNING_GPS, gpsTaskHandle); - return 0; - } + if (gpsEnabled) { + if (gpsPort) { + // Start gps task + xTaskCreate(gpsTask, (signed char *)"GPS", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &gpsTaskHandle); + TaskMonitorAdd(TASKINFO_RUNNING_GPS, gpsTaskHandle); + return 0; + } - AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); + AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); + } return -1; } @@ -117,8 +120,16 @@ int32_t GPSStart(void) int32_t GPSInitialize(void) { gpsPort = PIOS_COM_GPS; + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesGet(optionalModules); + if (optionalModules[HWSETTINGS_OPTIONALMODULES_GPS]==HWSETTINGS_OPTIONALMODULES_ENABLED) { + gpsEnabled=1; + } else { + gpsEnabled=0; + } - if (gpsPort) { + if (gpsPort && gpsEnabled) { GPSPositionInitialize(); #if !defined(PIOS_GPS_MINIMAL) GPSTimeInitialize(); @@ -138,8 +149,7 @@ int32_t GPSInitialize(void) return -1; } -// optional module, gets initialized separately -//MODULE_INITCALL(GPSInitialize, GPSStart) +MODULE_INITCALL(GPSInitialize, GPSStart) // **************** /**