rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

OP-423: Split task create and module init in order to postpone task creation once the full heap is available.

Browse Source 
Also implement some ordering (quite ugly still) in the module init and task creation order so we can decide which module to start/init first
and which module to start/init last.
This will be replaced/adapter with the uavobject list later (once it's implemented).
reserving some space for module init and task create parameters to customize module/task creation (this will be usefull once we get the list and customization from customer).

Changes have been made for OP and CC. Tested comped with CC,OP, sim_posix.
Only ran on bench with CC for couple of minutes (code increase expected but no dropping of stack which is good).

This gives task creation at the time wherethe all heap is available.
This commit is contained in:
Mathieu Rondonneau 2011-06-19 22:35:40 -07:00
parent 2b11395a67
commit fc1e3f574c
17 changed files with 233 additions and 117 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
flight/CopterControl/System/coptercontrol.c
View File

@ -48,9 +48,6 @@ extern void InitModules(void);
/* Prototype of PIOS_Board_Init() function */
extern void PIOS_Board_Init(void);
/* Init module section */
extern initcall_t __module_initcall_start[], __module_initcall_end[];
/**
* OpenPilot Main function:
*
@ -80,22 +77,7 @@ int main()
#endif
/* Initialize modules */
/* TODO: add id so we can parse this list later and replace module on the fly */
/* property flag will be add to give information like:
* - importance of the module (can be dropped or required for flight
* - parameter to enable feature at run-time (based on user setup on GCS)
* All this will be handled by bootloader. this section will add a function pointer
* and a pointer to a 32 bit in RAM with all the flags.
* Limited on CC this could be mapped on RAM for OP so we can grow the list at run-time.
*/
initcall_t *fn;
int32_t ret;
/* this one needs to be done first */
SystemModInitialize();
for (fn = __module_initcall_start; fn < __module_initcall_end; fn++)
ret = (*fn)();
MODULE_INITIALISE_ALL();
#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
/* Start the FreeRTOS scheduler which never returns.*/

14
flight/Modules/AHRSComms/ahrs_comms.c
View File

@ -69,8 +69,7 @@ static void ahrscommsTask(void *parameters);
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
module_initcall(AHRSCommsInitialize, 0);
int32_t AHRSCommsInitialize(void)
int32_t AHRSCommsStart(void)
{
// Start main task
xTaskCreate(ahrscommsTask, (signed char *)"AHRSComms", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
@ -80,6 +79,17 @@ int32_t AHRSCommsInitialize(void)
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AHRSCommsInitialize(void)
{
return 0;
}
module_initcall(AHRSCommsInitialize, 0, AHRSCommsStart, 0, MODULE_EXEC_NOORDER_FLAG);
/**
* Module thread, should not return.
*/

22
flight/Modules/Actuator/actuator.c
View File

@ -85,12 +85,24 @@ typedef struct {
int8_t matrix[5];
} __attribute__((packed)) Mixer_t;
/**
* @brief Module initialization
* @return 0
*/
int32_t ActuatorStart()
{
// Start main task
xTaskCreate(actuatorTask, (signed char*)"Actuator", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_ACTUATOR, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR);
return 0;
}
/**
* @brief Module initialization
* @return 0
*/
module_initcall(ActuatorInitialize, 0);
int32_t ActuatorInitialize()
{
// Create object queue
@ -101,14 +113,10 @@ int32_t ActuatorInitialize()
// If settings change, update the output rate
ActuatorSettingsConnectCallback(actuator_update_rate);
// Start main task
xTaskCreate(actuatorTask, (signed char*)"Actuator", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_ACTUATOR, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR);
return 0;
}
module_initcall(ActuatorInitialize, 0, ActuatorStart, 0, MODULE_EXEC_NOORDER_FLAG);
/**
* @brief Main Actuator module task

15
flight/Modules/Altitude/altitude.c
View File

@ -67,13 +67,22 @@ static void altitudeTask(void *parameters);
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
module_initcall(AltitudeInitialize, 0);
int32_t AltitudeInitialize()
int32_t AltitudeStart()
{
// Start main task
xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_ALTITUDE, taskHandle);
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AltitudeInitialize()
{
// init down-sampling data
alt_ds_temp = 0;
alt_ds_pres = 0;
@ -81,7 +90,7 @@ int32_t AltitudeInitialize()
return 0;
}
module_initcall(AltitudeInitialize, 0, AltitudeStart, 0, MODULE_EXEC_NOORDER_FLAG);
/**
* Module thread, should not return.
*/

30
flight/Modules/Attitude/attitude.c
View File

@ -94,7 +94,21 @@ static bool zero_during_arming = false;
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
module_initcall(AttitudeInitialize, 0);
int32_t AttitudeStart(void)
{
// Start main task
xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_ATTITUDE, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE);
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AttitudeInitialize(void)
{
// Initialize quaternion
@ -105,22 +119,20 @@ int32_t AttitudeInitialize(void)
attitude.q3 = 0;
attitude.q4 = 0;
AttitudeActualSet(&attitude);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
// Create queue for passing gyro data, allow 2 back samples in case
gyro_queue = xQueueCreate(1, sizeof(float) * 4);
if(gyro_queue == NULL)
if(gyro_queue == NULL)
return -1;
PIOS_ADC_SetQueue(gyro_queue);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
// Start main task
xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_ATTITUDE, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE);
return 0;
}
module_initcall(AttitudeInitialize, 0, AttitudeStart, 0, MODULE_EXEC_NOORDER_FLAG);
/**
* Module thread, should not return.
*/

2
flight/Modules/FirmwareIAP/firmwareiap.c
View File

@ -88,7 +88,7 @@ static void resetTask(UAVObjEvent *);
* \note
*
*/
module_initcall(FirmwareIAPInitialize, 0);
module_initcall(FirmwareIAPInitialize, 0, 0, 0, MODULE_EXEC_NOORDER_FLAG);
int32_t FirmwareIAPInitialize()
{
const struct pios_board_info * bdinfo = &pios_board_info_blob;

21
flight/Modules/FlightPlan/flightplan.c
View File

@ -57,7 +57,20 @@ extern unsigned char usrlib_img[];
/**
* Module initialization
*/
module_initcall(FlightPlanInitialize, 0);
int32_t FlightPlanStart()
{
taskHandle = NULL;
// Start VM thread
xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle);
return 0;
}
/**
* Module initialization
*/
int32_t FlightPlanInitialize()
{
taskHandle = NULL;
@ -71,13 +84,9 @@ int32_t FlightPlanInitialize()
// Listen for FlightPlanControl updates
FlightPlanControlConnectQueue(queue);
// Start VM thread
xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle);
return 0;
}
module_initcall(FlightPlanInitialize, 0, FlightPlanStart, 0, MODULE_EXEC_NOORDER_FLAG);
/**
* Module task
*/

24
flight/Modules/GPS/GPS.c
View File

@ -108,20 +108,28 @@ static uint32_t numParsingErrors;
* \return -1 if initialisation failed
* \return 0 on success
*/
module_initcall(GPSInitialize, 0);
int32_t GPSInitialize(void)
int32_t GPSStart(void)
{
signed portBASE_TYPE xReturn;
// TODO: Get gps settings object
gpsPort = PIOS_COM_GPS;
// Start gps task
xReturn = xTaskCreate(gpsTask, (signed char *)"GPS", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &gpsTaskHandle);
xTaskCreate(gpsTask, (signed char *)"GPS", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &gpsTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_GPS, gpsTaskHandle);
return 0;
}
/**
* Initialise the gps module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t GPSInitialize(void)
{
// TODO: Get gps settings object
gpsPort = PIOS_COM_GPS;
return 0;
}
module_initcall(GPSInitialize, 0, GPSStart, 0, MODULE_EXEC_NOORDER_FLAG);
// ****************
/**

19
flight/Modules/Guidance/guidance.c
View File

@ -82,7 +82,19 @@ static void updateVtolDesiredAttitude();
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
module_initcall(GuidanceInitialize, 0);
int32_t GuidanceStart()
{
// Start main task
xTaskCreate(guidanceTask, (signed char *)"Guidance", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &guidanceTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_GUIDANCE, guidanceTaskHandle);
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t GuidanceInitialize()
{
// Create object queue
@ -91,12 +103,9 @@ int32_t GuidanceInitialize()
// Listen for updates.
AttitudeRawConnectQueue(queue);
// Start main task
xTaskCreate(guidanceTask, (signed char *)"Guidance", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &guidanceTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_GUIDANCE, guidanceTaskHandle);
return 0;
}
module_initcall(GuidanceInitialize, 0, GuidanceStart, 0, MODULE_EXEC_NOORDER_FLAG);
static float northVelIntegral = 0;
static float eastVelIntegral = 0;

21
flight/Modules/ManualControl/manualcontrol.c
View File

@ -144,20 +144,33 @@ static bool validInputRange(int16_t min, int16_t max, uint16_t value);
/**
* Module initialization
*/
module_initcall(ManualControlInitialize, 0);
int32_t ManualControlStart()
{
/* Check the assumptions about uavobject enum's are correct */
if(!assumptions)
return -1;
// Start main task
xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
return 0;
}
/**
* Module initialization
*/
int32_t ManualControlInitialize()
{
/* Check the assumptions about uavobject enum's are correct */
if(!assumptions)
return -1;
// Start main task
xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
//ManualControlStart();
return 0;
}
module_initcall(ManualControlInitialize, 0, ManualControlStart, 0, MODULE_EXEC_NOORDER_FLAG);
/**
* Module task

36
flight/Modules/Stabilization/stabilization.c
View File

@ -89,28 +89,40 @@ static void SettingsUpdatedCb(UAVObjEvent * ev);
/**
* Module initialization
*/
module_initcall(StabilizationInitialize, 0);
int32_t StabilizationInitialize()
int32_t StabilizationStart()
{
// Initialize variables
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
// Listen for updates.
// AttitudeActualConnectQueue(queue);
AttitudeRawConnectQueue(queue);
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle());
// Start main task
xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_STABILIZATION, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION);
return 0;
}
/**
* Module initialization
*/
int32_t StabilizationInitialize()
{
// Initialize variables
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
// Listen for updates.
// AttitudeActualConnectQueue(queue);
AttitudeRawConnectQueue(queue);
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle());
// Start main task
return 0;
}
module_initcall(StabilizationInitialize, 0, StabilizationStart, 0, MODULE_EXEC_NOORDER_FLAG);
/**
* Module task
*/

