diff --git a/flight/Modules/Osd/osdgen/osdgen.c b/flight/Modules/Osd/osdgen/osdgen.c index 4bc427bf7..b6e0b2d56 100644 --- a/flight/Modules/Osd/osdgen/osdgen.c +++ b/flight/Modules/Osd/osdgen/osdgen.c @@ -2322,7 +2322,9 @@ int32_t osdgenStart(void) vSemaphoreCreateBinary(osdSemaphore); xTaskCreate(osdgenTask, (signed char *) "OSDGEN", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &osdgenTaskHandle); TaskMonitorAdd(TASKINFO_RUNNING_OSDGEN, osdgenTaskHandle); - +#ifdef PIOS_INCLUDE_WDG + PIOS_WDG_RegisterFlag(PIOS_WDG_OSDGEN); +#endif return 0; } @@ -2370,12 +2372,18 @@ static void osdgenTask(void *parameters) // intro for (int i = 0; i < 63; i++) { if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) { +#ifdef PIOS_INCLUDE_WDG + PIOS_WDG_UpdateFlag(PIOS_WDG_OSDGEN); +#endif clearGraphics(); introGraphics(); } } for (int i = 0; i < 63; i++) { if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) { +#ifdef PIOS_INCLUDE_WDG + PIOS_WDG_UpdateFlag(PIOS_WDG_OSDGEN); +#endif clearGraphics(); introGraphics(); introText(); @@ -2384,6 +2392,9 @@ static void osdgenTask(void *parameters) while (1) { if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) { +#ifdef PIOS_INCLUDE_WDG + PIOS_WDG_UpdateFlag(PIOS_WDG_OSDGEN); +#endif updateOnceEveryFrame(); } //xSemaphoreTake(osdSemaphore, portMAX_DELAY); diff --git a/flight/Modules/Sensors/simulated/sensors.c b/flight/Modules/Sensors/simulated/sensors.c index 0e21d7f91..fef692598 100644 --- a/flight/Modules/Sensors/simulated/sensors.c +++ b/flight/Modules/Sensors/simulated/sensors.c @@ -332,7 +332,7 @@ static void simulateModelQuadcopter() static float baro_offset = 0.0f; float Rbe[3][3]; - const float ACTUATOR_ALPHA = 0.99; + const float ACTUATOR_ALPHA = 0.8; const float MAX_THRUST = GRAV * 2; const float K_FRICTION = 1; const float GPS_PERIOD = 0.1; @@ -372,9 +372,9 @@ static void simulateModelQuadcopter() RateDesiredData rateDesired; RateDesiredGet(&rateDesired); - rpy[0] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * actuatorDesired.Roll * 250 * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; - rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * actuatorDesired.Pitch * 250 * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; - rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * actuatorDesired.Yaw * 250 *(1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; + rpy[0] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; + rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; + rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; GyrosData gyrosData; // Skip get as we set all the fields gyrosData.x = rpy[0] + rand_gauss(); diff --git a/flight/PiOS/Boards/STM32F4xx_OSD.h b/flight/PiOS/Boards/STM32F4xx_OSD.h index baf17a9c8..a49c3220d 100644 --- a/flight/PiOS/Boards/STM32F4xx_OSD.h +++ b/flight/PiOS/Boards/STM32F4xx_OSD.h @@ -124,6 +124,7 @@ TIM4 | STOPWATCH | #define PIOS_WDG_STABILIZATION 0x0002 #define PIOS_WDG_ATTITUDE 0x0004 #define PIOS_WDG_MANUAL 0x0008 +#define PIOS_WDG_OSDGEN 0x0010 // ***************************************************************** diff --git a/flight/targets/OSD/Makefile b/flight/targets/OSD/Makefile index 2d0deafcd..a67d2e155 100644 --- a/flight/targets/OSD/Makefile +++ b/flight/targets/OSD/Makefile @@ -37,6 +37,9 @@ MODULES += Telemetry OPTMODULES = +# Some diagnostics +CDEFS += -DDIAG_TASKS + # Misc options CFLAGS += -ffast-math diff --git a/flight/targets/OSD/System/pios_board.c b/flight/targets/OSD/System/pios_board.c index f85f6faf2..e7354fd5a 100644 --- a/flight/targets/OSD/System/pios_board.c +++ b/flight/targets/OSD/System/pios_board.c @@ -177,6 +177,11 @@ PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_internal_cfg, &pios_internal_flash_driv HwSettingsInitialize(); +#ifdef PIOS_INCLUDE_WDG + /* Initialize watchdog as early as possible to catch faults during init */ + PIOS_WDG_Init(); +#endif /* PIOS_INCLUDE_WDG */ + /* Initialize the alarms library */ AlarmsInitialize(); diff --git a/flight/targets/UAVObjects/uavobjectmanager.c b/flight/targets/UAVObjects/uavobjectmanager.c index 6e6edc38a..c2c4d0d72 100644 --- a/flight/targets/UAVObjects/uavobjectmanager.c +++ b/flight/targets/UAVObjects/uavobjectmanager.c @@ -41,6 +41,17 @@ // Macros #define SET_BITS(var, shift, value, mask) var = (var & ~(mask << shift)) | (value << shift); +/* Table of UAVO handles registered at compile time */ +extern struct UAVOData * __start__uavo_handles[] __attribute__((weak)); +extern struct UAVOData * __stop__uavo_handles[] __attribute__((weak)); + +#define UAVO_LIST_ITERATE(_item) \ +for (struct UAVOData ** _uavo_slot = __start__uavo_handles; \ + _uavo_slot && _uavo_slot < __stop__uavo_handles; \ + _uavo_slot++) { \ + struct UAVOData * _item = *_uavo_slot; \ + if (_item == NULL) continue; + /** * List of event queues and the eventmask associated with the queue. */ @@ -98,7 +109,6 @@ struct UAVOData { * inside the payload for this UAVO. */ struct UAVOMeta metaObj; - struct UAVOData * next; uint16_t instance_size; } __attribute__((packed)); @@ -168,7 +178,6 @@ static void customSPrintf(uint8_t * buffer, uint8_t * format, ...); #endif // Private variables -static struct UAVOData * uavo_list; static xSemaphoreHandle mutex; static const UAVObjMetadata defMetadata = { .flags = (ACCESS_READWRITE << UAVOBJ_ACCESS_SHIFT | @@ -192,9 +201,12 @@ static UAVObjStats stats; int32_t UAVObjInitialize() { // Initialize variables - uavo_list = NULL; memset(&stats, 0, sizeof(UAVObjStats)); + // Initialize the uavo handle table + memset(__start__uavo_handles, 0, + (uintptr_t)__stop__uavo_handles - (uintptr_t)__start__uavo_handles); + // Create mutex mutex = xSemaphoreCreateRecursiveMutex(); if (mutex == NULL) @@ -342,9 +354,6 @@ UAVObjHandle UAVObjRegister(uint32_t id, /* Initialize the embedded meta UAVO */ UAVObjInitMetaData (&uavo_data->metaObj); - /* Add the newly created object to the global list of objects */ - LL_APPEND(uavo_list, uavo_data); - /* Initialize object fields and metadata to default values */ if (initCb) initCb((UAVObjHandle) uavo_data, 0); @@ -378,8 +387,7 @@ UAVObjHandle UAVObjGetByID(uint32_t id) xSemaphoreTakeRecursive(mutex, portMAX_DELAY); // Look for object - struct UAVOData * tmp_obj; - LL_FOREACH(uavo_list, tmp_obj) { + UAVO_LIST_ITERATE(tmp_obj) if (tmp_obj->id == id) { found_obj = (UAVObjHandle *)tmp_obj; goto unlock_exit; @@ -1023,15 +1031,13 @@ int32_t UAVObjDelete(UAVObjHandle obj_handle, uint16_t instId) */ int32_t UAVObjSaveSettings() { - struct UAVOData *obj; - // Get lock xSemaphoreTakeRecursive(mutex, portMAX_DELAY); int32_t rc = -1; // Save all settings objects - LL_FOREACH(uavo_list, obj) { + UAVO_LIST_ITERATE(obj) // Check if this is a settings object if (UAVObjIsSettings(obj)) { // Save object @@ -1055,15 +1061,13 @@ unlock_exit: */ int32_t UAVObjLoadSettings() { - struct UAVOData *obj; - // Get lock xSemaphoreTakeRecursive(mutex, portMAX_DELAY); int32_t rc = -1; // Load all settings objects - LL_FOREACH(uavo_list, obj) { + UAVO_LIST_ITERATE(obj) // Check if this is a settings object if (UAVObjIsSettings(obj)) { // Load object @@ -1087,15 +1091,13 @@ unlock_exit: */ int32_t UAVObjDeleteSettings() { - struct UAVOData *obj; - // Get lock xSemaphoreTakeRecursive(mutex, portMAX_DELAY); int32_t rc = -1; // Save all settings objects - LL_FOREACH(uavo_list, obj) { + UAVO_LIST_ITERATE(obj) // Check if this is a settings object if (UAVObjIsSettings(obj)) { // Save object @@ -1119,15 +1121,13 @@ unlock_exit: */ int32_t UAVObjSaveMetaobjects() { - struct UAVOData *obj; - // Get lock xSemaphoreTakeRecursive(mutex, portMAX_DELAY); int32_t rc = -1; // Save all settings objects - LL_FOREACH(uavo_list, obj) { + UAVO_LIST_ITERATE(obj) // Save object if (UAVObjSave( (UAVObjHandle) MetaObjectPtr(obj), 0) == -1) { @@ -1148,15 +1148,13 @@ unlock_exit: */ int32_t UAVObjLoadMetaobjects() { - struct UAVOData *obj; - // Get lock xSemaphoreTakeRecursive(mutex, portMAX_DELAY); int32_t rc = -1; // Load all settings objects - LL_FOREACH(uavo_list, obj) { + UAVO_LIST_ITERATE(obj) // Load object if (UAVObjLoad((UAVObjHandle) MetaObjectPtr(obj), 0) == -1) { @@ -1177,15 +1175,13 @@ unlock_exit: */ int32_t UAVObjDeleteMetaobjects() { - struct UAVOData *obj; - // Get lock xSemaphoreTakeRecursive(mutex, portMAX_DELAY); int32_t rc = -1; // Load all settings objects - LL_FOREACH(uavo_list, obj) { + UAVO_LIST_ITERATE(obj) // Load object if (UAVObjDelete((UAVObjHandle) MetaObjectPtr(obj), 0) == -1) { @@ -1791,8 +1787,7 @@ void UAVObjIterate(void (*iterator) (UAVObjHandle obj)) xSemaphoreTakeRecursive(mutex, portMAX_DELAY); // Iterate through the list and invoke iterator for each object - struct UAVOData *obj; - LL_FOREACH(uavo_list, obj) { + UAVO_LIST_ITERATE(obj) (*iterator) ((UAVObjHandle) obj); (*iterator) ((UAVObjHandle) &obj->metaObj); } diff --git a/flight/targets/UAVObjects/uavobjecttemplate.c b/flight/targets/UAVObjects/uavobjecttemplate.c index 69b0c94d1..808faf496 100644 --- a/flight/targets/UAVObjects/uavobjecttemplate.c +++ b/flight/targets/UAVObjects/uavobjecttemplate.c @@ -40,7 +40,7 @@ #include "$(NAMELC).h" // Private variables -static UAVObjHandle handle = NULL; +static UAVObjHandle handle __attribute__((section("_uavo_handles"))); /** * Initialize object.