Anasse Abdoul / Mbed 2 deprecated Test_MPU6050

Dependencies:   mbed

Committer:
anasse
Date:
Thu Mar 31 07:43:50 2022 +0000
Revision:
0:a59a3d743804
vers0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
anasse 0:a59a3d743804 1 /*----------------------------------------------------------------------------
anasse 0:a59a3d743804 2 * RL-ARM - RTX
anasse 0:a59a3d743804 3 *----------------------------------------------------------------------------
anasse 0:a59a3d743804 4 * Name: RT_TASK.C
anasse 0:a59a3d743804 5 * Purpose: Task functions and system start up.
anasse 0:a59a3d743804 6 * Rev.: V4.60
anasse 0:a59a3d743804 7 *----------------------------------------------------------------------------
anasse 0:a59a3d743804 8 *
anasse 0:a59a3d743804 9 * Copyright (c) 1999-2009 KEIL, 2009-2015 ARM Germany GmbH
anasse 0:a59a3d743804 10 * All rights reserved.
anasse 0:a59a3d743804 11 * Redistribution and use in source and binary forms, with or without
anasse 0:a59a3d743804 12 * modification, are permitted provided that the following conditions are met:
anasse 0:a59a3d743804 13 * - Redistributions of source code must retain the above copyright
anasse 0:a59a3d743804 14 * notice, this list of conditions and the following disclaimer.
anasse 0:a59a3d743804 15 * - Redistributions in binary form must reproduce the above copyright
anasse 0:a59a3d743804 16 * notice, this list of conditions and the following disclaimer in the
anasse 0:a59a3d743804 17 * documentation and/or other materials provided with the distribution.
anasse 0:a59a3d743804 18 * - Neither the name of ARM nor the names of its contributors may be used
anasse 0:a59a3d743804 19 * to endorse or promote products derived from this software without
anasse 0:a59a3d743804 20 * specific prior written permission.
anasse 0:a59a3d743804 21 *
anasse 0:a59a3d743804 22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
anasse 0:a59a3d743804 23 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
anasse 0:a59a3d743804 24 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
anasse 0:a59a3d743804 25 * ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE
anasse 0:a59a3d743804 26 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
anasse 0:a59a3d743804 27 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
anasse 0:a59a3d743804 28 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
anasse 0:a59a3d743804 29 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
anasse 0:a59a3d743804 30 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
anasse 0:a59a3d743804 31 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
anasse 0:a59a3d743804 32 * POSSIBILITY OF SUCH DAMAGE.
anasse 0:a59a3d743804 33 *---------------------------------------------------------------------------*/
anasse 0:a59a3d743804 34
anasse 0:a59a3d743804 35 #include "rt_TypeDef.h"
anasse 0:a59a3d743804 36 #include "RTX_Conf.h"
anasse 0:a59a3d743804 37 #include "rt_System.h"
anasse 0:a59a3d743804 38 #include "rt_Task.h"
anasse 0:a59a3d743804 39 #include "rt_List.h"
anasse 0:a59a3d743804 40 #include "rt_MemBox.h"
anasse 0:a59a3d743804 41 #include "rt_Robin.h"
anasse 0:a59a3d743804 42 #include "rt_HAL_CM.h"
anasse 0:a59a3d743804 43
anasse 0:a59a3d743804 44 /*----------------------------------------------------------------------------
anasse 0:a59a3d743804 45 * Global Variables
anasse 0:a59a3d743804 46 *---------------------------------------------------------------------------*/
anasse 0:a59a3d743804 47
anasse 0:a59a3d743804 48 /* Running and next task info. */
anasse 0:a59a3d743804 49 struct OS_TSK os_tsk;
anasse 0:a59a3d743804 50
anasse 0:a59a3d743804 51 /* Task Control Blocks of idle demon */
anasse 0:a59a3d743804 52 struct OS_TCB os_idle_TCB;
anasse 0:a59a3d743804 53
anasse 0:a59a3d743804 54
anasse 0:a59a3d743804 55 /*----------------------------------------------------------------------------
anasse 0:a59a3d743804 56 * Local Functions
anasse 0:a59a3d743804 57 *---------------------------------------------------------------------------*/
anasse 0:a59a3d743804 58
anasse 0:a59a3d743804 59 OS_TID rt_get_TID (void) {
anasse 0:a59a3d743804 60 U32 tid;
anasse 0:a59a3d743804 61
anasse 0:a59a3d743804 62 for (tid = 1; tid <= os_maxtaskrun; tid++) {
anasse 0:a59a3d743804 63 if (os_active_TCB[tid-1] == NULL) {
anasse 0:a59a3d743804 64 return ((OS_TID)tid);
anasse 0:a59a3d743804 65 }
anasse 0:a59a3d743804 66 }
anasse 0:a59a3d743804 67 return (0);
anasse 0:a59a3d743804 68 }
anasse 0:a59a3d743804 69
anasse 0:a59a3d743804 70 #if defined (__CC_ARM) && !defined (__MICROLIB)
anasse 0:a59a3d743804 71 /*--------------------------- __user_perthread_libspace ---------------------*/
anasse 0:a59a3d743804 72 extern void *__libspace_start;
anasse 0:a59a3d743804 73
anasse 0:a59a3d743804 74 void *__user_perthread_libspace (void) {
anasse 0:a59a3d743804 75 /* Provide a separate libspace for each task. */
anasse 0:a59a3d743804 76 if (os_tsk.run == NULL) {
anasse 0:a59a3d743804 77 /* RTX not running yet. */
anasse 0:a59a3d743804 78 return (&__libspace_start);
anasse 0:a59a3d743804 79 }
anasse 0:a59a3d743804 80 return (void *)(os_tsk.run->std_libspace);
anasse 0:a59a3d743804 81 }
anasse 0:a59a3d743804 82 #endif
anasse 0:a59a3d743804 83
anasse 0:a59a3d743804 84 /*--------------------------- rt_init_context -------------------------------*/
anasse 0:a59a3d743804 85
anasse 0:a59a3d743804 86 void rt_init_context (P_TCB p_TCB, U8 priority, FUNCP task_body) {
anasse 0:a59a3d743804 87 /* Initialize general part of the Task Control Block. */
anasse 0:a59a3d743804 88 p_TCB->cb_type = TCB;
anasse 0:a59a3d743804 89 p_TCB->state = READY;
anasse 0:a59a3d743804 90 p_TCB->prio = priority;
anasse 0:a59a3d743804 91 p_TCB->p_lnk = NULL;
anasse 0:a59a3d743804 92 p_TCB->p_rlnk = NULL;
anasse 0:a59a3d743804 93 p_TCB->p_dlnk = NULL;
anasse 0:a59a3d743804 94 p_TCB->p_blnk = NULL;
anasse 0:a59a3d743804 95 p_TCB->delta_time = 0;
anasse 0:a59a3d743804 96 p_TCB->interval_time = 0;
anasse 0:a59a3d743804 97 p_TCB->events = 0;
anasse 0:a59a3d743804 98 p_TCB->waits = 0;
anasse 0:a59a3d743804 99 p_TCB->stack_frame = 0;
anasse 0:a59a3d743804 100
anasse 0:a59a3d743804 101 rt_init_stack (p_TCB, task_body);
anasse 0:a59a3d743804 102 }
anasse 0:a59a3d743804 103
anasse 0:a59a3d743804 104
anasse 0:a59a3d743804 105 /*--------------------------- rt_switch_req ---------------------------------*/
anasse 0:a59a3d743804 106
anasse 0:a59a3d743804 107 void rt_switch_req (P_TCB p_new) {
anasse 0:a59a3d743804 108 /* Switch to next task (identified by "p_new"). */
anasse 0:a59a3d743804 109 os_tsk.new_tsk = p_new;
anasse 0:a59a3d743804 110 p_new->state = RUNNING;
anasse 0:a59a3d743804 111 DBG_TASK_SWITCH(p_new->task_id);
anasse 0:a59a3d743804 112 }
anasse 0:a59a3d743804 113
anasse 0:a59a3d743804 114
anasse 0:a59a3d743804 115 /*--------------------------- rt_dispatch -----------------------------------*/
anasse 0:a59a3d743804 116
anasse 0:a59a3d743804 117 void rt_dispatch (P_TCB next_TCB) {
anasse 0:a59a3d743804 118 /* Dispatch next task if any identified or dispatch highest ready task */
anasse 0:a59a3d743804 119 /* "next_TCB" identifies a task to run or has value NULL (=no next task) */
anasse 0:a59a3d743804 120 if (next_TCB == NULL) {
anasse 0:a59a3d743804 121 /* Running task was blocked: continue with highest ready task */
anasse 0:a59a3d743804 122 next_TCB = rt_get_first (&os_rdy);
anasse 0:a59a3d743804 123 rt_switch_req (next_TCB);
anasse 0:a59a3d743804 124 }
anasse 0:a59a3d743804 125 else {
anasse 0:a59a3d743804 126 /* Check which task continues */
anasse 0:a59a3d743804 127 if (next_TCB->prio > os_tsk.run->prio) {
anasse 0:a59a3d743804 128 /* preempt running task */
anasse 0:a59a3d743804 129 rt_put_rdy_first (os_tsk.run);
anasse 0:a59a3d743804 130 os_tsk.run->state = READY;
anasse 0:a59a3d743804 131 rt_switch_req (next_TCB);
anasse 0:a59a3d743804 132 }
anasse 0:a59a3d743804 133 else {
anasse 0:a59a3d743804 134 /* put next task into ready list, no task switch takes place */
anasse 0:a59a3d743804 135 next_TCB->state = READY;
anasse 0:a59a3d743804 136 rt_put_prio (&os_rdy, next_TCB);
anasse 0:a59a3d743804 137 }
anasse 0:a59a3d743804 138 }
anasse 0:a59a3d743804 139 }
anasse 0:a59a3d743804 140
anasse 0:a59a3d743804 141
anasse 0:a59a3d743804 142 /*--------------------------- rt_block --------------------------------------*/
anasse 0:a59a3d743804 143
anasse 0:a59a3d743804 144 void rt_block (U16 timeout, U8 block_state) {
anasse 0:a59a3d743804 145 /* Block running task and choose next ready task. */
anasse 0:a59a3d743804 146 /* "timeout" sets a time-out value or is 0xffff (=no time-out). */
anasse 0:a59a3d743804 147 /* "block_state" defines the appropriate task state */
anasse 0:a59a3d743804 148 P_TCB next_TCB;
anasse 0:a59a3d743804 149
anasse 0:a59a3d743804 150 if (timeout) {
anasse 0:a59a3d743804 151 if (timeout < 0xffff) {
anasse 0:a59a3d743804 152 rt_put_dly (os_tsk.run, timeout);
anasse 0:a59a3d743804 153 }
anasse 0:a59a3d743804 154 os_tsk.run->state = block_state;
anasse 0:a59a3d743804 155 next_TCB = rt_get_first (&os_rdy);
anasse 0:a59a3d743804 156 rt_switch_req (next_TCB);
anasse 0:a59a3d743804 157 }
anasse 0:a59a3d743804 158 }
anasse 0:a59a3d743804 159
anasse 0:a59a3d743804 160
anasse 0:a59a3d743804 161 /*--------------------------- rt_tsk_pass -----------------------------------*/
anasse 0:a59a3d743804 162
anasse 0:a59a3d743804 163 void rt_tsk_pass (void) {
anasse 0:a59a3d743804 164 /* Allow tasks of same priority level to run cooperatively.*/
anasse 0:a59a3d743804 165 P_TCB p_new;
anasse 0:a59a3d743804 166
anasse 0:a59a3d743804 167 p_new = rt_get_same_rdy_prio();
anasse 0:a59a3d743804 168 if (p_new != NULL) {
anasse 0:a59a3d743804 169 rt_put_prio ((P_XCB)&os_rdy, os_tsk.run);
anasse 0:a59a3d743804 170 os_tsk.run->state = READY;
anasse 0:a59a3d743804 171 rt_switch_req (p_new);
anasse 0:a59a3d743804 172 }
anasse 0:a59a3d743804 173 }
anasse 0:a59a3d743804 174
anasse 0:a59a3d743804 175
anasse 0:a59a3d743804 176 /*--------------------------- rt_tsk_self -----------------------------------*/
anasse 0:a59a3d743804 177
anasse 0:a59a3d743804 178 OS_TID rt_tsk_self (void) {
anasse 0:a59a3d743804 179 /* Return own task identifier value. */
anasse 0:a59a3d743804 180 if (os_tsk.run == NULL) {
anasse 0:a59a3d743804 181 return (0);
anasse 0:a59a3d743804 182 }
anasse 0:a59a3d743804 183 return (os_tsk.run->task_id);
anasse 0:a59a3d743804 184 }
anasse 0:a59a3d743804 185
anasse 0:a59a3d743804 186
anasse 0:a59a3d743804 187 /*--------------------------- rt_tsk_prio -----------------------------------*/
anasse 0:a59a3d743804 188
anasse 0:a59a3d743804 189 OS_RESULT rt_tsk_prio (OS_TID task_id, U8 new_prio) {
anasse 0:a59a3d743804 190 /* Change execution priority of a task to "new_prio". */
anasse 0:a59a3d743804 191 P_TCB p_task;
anasse 0:a59a3d743804 192
anasse 0:a59a3d743804 193 if (task_id == 0) {
anasse 0:a59a3d743804 194 /* Change execution priority of calling task. */
anasse 0:a59a3d743804 195 os_tsk.run->prio = new_prio;
anasse 0:a59a3d743804 196 run:if (rt_rdy_prio() > new_prio) {
anasse 0:a59a3d743804 197 rt_put_prio (&os_rdy, os_tsk.run);
anasse 0:a59a3d743804 198 os_tsk.run->state = READY;
anasse 0:a59a3d743804 199 rt_dispatch (NULL);
anasse 0:a59a3d743804 200 }
anasse 0:a59a3d743804 201 return (OS_R_OK);
anasse 0:a59a3d743804 202 }
anasse 0:a59a3d743804 203
anasse 0:a59a3d743804 204 /* Find the task in the "os_active_TCB" array. */
anasse 0:a59a3d743804 205 if (task_id > os_maxtaskrun || os_active_TCB[task_id-1] == NULL) {
anasse 0:a59a3d743804 206 /* Task with "task_id" not found or not started. */
anasse 0:a59a3d743804 207 return (OS_R_NOK);
anasse 0:a59a3d743804 208 }
anasse 0:a59a3d743804 209 p_task = os_active_TCB[task_id-1];
anasse 0:a59a3d743804 210 p_task->prio = new_prio;
anasse 0:a59a3d743804 211 if (p_task == os_tsk.run) {
anasse 0:a59a3d743804 212 goto run;
anasse 0:a59a3d743804 213 }
anasse 0:a59a3d743804 214 rt_resort_prio (p_task);
anasse 0:a59a3d743804 215 if (p_task->state == READY) {
anasse 0:a59a3d743804 216 /* Task enqueued in a ready list. */
anasse 0:a59a3d743804 217 p_task = rt_get_first (&os_rdy);
anasse 0:a59a3d743804 218 rt_dispatch (p_task);
anasse 0:a59a3d743804 219 }
anasse 0:a59a3d743804 220 return (OS_R_OK);
anasse 0:a59a3d743804 221 }
anasse 0:a59a3d743804 222
anasse 0:a59a3d743804 223 /*--------------------------- rt_tsk_delete ---------------------------------*/
anasse 0:a59a3d743804 224
anasse 0:a59a3d743804 225 OS_RESULT rt_tsk_delete (OS_TID task_id) {
anasse 0:a59a3d743804 226 /* Terminate the task identified with "task_id". */
anasse 0:a59a3d743804 227 P_TCB task_context;
anasse 0:a59a3d743804 228
anasse 0:a59a3d743804 229 if (task_id == 0 || task_id == os_tsk.run->task_id) {
anasse 0:a59a3d743804 230 /* Terminate itself. */
anasse 0:a59a3d743804 231 os_tsk.run->state = INACTIVE;
anasse 0:a59a3d743804 232 os_tsk.run->tsk_stack = 0;
anasse 0:a59a3d743804 233 rt_stk_check ();
anasse 0:a59a3d743804 234 os_active_TCB[os_tsk.run->task_id-1] = NULL;
anasse 0:a59a3d743804 235
anasse 0:a59a3d743804 236 os_tsk.run->stack = NULL;
anasse 0:a59a3d743804 237 DBG_TASK_NOTIFY(os_tsk.run, __FALSE);
anasse 0:a59a3d743804 238 os_tsk.run = NULL;
anasse 0:a59a3d743804 239 rt_dispatch (NULL);
anasse 0:a59a3d743804 240 /* The program should never come to this point. */
anasse 0:a59a3d743804 241 }
anasse 0:a59a3d743804 242 else {
anasse 0:a59a3d743804 243 /* Find the task in the "os_active_TCB" array. */
anasse 0:a59a3d743804 244 if (task_id > os_maxtaskrun || os_active_TCB[task_id-1] == NULL) {
anasse 0:a59a3d743804 245 /* Task with "task_id" not found or not started. */
anasse 0:a59a3d743804 246 return (OS_R_NOK);
anasse 0:a59a3d743804 247 }
anasse 0:a59a3d743804 248 task_context = os_active_TCB[task_id-1];
anasse 0:a59a3d743804 249 rt_rmv_list (task_context);
anasse 0:a59a3d743804 250 rt_rmv_dly (task_context);
anasse 0:a59a3d743804 251 os_active_TCB[task_id-1] = NULL;
anasse 0:a59a3d743804 252
anasse 0:a59a3d743804 253 task_context->stack = NULL;
anasse 0:a59a3d743804 254 DBG_TASK_NOTIFY(task_context, __FALSE);
anasse 0:a59a3d743804 255 }
anasse 0:a59a3d743804 256 return (OS_R_OK);
anasse 0:a59a3d743804 257 }
anasse 0:a59a3d743804 258
anasse 0:a59a3d743804 259
anasse 0:a59a3d743804 260 /*--------------------------- rt_sys_init -----------------------------------*/
anasse 0:a59a3d743804 261
anasse 0:a59a3d743804 262 #ifdef __CMSIS_RTOS
anasse 0:a59a3d743804 263 void rt_sys_init (void) {
anasse 0:a59a3d743804 264 #else
anasse 0:a59a3d743804 265 void rt_sys_init (FUNCP first_task, U32 prio_stksz, void *stk) {
anasse 0:a59a3d743804 266 #endif
anasse 0:a59a3d743804 267 /* Initialize system and start up task declared with "first_task". */
anasse 0:a59a3d743804 268 U32 i;
anasse 0:a59a3d743804 269
anasse 0:a59a3d743804 270 DBG_INIT();
anasse 0:a59a3d743804 271
anasse 0:a59a3d743804 272 /* Initialize dynamic memory and task TCB pointers to NULL. */
anasse 0:a59a3d743804 273 for (i = 0; i < os_maxtaskrun; i++) {
anasse 0:a59a3d743804 274 os_active_TCB[i] = NULL;
anasse 0:a59a3d743804 275 }
anasse 0:a59a3d743804 276
anasse 0:a59a3d743804 277 /* Set up TCB of idle demon */
anasse 0:a59a3d743804 278 os_idle_TCB.task_id = 255;
anasse 0:a59a3d743804 279 os_idle_TCB.priv_stack = idle_task_stack_size;
anasse 0:a59a3d743804 280 os_idle_TCB.stack = idle_task_stack;
anasse 0:a59a3d743804 281 rt_init_context (&os_idle_TCB, 0, os_idle_demon);
anasse 0:a59a3d743804 282
anasse 0:a59a3d743804 283 /* Set up ready list: initially empty */
anasse 0:a59a3d743804 284 os_rdy.cb_type = HCB;
anasse 0:a59a3d743804 285 os_rdy.p_lnk = NULL;
anasse 0:a59a3d743804 286 /* Set up delay list: initially empty */
anasse 0:a59a3d743804 287 os_dly.cb_type = HCB;
anasse 0:a59a3d743804 288 os_dly.p_dlnk = NULL;
anasse 0:a59a3d743804 289 os_dly.p_blnk = NULL;
anasse 0:a59a3d743804 290 os_dly.delta_time = 0;
anasse 0:a59a3d743804 291
anasse 0:a59a3d743804 292 /* Fix SP and systemvariables to assume idle task is running */
anasse 0:a59a3d743804 293 /* Transform main program into idle task by assuming idle TCB */
anasse 0:a59a3d743804 294 #ifndef __CMSIS_RTOS
anasse 0:a59a3d743804 295 rt_set_PSP (os_idle_TCB.tsk_stack);
anasse 0:a59a3d743804 296 #endif
anasse 0:a59a3d743804 297 os_tsk.run = &os_idle_TCB;
anasse 0:a59a3d743804 298 os_tsk.run->state = RUNNING;
anasse 0:a59a3d743804 299
anasse 0:a59a3d743804 300 /* Initialize ps queue */
anasse 0:a59a3d743804 301 os_psq->first = 0;
anasse 0:a59a3d743804 302 os_psq->last = 0;
anasse 0:a59a3d743804 303 os_psq->size = os_fifo_size;
anasse 0:a59a3d743804 304
anasse 0:a59a3d743804 305 rt_init_robin ();
anasse 0:a59a3d743804 306
anasse 0:a59a3d743804 307 /* Intitialize SVC and PendSV */
anasse 0:a59a3d743804 308 rt_svc_init ();
anasse 0:a59a3d743804 309
anasse 0:a59a3d743804 310 #ifndef __CMSIS_RTOS
anasse 0:a59a3d743804 311 /* Intitialize and start system clock timer */
anasse 0:a59a3d743804 312 os_tick_irqn = os_tick_init ();
anasse 0:a59a3d743804 313 if (os_tick_irqn >= 0) {
anasse 0:a59a3d743804 314 OS_X_INIT(os_tick_irqn);
anasse 0:a59a3d743804 315 }
anasse 0:a59a3d743804 316
anasse 0:a59a3d743804 317 /* Start up first user task before entering the endless loop */
anasse 0:a59a3d743804 318 rt_tsk_create (first_task, prio_stksz, stk, NULL);
anasse 0:a59a3d743804 319 #endif
anasse 0:a59a3d743804 320 }
anasse 0:a59a3d743804 321
anasse 0:a59a3d743804 322
anasse 0:a59a3d743804 323 /*--------------------------- rt_sys_start ----------------------------------*/
anasse 0:a59a3d743804 324
anasse 0:a59a3d743804 325 #ifdef __CMSIS_RTOS
anasse 0:a59a3d743804 326 void rt_sys_start (void) {
anasse 0:a59a3d743804 327 /* Start system */
anasse 0:a59a3d743804 328
anasse 0:a59a3d743804 329 /* Intitialize and start system clock timer */
anasse 0:a59a3d743804 330 os_tick_irqn = os_tick_init ();
anasse 0:a59a3d743804 331 if (os_tick_irqn >= 0) {
anasse 0:a59a3d743804 332 OS_X_INIT(os_tick_irqn);
anasse 0:a59a3d743804 333 }
anasse 0:a59a3d743804 334 extern void RestoreContext();
anasse 0:a59a3d743804 335 RestoreContext(); // Start the first task
anasse 0:a59a3d743804 336 }
anasse 0:a59a3d743804 337 #endif
anasse 0:a59a3d743804 338
anasse 0:a59a3d743804 339 /*----------------------------------------------------------------------------
anasse 0:a59a3d743804 340 * end of file
anasse 0:a59a3d743804 341 *---------------------------------------------------------------------------*/