errors.cpp

00001 #include "errors.h"
00002 #include "globals.h"
00003 #include "prefs.h"
00004 
00005 int errors = 0, moded_errors = 0;
00006 int masks[32];
00007 
00008 void init_masks() {
00009     masks[ERR_THROTTLE_DISABLED] =  _MASK_MODE_RUN |
00010                                     _MASK_OP_TORQUE | _MASK_OP_DRIVING | _MASK_OP_SPEED | _MASK_OP_POSITION |
00011                                     _MASK_SRC_RC | _MASK_SRC_ANALOG;
00012     masks[ERR_POS_INVALID] =        _MASK_MODE_RUN | _MASK_MODE_ZERO | _MASK_MODE_CHR |
00013                                     _MASK_OP_TORQUE | _MASK_OP_DRIVING | _MASK_OP_SPEED | _MASK_OP_POSITION |
00014                                     _MASK_SRC_RC | _MASK_SRC_ANALOG | _MASK_SRC_TERMINAL | _MASK_SRC_SERIAL |
00015                                     _MASK_SRC_CAN | _MASK_SRC_INTERNAL;
00016     masks[ERR_NOT_DRIVING] =        _MASK_MODE_RUN |
00017                                     _MASK_OP_TORQUE | _MASK_OP_DRIVING | _MASK_OP_SPEED |
00018                                     _MASK_SRC_RC | _MASK_SRC_ANALOG | _MASK_SRC_TERMINAL | _MASK_SRC_SERIAL |
00019                                     _MASK_SRC_CAN | _MASK_SRC_INTERNAL;
00020 }
00021 
00022 void go_enabled() {
00023     control.enabled = true;
00024     io.en->write(1);
00025 }
00026 
00027 void go_disabled() {
00028     control.d_integral = 0.0f;
00029     control.q_integral = 0.0f;
00030     control.w_integral = 0.0f;
00031     control.enabled = false;
00032     io.en->write(0);
00033 }
00034 
00035 bool is_driving() {
00036     return fabsf(control.user_cmd) > 0.01f || fabsf(control.torque_percent) > 0.01f || fabsf(read.w) > _W_SAFE;
00037 }
00038 
00039 #define _seterr(x) errors |= (1 << x)
00040 #define _unseterr(x) errors &= ~(1 << x)
00041 #define _upderr(x) _unseterr(x) : _seterr(x)
00042 void check_errors_modeless() {
00043     io.throttle_in->get_enabled() ? _upderr(ERR_THROTTLE_DISABLED);
00044     io.pos->IsValid() ? _upderr(ERR_POS_INVALID);
00045     is_driving() ? _upderr(ERR_NOT_DRIVING);
00046 }
00047 
00048 bool checks_passed() {
00049     check_errors_modeless();
00050     for (int k = 0; k < NUM_ERRORS; k++) {
00051         int mask_mode = masks[k] & _mask_mode(BREMS_mode);
00052         int mask_op = masks[k] & _mask_op(BREMS_op);
00053         int mask_src = masks[k] & _mask_src(BREMS_src);
00054         if ((errors & (1 << k)) != 0 && mask_mode != 0 && mask_op != 0 && mask_src != 0) {
00055             moded_errors |= 1 << k;
00056         } else {
00057             moded_errors &= ~(1 << k);
00058         }
00059     }
00060 
00061     if (moded_errors != 0) {
00062         return false;
00063     }
00064     return true;
00065 }