OP-21/Flight Bootloader - AHRS programming fully working

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1681 ebee16cc-31ac-478f-84a7-5cbb03baadba

flight/Bootloaders/AHRS/main.c

@@ -40,12 +40,15 @@
 #define NSS_HOLD_STATE       ((GPIOB->IDR & GPIO_Pin_12) ? 0 : 1)
 enum bootloader_status boot_status;
 /* Private typedef -----------------------------------------------------------*/
-typedef void (*pFunction)(void);
+typedef void
+(*pFunction)(void);
 pFunction Jump_To_Application;
 uint32_t JumpAddress;
 /* Function Prototypes */
-void process_spi_request(void);
-void jump_to_app();
+void
+process_spi_request(void);
+void
+jump_to_app();
 uint8_t jumpFW = FALSE;
 uint32_t Fw_crc;
 /**
@@ -56,23 +59,25 @@ int main() {
 	uint8_t GO_dfu = false;
 	/* Brings up System using CMSIS functions, enables the LEDs. */
 	PIOS_SYS_Init();
-	 /* Enable Prefetch Buffer */
-		    FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
+	/* Enable Prefetch Buffer */
+	FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
 
-		    /* Flash 2 wait state */
-		    FLASH_SetLatency(FLASH_Latency_2);
-			RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);
+	/* Flash 2 wait state */
+	FLASH_SetLatency(FLASH_Latency_2);
+	RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);
 	/* Delay system */
 	PIOS_DELAY_Init();