26
flight/Modules/System/systemmod.c
View File

@ -84,18 +84,34 @@ static void updateSystemAlarms();
static void systemTask(void *parameters);
/**
* Initialise the module, called on startup.
* \returns 0 on success or -1 if initialisation failed
* Create the module task.
* \returns 0 on success or -1 if initialization failed
*/
int32_t SystemModInitialize(void)
int32_t SystemModStart(void)
{
// Initialize vars
stackOverflow = 0;
// Create system task
xTaskCreate(systemTask, (signed char *)"System", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &systemTaskHandle);
// Register task
TaskMonitorAdd(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
return 0;
}
/**
* Initialize the module, called on startup.
* \returns 0 on success or -1 if initialization failed
*/
int32_t SystemModInitialize(void)
{
SystemModStart();
return 0;
}
module_initcall(SystemModInitialize, 0, 0, 0, MODULE_EXEC_FIRST_FLAG);
/**
* System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
*/
@ -103,8 +119,8 @@ static void systemTask(void *parameters)
{
portTickType lastSysTime;
// Register task
TaskMonitorAdd(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
/* create all modules thread */
MODULE_TASKCREATE_ALL();
// Initialize vars
idleCounter = 0;

35
flight/Modules/Telemetry/telemetry.c
View File

@ -86,7 +86,28 @@ static void updateSettings();
* \return -1 if initialisation failed
* \return 0 on success
*/
module_initcall(TelemetryInitialize, 0);
int32_t TelemetryStart(void)
{
// Start telemetry tasks
xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle);
xTaskCreate(telemetryRxTask, (signed char *)"TelRx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYTX, telemetryTxTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle);
#if defined(PIOS_TELEM_PRIORITY_QUEUE)
xTaskCreate(telemetryTxPriTask, (signed char *)"TelPriTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TXPRI, &telemetryTxPriTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYTXPRI, telemetryTxPriTaskHandle);
#endif
return 0;
}
/**
* Initialise the telemetry module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t TelemetryInitialize(void)
{
UAVObjEvent ev;
@ -119,20 +140,10 @@ int32_t TelemetryInitialize(void)
GCSTelemetryStatsConnectQueue(priorityQueue);
TelemetrySettingsConnectQueue(priorityQueue);
// Start telemetry tasks
xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle);
xTaskCreate(telemetryRxTask, (signed char *)"TelRx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYTX, telemetryTxTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle);
#if defined(PIOS_TELEM_PRIORITY_QUEUE)
xTaskCreate(telemetryTxPriTask, (signed char *)"TelPriTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TXPRI, &telemetryTxPriTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYTXPRI, telemetryTxPriTaskHandle);
#endif
return 0;
}
module_initcall(TelemetryInitialize, 0, TelemetryStart, 0, MODULE_EXEC_LAST_FLAG);
/**
* Register a new object, adds object to local list and connects the queue depending on the object's

19
flight/OpenPilot/System/openpilot.c
View File

@ -68,11 +68,6 @@ extern void InitModules(void);
/* Prototype of PIOS_Board_Init() function */
extern void PIOS_Board_Init(void);
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
/* Init module section */
extern initcall_t __module_initcall_start[], __module_initcall_end[];
#endif
/**
* OpenPilot Main function:
*
@ -97,19 +92,9 @@ int main()
PIOS_Board_Init();
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
/* Initialize modules */
/* TODO: add id so we can parse this list later and replace module on the fly */
/* property flag will be add to give information like:
* - importance of the module (can be dropped or required for flight
* - parameter to enable feature at run-time (based on user setup on GCS)
* All this will be handled by bootloader. this section will add a function pointer
* and a pointer to a 32 bit in RAM with all the flags.
* Limited on CC this could be mapped on RAM for OP so we can grow the list at run-time.
*/
initcall_t *fn;
int32_t ret;
for (fn = __module_initcall_start; fn < __module_initcall_end; fn++)
ret = (*fn)();
MODULE_INITIALISE_ALL();
/* Create test tasks */
/* keep this just because it was there */

4
flight/PiOS.posix/inc/pios_initcall.h
View File

@ -39,7 +39,9 @@
*/
#define uavobj_initcall(fn)
#define module_initcall(fn, param)
#define module_initcall(ifn, iparam, sfn, sparam, flags)
#define MODULE_TASKCREATE_ALL();
#endif /* PIOS_INITCALL_H */

5
flight/PiOS.win32/inc/pios_initcall.h
View File

@ -39,7 +39,10 @@
*/
#define uavobj_initcall(fn)
#define module_initcall(fn, param)
#define module_initcall(ifn, iparam, sfn, sparam, flags)
#define MODULE_TASKCREATE_ALL();
#endif /* PIOS_INITCALL_H */

37
flight/PiOS/inc/pios_initcall.h
View File

@ -41,7 +41,21 @@
/*
* Used for initialization calls..
*/
#define MODULE_EXEC_NOORDER_FLAG 0xFA000000
#define MODULE_EXEC_FIRST_FLAG 0xFA000001
#define MODULE_EXEC_LAST_FLAG 0xFA000002
typedef int32_t (*initcall_t)(void);
typedef struct {
uint32_t flag;
uint32_t param_minit;
initcall_t fn_minit;
uint32_t param_tinit;
initcall_t fn_tinit;
} initmodule_t;
/* Init module section */
extern initmodule_t __module_initcall_start[], __module_initcall_end[];
/* initcalls are now grouped by functionality into separate
* subsections. Ordering inside the subsections is determined
@ -55,13 +69,26 @@ typedef int32_t (*initcall_t)(void);
static initcall_t __initcall_##fn##id __attribute__((__used__)) \
__attribute__((__section__(".initcall" level ".init"))) = fn
/* TODO: add param later */
#define __define_module_initcall(level,fn,param) \
static initcall_t __initcall_##fn __attribute__((__used__)) \
__attribute__((__section__(".initcall" level ".init"))) = fn
#define __define_module_initcall(level,ifn,iparam,sfn,sparam, param) \
static initmodule_t __initcall_##fn __attribute__((__used__)) \
__attribute__((__section__(".initcall" level ".init"))) = { .flag = param, .param_minit = iparam, .fn_minit = ifn, .param_tinit = sparam, .fn_tinit = sfn }
#define uavobj_initcall(fn) __define_initcall("uavobj",fn,1)
#define module_initcall(fn, param) __define_module_initcall("module",fn,param)
#define module_initcall(ifn, iparam, sfn, sparam, flags) __define_module_initcall("module", ifn, iparam, sfn, sparam, flags)
#define MODULE_INITIALISE_ALL(); for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) \
if (fn->fn_minit && ( (fn->flag & MODULE_EXEC_FIRST_FLAG) == MODULE_EXEC_FIRST_FLAG) ) \
(fn->fn_minit)(); \
for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) \
if (fn->fn_minit && ( (fn->flag & (MODULE_EXEC_LAST_FLAG | MODULE_EXEC_FIRST_FLAG)) == MODULE_EXEC_NOORDER_FLAG) ) \
(fn->fn_minit)(); \
for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) \
if (fn->fn_minit && ( (fn->flag & MODULE_EXEC_LAST_FLAG) == MODULE_EXEC_LAST_FLAG) ) \
(fn->fn_minit)();
#define MODULE_TASKCREATE_ALL(); for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) \
if (fn->fn_tinit) \
(fn->fn_tinit)();
#endif /* PIOS_INITCALL_H */