diff options
Diffstat (limited to 'src/actuators.c')
| -rwxr-xr-x | src/actuators.c | 128 |
1 files changed, 57 insertions, 71 deletions
diff --git a/src/actuators.c b/src/actuators.c index 5d65a64..dcb64d0 100755 --- a/src/actuators.c +++ b/src/actuators.c @@ -77,6 +77,7 @@ static float yaw; static int modules_init (void); static int modules_deinit (void); static int proc_actuators_it (void); +static int proc_actuators_ev (void); static int proc_actuators_delay (void); static int proc_ref_read (void); static int proc_actuators_set (void); @@ -93,22 +94,18 @@ static int proc_actuators_set (void); int proc_actuators_init (void) { - if (init_pending) { - init_pending = false; - } else { + if (!init_pending) return ERROR_OK; - } - if (modules_init()) { - goto err_mod; - } + if (modules_init()) + goto err; - return ERROR_OK; + init_pending = false; + return ERROR_OK; -err_mod: - init_pending = true; +err: return ERROR_NOK; } @@ -121,17 +118,15 @@ int proc_actuators_deinit (void) { int status; - status = ERROR_OK; - - if (!init_pending) { - init_pending = true; - } else { + if (init_pending) return status; - } - if (modules_deinit()) { + init_pending = true; + + status = ERROR_OK; + + if (modules_deinit()) status = ERROR_NOK; - } return status; } @@ -145,26 +140,20 @@ int proc_actuators (void) { if (init_pending) { - if (proc_actuators_init()) { + if (proc_actuators_init()) return ERROR_NOK; - } } switch (PROJECT_ACT_TASKING) { case PROJECT_ACT_DEL: - if (proc_actuators_it()) { - return ERROR_NOK; - } - break; + return proc_actuators_delay(); case PROJECT_ACT_IT: + return proc_actuators_it(); case PROJECT_ACT_EV: - if (proc_actuators_delay()) { - return ERROR_NOK; - } - break; + return proc_actuators_ev(); + default: + return ERROR_NOK; } - - return ERROR_OK; } @@ -176,21 +165,17 @@ static int modules_init (void) led_init(); button_init(); - if (delay_us_init()) { + if (delay_us_init()) goto err_delay; - } - if (can_init()) { + if (can_init()) goto err_can; - } - if (servo_init()) { + if (servo_init()) goto err_servo; - } if (PROJECT_ACT_TASKING != PROJECT_ACT_DEL) { - if (tim_it_init(ACT_REFRESH_PERIOD_US)) { + if (tim_it_init(ACT_REFRESH_PERIOD_US)) goto err_tim; - } } return ERROR_OK; @@ -215,18 +200,14 @@ static int modules_deinit (void) status = ERROR_OK; - if (servo_deinit()) { + if (servo_deinit()) status = ERROR_NOK; - } - if (can_deinit()) { + if (can_deinit()) status = ERROR_NOK; - } - if (tim_it_deinit()) { + if (tim_it_deinit()) status = ERROR_NOK; - } - if (delay_us_deinit()) { + if (delay_us_deinit()) status = ERROR_NOK; - } button_deinit(); led_deinit(); @@ -237,20 +218,33 @@ static int proc_actuators_it (void) { while (true) { - switch (PROJECT_ACT_TASKING) { - case PROJECT_ACT_IT: - __WFI(); - break; - case PROJECT_ACT_EV: - __WFE(); - break; + __WFI(); + + if (*tim_it_timx_interrupt_ptr) { + if (!proc_ref_read()) { + if (proc_actuators_set()) + return ERROR_NOK; + } + + *tim_it_timx_interrupt_ptr = false; + } else if (button_interrupt) { + led_toggle(); + + button_interrupt = false; } + } +} + +static int proc_actuators_ev (void) +{ + + while (true) { + __WFE(); if (*tim_it_timx_interrupt_ptr) { if (!proc_ref_read()) { - if (proc_actuators_set()) { + if (proc_actuators_set()) return ERROR_NOK; - } } *tim_it_timx_interrupt_ptr = false; @@ -269,9 +263,8 @@ static int proc_actuators_delay (void) delay_us(ACT_REFRESH_PERIOD_US); if (!proc_ref_read()) { - if (proc_actuators_set()) { + if (proc_actuators_set()) return ERROR_NOK; - } } } } @@ -280,9 +273,8 @@ static int proc_ref_read (void) { int8_t plane_pos [CAN_DATA_LEN]; - if (can_msg_read((uint8_t *)plane_pos)) { + if (can_msg_read((uint8_t *)plane_pos)) return ERROR_NOK; - } pitch = plane_pos[0]; roll = plane_pos[1]; @@ -296,33 +288,27 @@ static int proc_actuators_set (void) float tmp; tmp = alx_scale_linear_f(pitch, -40, 40, -90, 90); - if (servo_position_set(SERVO_S1, tmp)) { + if (servo_position_set(SERVO_S1, tmp)) return ERROR_NOK; - } tmp = alx_scale_linear_f(roll, -35, 35, -90, -20); - if (servo_position_set(SERVO_S2, tmp)) { + if (servo_position_set(SERVO_S2, tmp)) return ERROR_NOK; - } if (yaw > 0) { - if (servo_position_set(SERVO_S3, 90)) { + if (servo_position_set(SERVO_S3, 90)) return ERROR_NOK; - } tmp = alx_scale_linear_f(yaw, 0, 10, -90, 90); - if (servo_position_set(SERVO_S4, tmp)) { + if (servo_position_set(SERVO_S4, tmp)) return ERROR_NOK; - } } else { tmp = alx_scale_linear_f(yaw, -10, 0, -90, 90); - if (servo_position_set(SERVO_S3, tmp)) { + if (servo_position_set(SERVO_S3, tmp)) return ERROR_NOK; - } - if (servo_position_set(SERVO_S4, -90)) { + if (servo_position_set(SERVO_S4, -90)) return ERROR_NOK; - } } return ERROR_OK; |