-	for (uint32_t t = 0; t < 20000000; ++t) {
-		if (NSS_HOLD_STATE == 0)
+
+	for (uint32_t t = 0; t < 2; ++t) {
+		if (NSS_HOLD_STATE == 1)
 			GO_dfu = TRUE;
 		else {
 			GO_dfu = FALSE;
 			break;
 		}
+		PIOS_DELAY_WaitmS(1000);
 	}
-	GO_dfu=TRUE;
+	//GO_dfu = TRUE;
 	GO_dfu = GO_dfu;// OR with app boot request
 	if (GO_dfu == FALSE) {
 		jump_to_app();
@@ -126,14 +131,14 @@ void process_spi_request(void) {
 		return;
 	}
 
-
 	switch (user_rx_v0.payload.user.t) {
 
 	case OPAHRS_MSG_V0_REQ_FWUP_VERIFY:
 		opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
 		Fw_crc = crc_memory_calc();
 		lfsm_user_set_tx_v0(&user_tx_v0);
-		//boot_status=idle;
+		boot_status = idle;
+		PIOS_LED_Off(LED1);
 		user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
 		break;
 	case OPAHRS_MSG_V0_REQ_RESET:
@@ -141,7 +146,6 @@ void process_spi_request(void) {
 		PIOS_SYS_Reset();
 		break;
 	case OPAHRS_MSG_V0_REQ_VERSIONS:
-		//PIOS_LED_Toggle(LED1);
 		opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_VERSIONS);
 		user_tx_v0.payload.user.v.rsp.versions.bl_version = BOOTLOADER_VERSION;
 		user_tx_v0.payload.user.v.rsp.versions.hw_version = HW_VERSION;
@@ -153,15 +157,14 @@ void process_spi_request(void) {
 	case OPAHRS_MSG_V0_REQ_MEM_MAP:
 		opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_MEM_MAP);
 		user_tx_v0.payload.user.v.rsp.mem_map.density = HW_TYPE;
-		user_tx_v0.payload.user.v.rsp.mem_map.rw_flags = 0;//TODO
+		user_tx_v0.payload.user.v.rsp.mem_map.rw_flags = (BOARD_READABLE
+				| (BOARD_WRITABLA << 1));
 		user_tx_v0.payload.user.v.rsp.mem_map.size_of_code_memory
 				= SIZE_OF_CODE;
 		user_tx_v0.payload.user.v.rsp.mem_map.size_of_description
 				= SIZE_OF_DESCRIPTION;
 		user_tx_v0.payload.user.v.rsp.mem_map.start_of_user_code
 				= START_OF_USER_CODE;
-		//read_description(
-			//	(uint8_t *) &(user_tx_v0.payload.user.v.rsp.versions.description));
 		lfsm_user_set_tx_v0(&user_tx_v0);
 		break;
 	case OPAHRS_MSG_V0_REQ_SERIAL:
@@ -169,28 +172,32 @@ void process_spi_request(void) {
 		PIOS_SYS_SerialNumberGet(
 				(char *) &(user_tx_v0.payload.user.v.rsp.serial.serial_bcd));
 		lfsm_user_set_tx_v0(&user_tx_v0);
-		//PIOS_LED_Toggle(LED1);
 		break;
 	case OPAHRS_MSG_V0_REQ_FWUP_STATUS:
-			opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
-			user_tx_v0.payload.user.v.rsp.fwup_status.status=boot_status;
-			lfsm_user_set_tx_v0(&user_tx_v0);
-			PIOS_LED_Toggle(LED1);
-			break;
+		opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
+		user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
+		lfsm_user_set_tx_v0(&user_tx_v0);
+		break;
 	case OPAHRS_MSG_V0_REQ_FWUP_DATA:
+		PIOS_LED_On(LED1);
 		opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
 		if (!(user_rx_v0.payload.user.v.req.fwup_data.adress
 				< START_OF_USER_CODE)) {
-			if (FLASH_ProgramWord(
-					user_rx_v0.payload.user.v.req.fwup_data.adress,
-					user_rx_v0.payload.user.v.req.fwup_data.data)
-					!= FLASH_COMPLETE) {
-				boot_status = write_error;
+			for (uint8_t x = 0; x
+					< user_rx_v0.payload.user.v.req.fwup_data.size; ++x) {
+				if (FLASH_ProgramWord(
+						(user_rx_v0.payload.user.v.req.fwup_data.adress
+								+ ((uint32_t)(x * 4))),
+						user_rx_v0.payload.user.v.req.fwup_data.data[x])
+						!= FLASH_COMPLETE) {
+					boot_status = write_error;
+					break;
+				}
 			}
-
 		} else {
 			boot_status = outside_dev_capabilities;
 		}
+		PIOS_LED_Off(LED1);
 		user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
 		lfsm_user_set_tx_v0(&user_tx_v0);
 		break;
@@ -208,7 +215,6 @@ void process_spi_request(void) {
 			break;
 		}
 
-
 		break;
 	case OPAHRS_MSG_V0_REQ_BOOT:
 		PIOS_DELAY_WaitmS(user_rx_v0.payload.user.v.req.boot.boot_delay_in_ms);
@@ -225,6 +231,10 @@ void process_spi_request(void) {
 	return;
 }
 void jump_to_app() {
+	while (TRUE) {
+		PIOS_LED_Toggle(LED1);
+		PIOS_DELAY_WaitmS(1000);
+	}
 	if (((*(__IO uint32_t*) START_OF_USER_CODE) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */
 		FLASH_Lock();
 		RCC_APB2PeriphResetCmd(0xffffffff, ENABLE);

flight/Bootloaders/OpenPilot/main.c

@@ -81,7 +81,7 @@ int main() {
 	if (BSL_HOLD_STATE == 0)
 		USB_connected = TRUE;
 
-	GO_dfu = USB_connected || User_DFU_request;
+	GO_dfu = (USB_connected==TRUE) || (User_DFU_request==TRUE);
 	if (GO_dfu == TRUE) {
 		PIOS_Board_Init();
 		PIOS_OPAHRS_Init();

flight/Bootloaders/OpenPilot/op_dfu.c

@@ -62,7 +62,7 @@ uint8_t Data2;
 uint8_t Data3;
 uint8_t offset = 0;
 uint32_t aux;
-uint8_t spi_dev_desc[200]={0};
+uint8_t spi_dev_desc[200] = { 0 };
 //Download vars
 uint32_t downSizeOfLastPacket = 0;
 uint32_t downPacketTotal = 0;
@@ -99,10 +99,14 @@ void DataDownload(DownloadAction action) {
 			switch (currentProgrammingDestination) {
 			case Remote_flash_via_spi:
 				if (downType == Descript) {
-					SendBuffer[6 + (x * 4)] = spi_dev_desc[(uint8_t)partoffset];
-					SendBuffer[7 + (x * 4)] = spi_dev_desc[(uint8_t)partoffset + 1];
-					SendBuffer[8 + (x * 4)] = spi_dev_desc[(uint8_t)partoffset + 2];
-					SendBuffer[9 + (x * 4)] = spi_dev_desc[(uint8_t)partoffset + 3];
+					SendBuffer[6 + (x * 4)]
+							= spi_dev_desc[(uint8_t) partoffset];
+					SendBuffer[7 + (x * 4)] = spi_dev_desc[(uint8_t) partoffset
+							+ 1];
+					SendBuffer[8 + (x * 4)] = spi_dev_desc[(uint8_t) partoffset
+							+ 2];
+					SendBuffer[9 + (x * 4)] = spi_dev_desc[(uint8_t) partoffset
+							+ 3];
 				}
 				break;
 			case Self_flash:
@@ -224,7 +228,6 @@ void processComand(uint8_t *xReceive_Buffer) {
 							}
 							break;
 						default:
-
 							break;
 						}
 					}
@@ -246,51 +249,64 @@ void processComand(uint8_t *xReceive_Buffer) {
 					{
 						numberOfWords = SizeOfLastPacket;
 					}
-					for (uint8_t x = 0; x < numberOfWords; ++x) {
-						offset = 4 * x;
-						Data = xReceive_Buffer[DATA + offset] << 24;
-						Data += xReceive_Buffer[DATA + 1 + offset] << 16;
-						Data += xReceive_Buffer[DATA + 2 + offset] << 8;
-						Data += xReceive_Buffer[DATA + 3 + offset];
-						aux = baseOfAdressType(TransferType) + (uint32_t)(Count
-								* 14 * 4 + x * 4);
-						uint8_t result = 0;
-						struct opahrs_msg_v0 rsp;
-						struct opahrs_msg_v0 req;
-						switch (currentProgrammingDestination) {
-						case Self_flash:
+					struct opahrs_msg_v0 rsp;
+					struct opahrs_msg_v0 req;
+					uint8_t result = 0;
+					switch (currentProgrammingDestination) {
+					case Self_flash:
+						for (uint8_t x = 0; x < numberOfWords; ++x) {
+							offset = 4 * x;
+							Data = xReceive_Buffer[DATA + offset] << 24;
+							Data += xReceive_Buffer[DATA + 1 + offset] << 16;
+							Data += xReceive_Buffer[DATA + 2 + offset] << 8;
+							Data += xReceive_Buffer[DATA + 3 + offset];
+							aux = baseOfAdressType(TransferType) + (uint32_t)(
+									Count * 14 * 4 + x * 4);
+							uint8_t result = 0;
 							for (int retry = 0; retry < MAX_WRI_RETRYS; ++retry) {
 								if (result == 0) {
 									result = (FLASH_ProgramWord(aux, Data)
 											== FLASH_COMPLETE) ? 1 : 0;
 								}
 							}
-							break;
-						case Remote_flash_via_spi:
-							req.payload.user.v.req.fwup_data.adress = aux;
-							req.payload.user.v.req.fwup_data.data = Data;
-							if (PIOS_OPAHRS_bl_FwupData(&req, &rsp)
-									== OPAHRS_RESULT_OK) {
-								if (rsp.payload.user.v.rsp.fwup_status.status
-										== started) {
-									result = TRUE;
-								} else if (rsp.payload.user.v.rsp.fwup_status.status
-										== outside_dev_capabilities) {
-									result = TRUE;
-									DeviceState = outsideDevCapabilities;
-								} else
-									result = FALSE;
-							}
-							break;
-						default:
-							result = 0;
-							break;
 						}
-						if (result != 1) {
-							DeviceState = Last_operation_failed;
-							Aditionals = (uint32_t) Command;
+						break;
+					case Remote_flash_via_spi:
+						for (uint8_t x = 0; x < numberOfWords; ++x) {
+							offset = 4 * x;
+							Data = xReceive_Buffer[DATA + offset] << 24;
+							Data += xReceive_Buffer[DATA + 1 + offset] << 16;
+							Data += xReceive_Buffer[DATA + 2 + offset] << 8;
+							Data += xReceive_Buffer[DATA + 3 + offset];
+							req.payload.user.v.req.fwup_data.data[x] = Data;
 						}
+						aux = (baseOfAdressType(TransferType) + (uint32_t)(
+								Count * 14 * 4));
+						req.payload.user.v.req.fwup_data.adress = aux;
+						req.payload.user.v.req.fwup_data.size = numberOfWords;
+						if (PIOS_OPAHRS_bl_FwupData(&req, &rsp)
+								== OPAHRS_RESULT_OK) {
+							if (rsp.payload.user.v.rsp.fwup_status.status
+									== write_error) {
+								result = FALSE;
+							} else if (rsp.payload.user.v.rsp.fwup_status.status
+									== outside_dev_capabilities) {
+								result = TRUE;
+								DeviceState = outsideDevCapabilities;
+							} else
+								result = TRUE;
+						} else
+							result = FALSE;
+						break;
+					default:
+						result = 0;
+						break;
 					}
+					if (result != 1) {
+						DeviceState = Last_operation_failed;
+						Aditionals = (uint32_t) Command;
+					}
+
 					++Next_Packet;
 				} else {
 
@@ -340,9 +356,20 @@ void processComand(uint8_t *xReceive_Buffer) {
 		PIOS_COM_SendBuffer(PIOS_COM_TELEM_USB, Buffer + 1, 63);//FIX+1
 		break;
 	case JumpFW:
-		FLASH_Lock();
-		PIOS_OPAHRS_bl_boot();
-		JumpToApp = 1;
+		if (numberOfDevices > 1) {
+			struct opahrs_msg_v0 rsp;
+			PIOS_OPAHRS_bl_boot(0);
+			if (PIOS_OPAHRS_bl_FwupStatus(&rsp) == OPAHRS_RESULT_OK) {
+				DeviceState = failed_jump;
+				break;
+			} else {
+				FLASH_Lock();
+				JumpToApp = 1;
+			}
+		} else {
+			FLASH_Lock();
+			JumpToApp = 1;
+		}
 		break;
 	case Reset:
 		//PIOS Reset_Device();
@@ -458,9 +485,9 @@ void OPDfuIni(uint8_t discover) {
 				dev.BL_Version = rsp.payload.user.v.rsp.versions.bl_version;
 				dev.FW_Crc = rsp.payload.user.v.rsp.versions.fw_version;
 				dev.devID = rsp.payload.user.v.rsp.versions.hw_version;
-				for(int x=0;x<200;++x)
-				{
-				spi_dev_desc[x]=(uint8_t)rsp.payload.user.v.rsp.versions.description[x];
+				for (int x = 0; x < 200; ++x) {
+					spi_dev_desc[x]
+							= (uint8_t) rsp.payload.user.v.rsp.versions.description[x];
 
 				}
 				if (PIOS_OPAHRS_bl_GetMemMap(&rsp) == OPAHRS_RESULT_OK) {