diff options
Diffstat (limited to 'src/ctrl.c')
| -rwxr-xr-x[-rw-r--r--] | src/ctrl.c | 202 |
1 files changed, 155 insertions, 47 deletions
diff --git a/src/ctrl.c b/src/ctrl.c index 522c844..8856b64 100644..100755 --- a/src/ctrl.c +++ b/src/ctrl.c @@ -37,7 +37,7 @@ #include "led.h" #include "nunchuk.h" #include "tim.h" -/* Project -------------------------------------------------------------------*/ +/* project -------------------------------------------------------------------*/ #include "ctrl.h" @@ -59,8 +59,10 @@ /****************************************************************************** ******* variables ************************************************************ ******************************************************************************/ +/* Volatile ------------------------------------------------------------------*/ /* Global --------------------------------------------------------------------*/ /* Static --------------------------------------------------------------------*/ +static bool init_pending = true; static float pitch; static float roll; static float yaw; @@ -71,9 +73,11 @@ static bool level; ******* static functions (prototypes) **************************************** ******************************************************************************/ static int modules_init (void); -#if 0 +static int modules_deinit (void); static int proc_init (void); -#endif +static void proc_deinit (void); +static int proc_ctrl_it (void); +static int proc_ctrl_delay (void); static int proc_ctrl_read (void *data); static int proc_ctrl_report (void *data); @@ -86,63 +90,79 @@ static int proc_ctrl_report (void *data); * @return Error * @note Sets global variable 'prj_error' */ -int proc_ctrl_init (void) +int proc_ctrl_init (void) { - if (modules_init()) { - return ERROR_NOK; + + if (init_pending) { + init_pending = false; + } else { + return ERROR_OK; } -#if 0 + if (modules_init()) { + goto err_mod; + } if (proc_init()) { - return ERROR_NOK; + goto err_proc; } -#endif level = true; return ERROR_OK; + + +err_proc: + modules_deinit(); +err_mod: + + return ERROR_NOK; } /** - * @brief Run controller process (based on timer interrupts) + * @brief Deinitialize controller process * @return Error * @note Sets global variable 'prj_error' */ -int proc_ctrl_1 (void) +int proc_ctrl_deinit (void) { - while (true) { - __WFE(); - if (tim_tim3_interrupt) { - if (tim_callback_exe()) { - prj_error_handle(); - return ERROR_NOK; - } - tim_tim3_interrupt = false; - } + int status; + + status = ERROR_OK; + + if (!init_pending) { + init_pending = true; + } else { + return status; } - return ERROR_OK; + proc_deinit(); + if (modules_deinit()) { + status = ERROR_NOK; + } + + return status; } /** - * @brief Run controller process (based on delays) + * @brief Run controller process * @return Error * @note Sets global variable 'prj_error' */ -int proc_ctrl_2 (void) +int proc_ctrl (void) { - delay_us(1000u); - - while (true) { - delay_us(20000u); - if (proc_ctrl_read(NULL)) { - prj_error_handle(); + if (init_pending) { + if (proc_ctrl_init()) { return ERROR_NOK; } + } - if (proc_ctrl_report(NULL)) { - prj_error_handle(); + if (CTRL_TASK_MODE_IT) { + if (proc_ctrl_it()) { + return ERROR_NOK; + } + } else { + if (proc_ctrl_delay()) { return ERROR_NOK; } } @@ -156,39 +176,127 @@ int proc_ctrl_2 (void) ******************************************************************************/ static int modules_init (void) { -#if 0 - if (tim_tim3_init(REFRESH_FREQ)) { - return ERROR_NOK; - } -#endif + led_init(); if (delay_us_init()) { - return ERROR_NOK; + goto err_delay; } - if (can_init()) { - return ERROR_NOK; + + if (CTRL_TASK_MODE_IT) { + if (tim_tim3_init(CTRL_REFRESH_PERIOD_US)) { + goto err_tim; + } } + if (can_init()) { + goto err_can; + } if (nunchuk_init()) { - return ERROR_NOK; + goto err_nunchuk; } return ERROR_OK; + + +err_nunchuk: + can_deinit(); +err_can: + delay_us_deinit(); +err_delay: + led_deinit(); + tim_tim3_deinit(); +err_tim: + + return ERROR_NOK; +} + +static int modules_deinit (void) +{ + int status; + + status = ERROR_OK; + + if (nunchuk_deinit()) { + status = ERROR_NOK; + } + if (can_deinit()) { + status = ERROR_NOK; + } + if (tim_tim3_deinit()) { + status = ERROR_NOK; + } + if (delay_us_deinit()) { + status = ERROR_NOK; + } + led_deinit(); + + + return status; } -#if 0 static int proc_init (void) { - if (tim_callback_push(&proc_ctrl_read, NULL)) { - return ERROR_NOK; + + if (CTRL_TASK_MODE_IT) { + if (tim_callback_push(&proc_ctrl_read, NULL)) { + goto err_push; + } + if (tim_callback_push(&proc_ctrl_report, NULL)) { + goto err_push; + } + + return ERROR_OK; + } else { + return ERROR_OK; } - if (tim_callback_push(&proc_ctrl_report, NULL)) { - return ERROR_NOK; + + +err_push: + proc_deinit(); + + return ERROR_NOK; +} + +static void proc_deinit (void) +{ + + if (CTRL_TASK_MODE_IT) { + while (tim_callback_pop()) { + ; + } } +} - return ERROR_OK; +static int proc_ctrl_it (void) +{ + + while (true) { + __WFE(); +// __WFI(); + + if (tim_tim3_interrupt) { + if (tim_callback_exe()) { + return ERROR_NOK; + } + tim_tim3_interrupt = false; + } + } +} + +static int proc_ctrl_delay (void) +{ + + while (true) { + delay_us(CTRL_REFRESH_PERIOD_US); + + if (proc_ctrl_read(NULL)) { + return ERROR_NOK; + } + if (proc_ctrl_report(NULL)) { + return ERROR_NOK; + } + } } -#endif static int proc_ctrl_read (void *data) { |