Home Esplora Aiuto
Registrati Accedi
wangdan
/
N3_sub
1
0
Forka 0
File Problemi 0 Pull Requests 0 Wiki
Albero (Tree): 19b7cd0471
Rami (Branch) Tag
N3_sub
/
libraries
/
AP_UAVCAN
/
HVcallback.bad

HVcallback.bad 6.5 KB
Cronologia Originale

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123 
  1. #ifndef HVCALLBACK_H_
    2. 

  2. #define HVCALLBACK_H_
    3. 

  3. 

  4. #include <uavcan/uavcan.hpp>
    5. 

  5. 

  6. #include <AP_HAL/CAN.h>
    7. 

  7. #include <AP_HAL/Semaphores.h>
    8. 

  8. #include <AP_Param/AP_Param.h>
    9. 

  9. 

  10. #include <uavcan/helpers/heap_based_pool_allocator.hpp>
    11. 

  11. #include "AP_UAVCAN_Servers.h"
    12. 

  12. #define Fault_ACK {0x00,0x38,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}
    13. 

  13. 

  14. #define TorqueMax 15000
    15. 

  15. #define TorqueMin 500
    16. 

  16. #define HVspeedMax 2200
    17. 

  17. #define HVspeedMin 250
    18. 

  18. 

  19. #define Start 0x18
    20. 

  20. #define Stop 0x20
    21. 

  21. #define ELT_IDENTIFIER_POS 6U
    22. 

  22. #define TYPE_POS 3U
    23. 

  23. 

  24. #define TYPE_DATA_8BIT    (1U << TYPE_POS)
    25. 

  25. #define TYPE_DATA_16BIT   (2U << TYPE_POS)
    26. 

  26. #define TYPE_DATA_32BIT   (3U << TYPE_POS)
    27. 

  27. #define TYPE_DATA_RAW     (5U << TYPE_POS)
    28. 

  28.   /* TYPE_DATA_8BIT registers definition */
    29. 

  29.   
    30. 

  30. #define  MC_REG_STATUS                 ((1U << ELT_IDENTIFIER_POS) | TYPE_DATA_8BIT )
    31. 

  31. #define  MC_REG_CONTROL_MODE           ((2U << ELT_IDENTIFIER_POS) | TYPE_DATA_8BIT )
    32. 

  32. #define  MC_REG_RUC_STAGE_NBR          ((3U << ELT_IDENTIFIER_POS) | TYPE_DATA_8BIT )
    33. 

  33. 

  34.   
    35. 

  35.   /* TYPE_DATA_16BIT registers definition */
    36. 

  36. #define  MC_REG_SPEED_KP               ((2U   << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    37. 

  37. #define  MC_REG_SPEED_KI               ((3U   << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    38. 

  38. #define  MC_REG_SPEED_KD               ((4U   << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    39. 

  39. #define  MC_REG_I_Q_KP                 ((6   << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    40. 

  40. #define  MC_REG_I_Q_KI                 ((7   << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    41. 

  41. #define  MC_REG_I_Q_KD                 ((8   << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    42. 

  42. #define  MC_REG_I_D_KP                 ((10  << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    43. 

  43. #define  MC_REG_I_D_KI                 ((11  << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    44. 

  44. #define  MC_REG_I_D_KD                 ((12  << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    45. 

  45. #define  MC_REG_STOPLL_C1              ((13  << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    46. 

  46. #define  MC_REG_STOPLL_C2              ((14  << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    47. 

  47. #define  MC_REG_STOCORDIC_C1           ((15  << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    48. 

  48. #define  MC_REG_STOCORDIC_C2           ((16  << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    49. 

  49. #define  MC_REG_STOPLL_KI              ((17  << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    50. 

  50. #define  MC_REG_STOPLL_KP              ((18  << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    51. 

  51. #define  MC_REG_BUS_VOLTAGE            ((22U << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    52. 

  52. #define  MC_REG_HEATS_TEMP             ((23U << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    53. 

  53. #define  MC_REG_MOTOR_POWER            ((24U << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    54. 

  54. #define  MC_REG_I_A                    ((31U << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    55. 

  55. #define  MC_REG_I_B                    ((32U << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    56. 

  56. #define  MC_REG_I_ALPHA_MEAS           ((33U << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    57. 

  57. #define  MC_REG_I_BETA_MEAS            ((34U << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    58. 

  58. #define  MC_REG_I_Q_MEAS               ((35U << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    59. 

  59. #define  MC_REG_I_D_MEAS               ((36U << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    60. 

  60. #define  MC_REG_I_Q_REF                ((37U << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    61. 

  61. #define  MC_REG_I_D_REF                ((38U << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    62. 

  62. #define  MC_REG_V_Q                    ((39U << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    63. 

  63. #define  MC_REG_V_D                    ((40U << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    64. 

  64. #define  MC_REG_V_ALPHA                ((41U << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    65. 

  65. #define  MC_REG_V_BETA                 ((42U << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    66. 

  66. #define  MC_REG_STOPLL_EL_ANGLE        ((45U << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    67. 

  67. #define  MC_REG_STOPLL_ROT_SPEED       ((46U << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    68. 

  68. #define  MC_REG_STOPLL_I_ALPHA         ((47U << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    69. 

  69. #define  MC_REG_STOPLL_I_BETA          ((48U << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    70. 

  70. #define  MC_REG_STOPLL_BEMF_ALPHA      ((49U << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    71. 

  71. #define  MC_REG_STOPLL_BEMF_BETA       ((50U << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    72. 

  72. #define  MC_REG_SPEED_KP_DIV           ((81  << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    73. 

  73. #define  MC_REG_SPEED_KI_DIV           ((82  << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    74. 

  74. #define  MC_REG_SPEED_KD_DIV           ((83  << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    75. 

  75. #define  MC_REG_I_D_KP_DIV             ((84  << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    76. 

  76. #define  MC_REG_I_D_KI_DIV             ((85  << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    77. 

  77. #define  MC_REG_I_D_KD_DIV             ((86  << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    78. 

  78. #define  MC_REG_I_Q_KP_DIV             ((87  << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    79. 

  79. #define  MC_REG_I_Q_KI_DIV             ((88  << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    80. 

  80. #define  MC_REG_I_Q_KD_DIV             ((89  << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    81. 

  81. #define  MC_REG_STOPLL_KI_DIV          ((99  << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    82. 

  82. #define  MC_REG_STOPLL_KP_DIV          ((100 << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    83. 

  83. 

  84. #define  MC_REG_STARTUP_CURRENT_REF    ((105 << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    85. 

  85. #define  MC_REG_PULSE_VALUE            ((106 << ELT_IDENTIFIER_POS) | TYPE_DATA_16BIT )
    86. 

  86.   
    87. 

  87.   /* TYPE_DATA_32BIT registers definition */
    88. 

  88. #define  MC_REG_FAULTS_FLAGS           ((0  << ELT_IDENTIFIER_POS) | TYPE_DATA_32BIT )
    89. 

  89. #define  MC_REG_SPEED_MEAS             ((1  << ELT_IDENTIFIER_POS) | TYPE_DATA_32BIT )
    90. 

  90. #define  MC_REG_SPEED_REF              ((2  << ELT_IDENTIFIER_POS) | TYPE_DATA_32BIT )
    91. 

  91. #define  MC_REG_STOPLL_EST_BEMF        ((3  << ELT_IDENTIFIER_POS) | TYPE_DATA_32BIT ) /* To be checked shifted by >> 16*/
    92. 

  92. #define  MC_REG_STOPLL_OBS_BEMF        ((4  << ELT_IDENTIFIER_POS) | TYPE_DATA_32BIT ) /* To be checked shifted by >> 16*/
    93. 

  93. 

  94.   
    95. 

  95. 

  96. #define  MC_REG_SPEED_RAMP            ((6U << ELT_IDENTIFIER_POS) | TYPE_DATA_RAW )
    97. 

  97. #define  MC_REG_TORQUE_RAMP           ((7U << ELT_IDENTIFIER_POS) | TYPE_DATA_RAW )
    98. 

  98. #define  MC_REG_REVUP_DATA            ((8U << ELT_IDENTIFIER_POS) | TYPE_DATA_RAW ) /* Configure all steps*/
    99. 

  99. #define  MC_REG_CURRENT_REF           ((13U  << ELT_IDENTIFIER_POS) | TYPE_DATA_RAW )
    100. 

  100. 

  101.   struct HVmes
    102. 

  102.   {
    103. 

  103. 	  int16_t voltage;
    104. 

  104. 	  int32_t speed;
    105. 

  105. 	  uint32_t fault;
    106. 

  106. 	  uint16_t state;
    107. 

  107. 	  int16_t power;
    108. 

  108. 	  int16_t temperature;
    109. 

  109. 	  
    110. 

  110.   };
    111. 

  111. 

  112.   extern uint8_t hv_motor_cmd_temp ;
    113. 

  113.   extern uint8_t hv_motor_cmd;
    114. 

  114.   extern int16_t hv_motor_speed_32 ;
    115. 

  115. 

  116. 

  117. void HVregs_v5y4(uint8_t _buffer[],int32_t data[]);
    118. 

  118. void HVcycle(uint8_t _code,uint8_t _buffer[6],HVmes &Thruster);
    119. 

  119. 

  120. 

  121. #endif
    122. 

  122. 

  123.