PageRenderTime 467ms CodeModel.GetById 19ms RepoModel.GetById 0ms app.codeStats 0ms

/software/patches-essential/0005-CM730-firmware-changes-for-NimbRo-OP.patch

https://github.com/NimbRo/nimbro-op
Patch | 283 lines | 239 code | 44 blank | 0 comment | 0 complexity | 3bf843626c57a67a3261ea33b7412720 MD5 | raw file
Possible License(s): MPL-2.0-no-copyleft-exception, BSD-3-Clause 
    

  1. From 53a5016aa2b4afd0a846c4ead873da31042b70cb Mon Sep 17 00:00:00 2001
    2. 

  2. From: sebastian <sebastian>
    3. 

  3. Date: Fri, 12 Oct 2012 12:08:28 +0200
    4. 

  4. Subject: [PATCH 05/25] CM730 firmware changes for NimbRo-OP
    5. 

  5. 

  6. Changelist:
    7. 

  7.  - firmware: changed toolchain prefix
    8. 

  8.  - firmware: adapted voltage thresholds for 4 cells
    9. 

  9.  - firmware: tolerate voltages below 13.0V (power supply)
    10. 

  10.  - firmware: fixed timer interrupt bug
    11. 

  11.  - firmware: enable higher resolution on gyro
    12. 

  12.  - firmware: switch dxl torque off on startup/reset
    13. 

  13. ---
    14. 

  14.  firmware/CM730_APP/inc/CM_DXL_COM.h   |    2 ++
    15. 

  15.  firmware/CM730_APP/src/CM_DXL_COM.c   |   26 ++++++++++++++++++++++++++
    16. 

  16.  firmware/CM730_APP/src/isr.c          |   14 +++++++-------
    17. 

  17.  firmware/CM730_APP/src/main.c         |   13 ++++++++++---
    18. 

  18.  firmware/CM730_APP/src/stm32f10x_it.c |    9 +++++----
    19. 

  19.  firmware/CM730_HW/inc/usart.h         |    2 +-
    20. 

  20.  firmware/CM730_HW/src/gyro_acc.c      |    2 +-
    21. 

  21.  firmware/CM730_HW/src/usart.c         |   24 ++++++++++++++++--------
    22. 

  22.  firmware/Makefile                     |    2 +-
    23. 

  23.  9 files changed, 69 insertions(+), 25 deletions(-)
    24. 

  24. 

  25. diff --git a/firmware/CM730_APP/inc/CM_DXL_COM.h b/firmware/CM730_APP/inc/CM_DXL_COM.h
    26. 

  26. index 519e7a8..7b0d91a 100644
    27. 

  27. --- a/firmware/CM730_APP/inc/CM_DXL_COM.h
    28. 

  28. +++ b/firmware/CM730_APP/inc/CM_DXL_COM.h
    29. 

  29. @@ -283,6 +283,8 @@ extern volatile byte gbLEDPwm;
    30. 

  30.  

    31. 

  31.  void Process(void);

    32. 

  32.  

    33. 

  33. +void TorqueOff();
    34. 

  34. +
    35. 

  35.  

    36. 

  36.  #endif /* _DYNAMIXEL_CM_DXL_COM_HEADER */

    37. 

  37.  

    38. 

  38. diff --git a/firmware/CM730_APP/src/CM_DXL_COM.c b/firmware/CM730_APP/src/CM_DXL_COM.c
    39. 

  39. index 2c34671..0a9ae4f 100644
    40. 

  40. --- a/firmware/CM730_APP/src/CM_DXL_COM.c
    41. 

  41. +++ b/firmware/CM730_APP/src/CM_DXL_COM.c
    42. 

  42. @@ -344,6 +344,28 @@ void ProcessAfterWriting(void);
    43. 

  43.  

    44. 

  44.  u8 gbDxlPwr;

    45. 

  45.  

    46. 

  46. +void TorqueOff(void)
    47. 

  47. +{
    48. 

  48. +	gbTxD0BufferReadPointer = gbTxD0BufferWritePointer = 0;
    49. 

  49. +	gbpTxD0Buffer[gbTxD0BufferWritePointer++] = 0xff;
    50. 

  50. +	gbpTxD0Buffer[gbTxD0BufferWritePointer++] = 0xff;
    51. 

  51. +	gbpTxD0Buffer[gbTxD0BufferWritePointer++] = 0xFE; // Broadcast
    52. 

  52. +	gbpTxD0Buffer[gbTxD0BufferWritePointer++] = 4;
    53. 

  53. +	gbpTxD0Buffer[gbTxD0BufferWritePointer++] = INST_WRITE;
    54. 

  54. +	gbpTxD0Buffer[gbTxD0BufferWritePointer++] = 0x18; // Torque enable
    55. 

  55. +	gbpTxD0Buffer[gbTxD0BufferWritePointer++] = 0x00; // off
    56. 

  56. +	gbpTxD0Buffer[gbTxD0BufferWritePointer++] = ~((u8)(0xFE + 4 + INST_WRITE + 0x18 + 0x00));
    57. 

  57. +
    58. 

  58. +	if ( GPIO_ReadOutputDataBit(PORT_ENABLE_TXD, PIN_ENABLE_TXD) == Bit_RESET) {
    59. 

  59. +		//TxDString(USART_ZIGBEE,"\r\n TEST0");
    60. 

  60. +		//if (TXD0_FINISH) {
    61. 

  61. +		GPIO_ResetBits(PORT_ENABLE_RXD, PIN_ENABLE_RXD);	// RX Disable
    62. 

  62. +		GPIO_SetBits(PORT_ENABLE_TXD, PIN_ENABLE_TXD);	// TX Enable
    63. 

  63. +
    64. 

  64. +		USART_SendData(USART1, gbpTxD0Buffer[gbTxD0BufferReadPointer++]);
    65. 

  65. +		USART_ITConfig(USART1, USART_IT_TC, ENABLE);
    66. 

  66. +	}
    67. 

  67. +}
    68. 

  68.  

    69. 

  69.  void Process(void)

    70. 

  70.  {

    71. 

  71. @@ -934,6 +956,10 @@ void ProcessAfterWriting(void)
    72. 

  72.  

    73. 

  73.  		case	P_DYNAMIXEL_POWER:

    74. 

  74.  				dxl_set_power(GB_DYNAMIXEL_POWER);

    75. 

  75. +				if(GB_DYNAMIXEL_POWER == ON)
    76. 

  76. +				{
    77. 

  77. +					enableDXLForwarding();
    78. 

  78. +				}
    79. 

  79.  				gbDxlPwr = GB_DYNAMIXEL_POWER;

    80. 

  80.  		break;

    81. 

  81.  		case	P_LED_PANNEL:

    82. 

  82. diff --git a/firmware/CM730_APP/src/isr.c b/firmware/CM730_APP/src/isr.c
    83. 

  83. index 39c4bcd..19b5595 100644
    84. 

  84. --- a/firmware/CM730_APP/src/isr.c
    85. 

  85. +++ b/firmware/CM730_APP/src/isr.c
    86. 

  86. @@ -25,12 +25,12 @@
    87. 

  87.  #define BLINK_COUNT		10 //(0.5s * 10 = 5s)

    88. 

  88.  #define BLINK_TIME		1042 //(480us * 1042 = 0.5s)

    89. 

  89.  

    90. 

  90. -#define VOLTAGE_LEVEL_5     121     //12.05V

    91. 

  91. -#define VOLTAGE_LEVEL_4     116     //11.62V

    92. 

  92. -#define VOLTAGE_LEVEL_3     114     //11.40V

    93. 

  93. -#define VOLTAGE_LEVEL_2     112     //11.25V

    94. 

  94. -#define VOLTAGE_LEVEL_1     110     //11.05V

    95. 

  95. -#define VOLTAGE_LEVEL_0     106     //10.64V

    96. 

  96. +#define VOLTAGE_LEVEL_5     161     //12.05V
    97. 

  97. +#define VOLTAGE_LEVEL_4     155     //11.62V
    98. 

  98. +#define VOLTAGE_LEVEL_3     152     //11.40V
    99. 

  99. +#define VOLTAGE_LEVEL_2     149     //11.25V
    100. 

  100. +#define VOLTAGE_LEVEL_1     147     //11.05V
    101. 

  101. +#define VOLTAGE_LEVEL_0     141     //10.64V
    102. 

  102.  #define LOW_BATTERY_COUNT       20  //

    103. 

  103.  

    104. 

  104.  #define	SOUND_LOW_VOLTAGE_LENGTH    255

    105. 

  105. @@ -57,7 +57,7 @@ void ISR_BATTERY_CHECK(void)
    106. 

  106.  	static byte bLowBatteryCount;

    107. 

  107.  	GB_ADC0_VOLTAGE = getVoltage();

    108. 

  108.  

    109. 

  109. -	if (  GB_ADC0_VOLTAGE <= VOLTAGE_LEVEL_1 ) {

    110. 

  110. +	if (  GB_ADC0_VOLTAGE <= VOLTAGE_LEVEL_1 && GB_ADC0_VOLTAGE > 130) {
    111. 

  111.  		if (bLowBatteryCount < LOW_BATTERY_COUNT) bLowBatteryCount++;

    112. 

  112.  	}

    113. 

  113.  	else {

    114. 

  114. diff --git a/firmware/CM730_APP/src/main.c b/firmware/CM730_APP/src/main.c
    115. 

  115. index f560a99..6d506fd 100644
    116. 

  116. --- a/firmware/CM730_APP/src/main.c
    117. 

  117. +++ b/firmware/CM730_APP/src/main.c
    118. 

  118. @@ -48,8 +48,7 @@ int main(void)
    119. 

  119.  

    120. 

  120.  	System_Configuration();

    121. 

  121.  

    122. 

  122. -	dxl_set_power(OFF);

    123. 

  123. -	//dxl_set_power(ON);

    124. 

  124. +	dxl_set_power(ON);
    125. 

  125.  	Zigbee_SetState(ON);

    126. 

  126.  	//gbDxlPwr = ON;

    127. 

  127.  

    128. 

  128. @@ -136,8 +135,14 @@ int main(void)
    129. 

  129.      gbLEDBlinkCounter = 8;

    130. 

  130.  

    131. 

  131.  

    132. 

  132. +	for (bCount = 0; bCount < 3; bCount++) {
    133. 

  133. +		LED_SetState(LED_MANAGE|LED_EDIT|LED_PLAY, ON);
    134. 

  134. +		mDelay(50);
    135. 

  135. +		LED_SetState(LED_MANAGE|LED_EDIT|LED_PLAY, OFF);
    136. 

  136. +		mDelay(50);
    137. 

  137. +	}
    138. 

  138.  

    139. 

  139. -

    140. 

  140. +	TorqueOff();
    141. 

  141.  

    142. 

  142.  	for (bCount = 0; bCount < 3; bCount++) {

    143. 

  143.  		LED_SetState(LED_MANAGE|LED_EDIT|LED_PLAY, ON);

    144. 

  144. @@ -145,6 +150,8 @@ int main(void)
    145. 

  145.  		LED_SetState(LED_MANAGE|LED_EDIT|LED_PLAY, OFF);

    146. 

  146.  		mDelay(50);

    147. 

  147.  	}

    148. 

  148. +
    149. 

  149. +	dxl_set_power(OFF);
    150. 

  150.  /*

    151. 

  151.  	dxl_set_power(1);

    152. 

  152.  	gbDxlPwr = 1;

    153. 

  153. diff --git a/firmware/CM730_APP/src/stm32f10x_it.c b/firmware/CM730_APP/src/stm32f10x_it.c
    154. 

  154. index 8ac8324..0364377 100644
    155. 

  155. --- a/firmware/CM730_APP/src/stm32f10x_it.c
    156. 

  156. +++ b/firmware/CM730_APP/src/stm32f10x_it.c
    157. 

  157. @@ -38,7 +38,6 @@ extern vu16 CCR3_Val;       // from system_init.c
    158. 

  158.  extern vu16 CCR4_Val;       // from system_init.c

    159. 

  159.  

    160. 

  160.  

    161. 

  161. -vu32 capture = 0;

    162. 

  162.  vu8 Counter = 0;

    163. 

  163.  vu16 gwCounter1 = 0;

    164. 

  164.  

    165. 

  165. @@ -478,6 +477,8 @@ void TIM1_CC_IRQHandler(void)
    166. 

  166.  {

    167. 

  167.  }

    168. 

  168.  

    169. 

  169. +extern u8 ReadButton(void);
    170. 

  170. +
    171. 

  171.  /*******************************************************************************

    172. 

  172.  * Function Name  : TIM2_IRQHandler

    173. 

  173.  * Description    : This function handles TIM2 global interrupt request.

    174. 

  174. @@ -487,14 +488,14 @@ void TIM1_CC_IRQHandler(void)
    175. 

  175.  *******************************************************************************/

    176. 

  176.  void TIM2_IRQHandler(void)

    177. 

  177.  {

    178. 

  178. -	static b1Sec=0;

    179. 

  179. +	static byte b1Sec=0;
    180. 

  180.  

    181. 

  181.  	if (TIM_GetITStatus(TIM2, TIM_IT_CC4) != RESET) // 120us, 8000Hz

    182. 

  182.  	{

    183. 

  183.  		TIM_ClearITPendingBit(TIM2, TIM_IT_CC4);

    184. 

  184.  		ISR_ADC();

    185. 

  185. -		capture = TIM_GetCapture4(TIM2);

    186. 

  186. -		TIM_SetCompare4(TIM2, capture + CCR4_Val);

    187. 

  187. +        TIM_SetCounter(TIM2, 0);
    188. 

  188. +        TIM_SetCompare4(TIM2, CCR4_Val);
    189. 

  189.  

    190. 

  190.  

    191. 

  191.  		if( !( gwCounter1 & 7 ) ) // 840us

    192. 

  192. diff --git a/firmware/CM730_HW/inc/usart.h b/firmware/CM730_HW/inc/usart.h
    193. 

  193. index 3f5c008..805d2cb 100644
    194. 

  194. --- a/firmware/CM730_HW/inc/usart.h
    195. 

  195. +++ b/firmware/CM730_HW/inc/usart.h
    196. 

  196. @@ -32,7 +32,7 @@ void __ISR_USART_DXL(void);
    197. 

  197.  void __ISR_USART_ZIGBEE(void);

    198. 

  198.  void __ISR_USART_PC(void);

    199. 

  199.  

    200. 

  200. -

    201. 

  201. +void enableDXLForwarding();
    202. 

  202.  

    203. 

  203.  #endif /* __USART_H */

    204. 

  204.  

    205. 

  205. diff --git a/firmware/CM730_HW/src/gyro_acc.c b/firmware/CM730_HW/src/gyro_acc.c
    206. 

  206. index 8e6bbd3..f125989 100644
    207. 

  207. --- a/firmware/CM730_HW/src/gyro_acc.c
    208. 

  208. +++ b/firmware/CM730_HW/src/gyro_acc.c
    209. 

  209. @@ -263,7 +263,7 @@ void Gyro_Configuration(void)
    210. 

  210.  	while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_RXNE) == RESET);

    211. 

  211.  	Push_SPI_Data( SPI_I2S_ReceiveData(SPI2) );

    212. 

  212.  

    213. 

  213. -	SPI_I2S_SendData(SPI2,0x20);	// WRITE 0XFF TO CTRL_REG1(0X20). OUTPUT DATA RATE 800HZ, POWER : NORMAL, XYZ ENABLE.

    214. 

  214. +	SPI_I2S_SendData(SPI2,0x00);	// WRITE 0XFF TO CTRL_REG1(0X20). OUTPUT DATA RATE 800HZ, POWER : NORMAL, XYZ ENABLE.
    215. 

  215.  	while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);

    216. 

  216.  	while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_RXNE) == RESET);

    217. 

  217.  	Push_SPI_Data( SPI_I2S_ReceiveData(SPI2) );

    218. 

  218. diff --git a/firmware/CM730_HW/src/usart.c b/firmware/CM730_HW/src/usart.c
    219. 

  219. index 8f8b057..42d478f 100644
    220. 

  220. --- a/firmware/CM730_HW/src/usart.c
    221. 

  221. +++ b/firmware/CM730_HW/src/usart.c
    222. 

  222. @@ -20,6 +20,8 @@
    223. 

  223.  /* Private macro -------------------------------------------------------------*/

    224. 

  224.  /* Private variables ---------------------------------------------------------*/

    225. 

  225.  

    226. 

  226. +volatile u8 gbDXLForwarding = 0;
    227. 

  227. +
    228. 

  228.  // usart buffer

    229. 

  229.  /*

    230. 

  230.  volatile u16 gwpUSART_DXL_Buffer[USART_BUFFER_SIZE+1];

    231. 

  231. @@ -133,16 +135,18 @@ void __ISR_USART_PC(void)
    232. 

  232.  		//if (TXD0_READY) {

    233. 

  233.  

    234. 

  234.  

    235. 

  235. -					//if( GB_DYNAMIXEL_POWER)

    236. 

  236. -		if ( GPIO_ReadOutputDataBit(PORT_ENABLE_TXD, PIN_ENABLE_TXD) == Bit_RESET) {

    237. 

  237. -			//if (TXD0_FINISH) {

    238. 

  238. -			GPIO_ResetBits(PORT_ENABLE_RXD, PIN_ENABLE_RXD);	// RX Disable

    239. 

  239. -			GPIO_SetBits(PORT_ENABLE_TXD, PIN_ENABLE_TXD);	// TX Enable

    240. 

  240. +		if(gbDXLForwarding)
    241. 

  241. +		{
    242. 

  242. +			if ( GPIO_ReadOutputDataBit(PORT_ENABLE_TXD, PIN_ENABLE_TXD) == Bit_RESET) {
    243. 

  243. +				//if (TXD0_FINISH) {
    244. 

  244. +				GPIO_ResetBits(PORT_ENABLE_RXD, PIN_ENABLE_RXD);	// RX Disable
    245. 

  245. +				GPIO_SetBits(PORT_ENABLE_TXD, PIN_ENABLE_TXD);	// TX Enable
    246. 

  246.  

    247. 

  247. -			USART_SendData(USART1, gbpTxD0Buffer[gbTxD0BufferReadPointer++]);

    248. 

  248. -			//gbTxD0BufferReadPointer &= 0x3FF;

    249. 

  249. -			USART_ITConfig(USART1, USART_IT_TC, ENABLE);

    250. 

  250. +				USART_SendData(USART1, gbpTxD0Buffer[gbTxD0BufferReadPointer++]);
    251. 

  251. +				//gbTxD0BufferReadPointer &= 0x3FF;
    252. 

  252. +				USART_ITConfig(USART1, USART_IT_TC, ENABLE);
    253. 

  253.  

    254. 

  254. +			}
    255. 

  255.  		}

    256. 

  256.  

    257. 

  257.  	}

    258. 

  258. @@ -238,5 +242,9 @@ void __ISR_USART_DXL(void)
    259. 

  259.  

    260. 

  260.  }

    261. 

  261.  

    262. 

  262. +void enableDXLForwarding(void)
    263. 

  263. +{
    264. 

  264. +	gbDXLForwarding = 1;
    265. 

  265. +}
    266. 

  266.  

    267. 

  267.  /******************* (C) COPYRIGHT 2010 ROBOTIS *****END OF FILE****/

    268. 

  268. diff --git a/firmware/Makefile b/firmware/Makefile
    269. 

  269. index 68f9d93..f05f2e7 100644
    270. 

  270. --- a/firmware/Makefile
    271. 

  271. +++ b/firmware/Makefile
    272. 

  272. @@ -6,7 +6,7 @@ INCLUDE_DIRS = -I. -Istm32f10x_lib/inc -ICM730_HW/inc -ICM730_APP/inc
    273. 

  273.  #INCLUDE_DIRS = -I.

    274. 

  274.  LIBRARY_DIRS = -Lstm32f10x_lib

    275. 

  275.  

    276. 

  276. -TCHAIN_PREFIX=arm-eabi-

    277. 

  277. +TCHAIN_PREFIX=arm-none-eabi-
    278. 

  278.  

    279. 

  279.  

    280. 

  280.  CC = $(TCHAIN_PREFIX)gcc

    281. 

  281. -- 
    282. 

  282. 1.7.10.4
    283. 

  283. 

  284.