From 61f18a122d6114cdf70d1e210fe9a3776e6cc880 Mon Sep 17 00:00:00 2001 From: dongie Date: Thu, 29 May 2014 09:28:09 +0900 Subject: [PATCH 1/5] remove i2c driver function calls and directly read/write I2C->DR --- src/drv_i2c.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/drv_i2c.c b/src/drv_i2c.c index 98ab5c5a33..2b40ec6994 100755 --- a/src/drv_i2c.c +++ b/src/drv_i2c.c @@ -199,18 +199,18 @@ void i2c_ev_handler(void) if (reading && subaddress_sent) { //EV7_2, EV7_3 if (bytes > 2) { //EV7_2 I2C_AcknowledgeConfig(I2Cx, DISABLE); //turn off ACK - read_p[index++] = I2C_ReceiveData(I2Cx); //read data N-2 + read_p[index++] = (uint8_t)I2Cx->DR; // read data N-2 I2C_GenerateSTOP(I2Cx, ENABLE); //program the Stop final_stop = 1; //reuired to fix hardware - read_p[index++] = I2C_ReceiveData(I2Cx); //read data N-1 + read_p[index++] = (uint8_t)I2Cx->DR; // read data N-1 I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); //enable TXE to allow the final EV7 } else { //EV7_3 if (final_stop) I2C_GenerateSTOP(I2Cx, ENABLE); //program the Stop else I2C_GenerateSTART(I2Cx, ENABLE); //program a rep start - read_p[index++] = I2C_ReceiveData(I2Cx); //read data N-1 - read_p[index++] = I2C_ReceiveData(I2Cx); //read data N + read_p[index++] = (uint8_t)I2Cx->DR; // read data N-1 + read_p[index++] = (uint8_t)I2Cx->DR; // read data N index++; //to show job completed } } else { //EV8_2, which may be due to a subaddress sent or a write completion @@ -228,19 +228,19 @@ void i2c_ev_handler(void) //we must wait for the start to clear, otherwise we get constant BTF while (I2Cx->CR1 & 0x0100) { ; } } else if (SReg_1 & 0x0040) { //Byte received - EV7 - read_p[index++] = I2C_ReceiveData(I2Cx); + read_p[index++] = (uint8_t)I2Cx->DR; if (bytes == (index + 3)) I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); //disable TXE to allow the buffer to flush so we can get an EV7_2 if (bytes == index) //We have completed a final EV7 index++; //to show job is complete } else if (SReg_1 & 0x0080) { //Byte transmitted -EV8/EV8_1 if (index != -1) { //we dont have a subaddress to send - I2C_SendData(I2Cx, write_p[index++]); + I2Cx->DR = write_p[index++]; if (bytes == index) //we have sent all the data I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); //disable TXE to allow the buffer to flush } else { index++; - I2C_SendData(I2Cx, reg); //send the subaddress + I2Cx->DR = reg; // send the subaddress if (reading || !bytes) //if receiving or sending 0 bytes, flush now I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); //disable TXE to allow the buffer to flush } From 8b6ff25bdbd75c16b4562aab3249460666862102 Mon Sep 17 00:00:00 2001 From: dongie Date: Thu, 29 May 2014 11:07:52 +0900 Subject: [PATCH 2/5] reformat I2C driver and align comments. add timeout checking and move hardware reinit common code into separate function. moved nvic priority group init into drv_system, where it belongs --- src/drv_i2c.c | 332 +++++++++++++++++++++++------------------------ src/drv_system.c | 2 + 2 files changed, 164 insertions(+), 170 deletions(-) diff --git a/src/drv_i2c.c b/src/drv_i2c.c index 2b40ec6994..6bbd8ec1f6 100755 --- a/src/drv_i2c.c +++ b/src/drv_i2c.c @@ -45,34 +45,12 @@ static volatile uint8_t reading; static volatile uint8_t* write_p; static volatile uint8_t* read_p; -static void i2c_er_handler(void) +static bool i2cHandleHardwareFailure(void) { - volatile uint32_t SR1Register; - // Read the I2C1 status register - SR1Register = I2Cx->SR1; - if (SR1Register & 0x0F00) { //an error - error = true; - // I2C1error.error = ((SR1Register & 0x0F00) >> 8); //save error - // I2C1error.job = job; //the task - } - // If AF, BERR or ARLO, abandon the current job and commence new if there are jobs - if (SR1Register & 0x0700) { - (void)I2Cx->SR2; // read second status register to clear ADDR if it is set (note that BTF will not be set after a NACK) - I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable the RXNE/TXE interrupt - prevent the ISR tailchaining onto the ER (hopefully) - if (!(SR1Register & 0x0200) && !(I2Cx->CR1 & 0x0200)) { // if we dont have an ARLO error, ensure sending of a stop - if (I2Cx->CR1 & 0x0100) { // We are currently trying to send a start, this is very bad as start,stop will hang the peripheral - while (I2Cx->CR1 & 0x0100); // wait for any start to finish sending - I2C_GenerateSTOP(I2Cx, ENABLE); // send stop to finalise bus transaction - while (I2Cx->CR1 & 0x0200); // wait for stop to finish sending - i2cInit(I2Cx); // reset and configure the hardware - } else { - I2C_GenerateSTOP(I2Cx, ENABLE); // stop to free up the bus - I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); // Disable EVT and ERR interrupts while bus inactive - } - } - } - I2Cx->SR1 &= ~0x0F00; // reset all the error bits to clear the interrupt - busy = 0; + i2cErrorCount++; + // reinit peripheral + clock out garbage + i2cInit(I2Cx); + return false; } bool i2cWriteBuffer(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data) @@ -89,21 +67,20 @@ bool i2cWriteBuffer(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data) busy = 1; error = false; - if (!(I2Cx->CR2 & I2C_IT_EVT)) { //if we are restarting the driver - if (!(I2Cx->CR1 & 0x0100)) { // ensure sending a start - while (I2Cx->CR1 & 0x0200) { ; } //wait for any stop to finish sending - I2C_GenerateSTART(I2Cx, ENABLE); //send the start for the new job + if (!(I2Cx->CR2 & I2C_IT_EVT)) { // if we are restarting the driver + if (!(I2Cx->CR1 & 0x0100)) { // ensure sending a start + while (I2Cx->CR1 & 0x0200 && --timeout > 0) { ; } // wait for any stop to finish sending + if (timeout == 0) + return i2cHandleHardwareFailure(); + I2C_GenerateSTART(I2Cx, ENABLE); // send the start for the new job } - I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, ENABLE); //allow the interrupts to fire off again + I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, ENABLE); // allow the interrupts to fire off again } - while (busy && --timeout > 0); - if (timeout == 0) { - i2cErrorCount++; - // reinit peripheral + clock out garbage - i2cInit(I2Cx); - return false; - } + timeout = I2C_DEFAULT_TIMEOUT; + while (busy && --timeout > 0) { ; } + if (timeout == 0) + return i2cHandleHardwareFailure(); return !error; } @@ -127,139 +104,156 @@ bool i2cRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) busy = 1; error = false; - if (!(I2Cx->CR2 & I2C_IT_EVT)) { //if we are restarting the driver - if (!(I2Cx->CR1 & 0x0100)) { // ensure sending a start - while (I2Cx->CR1 & 0x0200 && --timeout > 0) { ; } //wait for any stop to finish sending - if (timeout == 0) { - i2cErrorCount++; - // reinit peripheral + clock out garbage - i2cInit(I2Cx); - return false; - } - I2C_GenerateSTART(I2Cx, ENABLE); //send the start for the new job + if (!(I2Cx->CR2 & I2C_IT_EVT)) { // if we are restarting the driver + if (!(I2Cx->CR1 & 0x0100)) { // ensure sending a start + while (I2Cx->CR1 & 0x0200 && --timeout > 0) { ; } // wait for any stop to finish sending + if (timeout == 0) + return i2cHandleHardwareFailure(); + I2C_GenerateSTART(I2Cx, ENABLE); // send the start for the new job } - I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, ENABLE); //allow the interrupts to fire off again + I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, ENABLE); // allow the interrupts to fire off again } timeout = I2C_DEFAULT_TIMEOUT; - while (busy && --timeout > 0); - if (timeout == 0) { - i2cErrorCount++; - // reinit peripheral + clock out garbage - i2cInit(I2Cx); - return false; - } + while (busy && --timeout > 0) { ; } + if (timeout == 0) + return i2cHandleHardwareFailure(); return !error; } -void i2c_ev_handler(void) +static void i2c_er_handler(void) { - static uint8_t subaddress_sent, final_stop; //flag to indicate if subaddess sent, flag to indicate final bus condition - static int8_t index; //index is signed -1==send the subaddress - uint8_t SReg_1 = I2Cx->SR1; //read the status register here + // Read the I2C1 status register + volatile uint32_t SR1Register = I2Cx->SR1; - if (SReg_1 & 0x0001) { //we just sent a start - EV5 in ref manual - I2Cx->CR1 &= ~0x0800; //reset the POS bit so ACK/NACK applied to the current byte - I2C_AcknowledgeConfig(I2Cx, ENABLE); //make sure ACK is on - index = 0; //reset the index - if (reading && (subaddress_sent || 0xFF == reg)) { //we have sent the subaddr - subaddress_sent = 1; //make sure this is set in case of no subaddress, so following code runs correctly - if (bytes == 2) - I2Cx->CR1 |= 0x0800; //set the POS bit so NACK applied to the final byte in the two byte read - I2C_Send7bitAddress(I2Cx, addr, I2C_Direction_Receiver); //send the address and set hardware mode - } else { //direction is Tx, or we havent sent the sub and rep start - I2C_Send7bitAddress(I2Cx, addr, I2C_Direction_Transmitter); //send the address and set hardware mode - if (reg != 0xFF) //0xFF as subaddress means it will be ignored, in Tx or Rx mode - index = -1; //send a subaddress - } - } else if (SReg_1 & 0x0002) { //we just sent the address - EV6 in ref manual - // Read SR1,2 to clear ADDR - __DMB(); // memory fence to control hardware - if (bytes == 1 && reading && subaddress_sent) { // we are receiving 1 byte - EV6_3 - I2C_AcknowledgeConfig(I2Cx, DISABLE); // turn off ACK - __DMB(); - (void)I2Cx->SR2; // clear ADDR after ACK is turned off - I2C_GenerateSTOP(I2Cx, ENABLE); // program the stop - final_stop = 1; - I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); // allow us to have an EV7 - } else { // EV6 and EV6_1 - (void)I2Cx->SR2; // clear the ADDR here - __DMB(); - if (bytes == 2 && reading && subaddress_sent) { //rx 2 bytes - EV6_1 - I2C_AcknowledgeConfig(I2Cx, DISABLE); //turn off ACK - I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); //disable TXE to allow the buffer to fill - } else if (bytes == 3 && reading && subaddress_sent) //rx 3 bytes - I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); //make sure RXNE disabled so we get a BTF in two bytes time - else //receiving greater than three bytes, sending subaddress, or transmitting - I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); - } - } else if (SReg_1 & 0x004) { //Byte transfer finished - EV7_2, EV7_3 or EV8_2 - final_stop = 1; - if (reading && subaddress_sent) { //EV7_2, EV7_3 - if (bytes > 2) { //EV7_2 - I2C_AcknowledgeConfig(I2Cx, DISABLE); //turn off ACK - read_p[index++] = (uint8_t)I2Cx->DR; // read data N-2 - I2C_GenerateSTOP(I2Cx, ENABLE); //program the Stop - final_stop = 1; //reuired to fix hardware - read_p[index++] = (uint8_t)I2Cx->DR; // read data N-1 - I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); //enable TXE to allow the final EV7 - } else { //EV7_3 - if (final_stop) - I2C_GenerateSTOP(I2Cx, ENABLE); //program the Stop - else - I2C_GenerateSTART(I2Cx, ENABLE); //program a rep start - read_p[index++] = (uint8_t)I2Cx->DR; // read data N-1 - read_p[index++] = (uint8_t)I2Cx->DR; // read data N - index++; //to show job completed + if (SR1Register & 0x0F00) // an error + error = true; + + // If AF, BERR or ARLO, abandon the current job and commence new if there are jobs + if (SR1Register & 0x0700) { + (void)I2Cx->SR2; // read second status register to clear ADDR if it is set (note that BTF will not be set after a NACK) + I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable the RXNE/TXE interrupt - prevent the ISR tailchaining onto the ER (hopefully) + if (!(SR1Register & 0x0200) && !(I2Cx->CR1 & 0x0200)) { // if we dont have an ARLO error, ensure sending of a stop + if (I2Cx->CR1 & 0x0100) { // We are currently trying to send a start, this is very bad as start, stop will hang the peripheral + while (I2Cx->CR1 & 0x0100) { ; } // wait for any start to finish sending + I2C_GenerateSTOP(I2Cx, ENABLE); // send stop to finalise bus transaction + while (I2Cx->CR1 & 0x0200) { ; } // wait for stop to finish sending + i2cInit(I2Cx); // reset and configure the hardware + } else { + I2C_GenerateSTOP(I2Cx, ENABLE); // stop to free up the bus + I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); // Disable EVT and ERR interrupts while bus inactive } - } else { //EV8_2, which may be due to a subaddress sent or a write completion - if (subaddress_sent || (writing)) { - if (final_stop) - I2C_GenerateSTOP(I2Cx, ENABLE); //program the Stop - else - I2C_GenerateSTART(I2Cx, ENABLE); //program a rep start - index++; //to show that the job is complete - } else { //We need to send a subaddress - I2C_GenerateSTART(I2Cx, ENABLE); //program the repeated Start - subaddress_sent = 1; //this is set back to zero upon completion of the current task - } - } - //we must wait for the start to clear, otherwise we get constant BTF - while (I2Cx->CR1 & 0x0100) { ; } - } else if (SReg_1 & 0x0040) { //Byte received - EV7 - read_p[index++] = (uint8_t)I2Cx->DR; - if (bytes == (index + 3)) - I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); //disable TXE to allow the buffer to flush so we can get an EV7_2 - if (bytes == index) //We have completed a final EV7 - index++; //to show job is complete - } else if (SReg_1 & 0x0080) { //Byte transmitted -EV8/EV8_1 - if (index != -1) { //we dont have a subaddress to send - I2Cx->DR = write_p[index++]; - if (bytes == index) //we have sent all the data - I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); //disable TXE to allow the buffer to flush - } else { - index++; - I2Cx->DR = reg; // send the subaddress - if (reading || !bytes) //if receiving or sending 0 bytes, flush now - I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); //disable TXE to allow the buffer to flush } } - if (index == bytes + 1) { //we have completed the current job - //Completion Tasks go here - //End of completion tasks - subaddress_sent = 0; //reset this here - // I2Cx->CR1 &= ~0x0800; //reset the POS bit so NACK applied to the current byte - if (final_stop) //If there is a final stop and no more jobs, bus is inactive, disable interrupts to prevent BTF - I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); //Disable EVT and ERR interrupts while bus inactive + I2Cx->SR1 &= ~0x0F00; // reset all the error bits to clear the interrupt + busy = 0; +} + +void i2c_ev_handler(void) +{ + static uint8_t subaddress_sent, final_stop; // flag to indicate if subaddess sent, flag to indicate final bus condition + static int8_t index; // index is signed -1 == send the subaddress + uint8_t SReg_1 = I2Cx->SR1; // read the status register here + + if (SReg_1 & 0x0001) { // we just sent a start - EV5 in ref manual + I2Cx->CR1 &= ~0x0800; // reset the POS bit so ACK/NACK applied to the current byte + I2C_AcknowledgeConfig(I2Cx, ENABLE); // make sure ACK is on + index = 0; // reset the index + if (reading && (subaddress_sent || 0xFF == reg)) { // we have sent the subaddr + subaddress_sent = 1; // make sure this is set in case of no subaddress, so following code runs correctly + if (bytes == 2) + I2Cx->CR1 |= 0x0800; // set the POS bit so NACK applied to the final byte in the two byte read + I2C_Send7bitAddress(I2Cx, addr, I2C_Direction_Receiver); // send the address and set hardware mode + } else { // direction is Tx, or we havent sent the sub and rep start + I2C_Send7bitAddress(I2Cx, addr, I2C_Direction_Transmitter); // send the address and set hardware mode + if (reg != 0xFF) // 0xFF as subaddress means it will be ignored, in Tx or Rx mode + index = -1; // send a subaddress + } + } else if (SReg_1 & 0x0002) { // we just sent the address - EV6 in ref manual + // Read SR1,2 to clear ADDR + __DMB(); // memory fence to control hardware + if (bytes == 1 && reading && subaddress_sent) { // we are receiving 1 byte - EV6_3 + I2C_AcknowledgeConfig(I2Cx, DISABLE); // turn off ACK + __DMB(); + (void)I2Cx->SR2; // clear ADDR after ACK is turned off + I2C_GenerateSTOP(I2Cx, ENABLE); // program the stop + final_stop = 1; + I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); // allow us to have an EV7 + } else { // EV6 and EV6_1 + (void)I2Cx->SR2; // clear the ADDR here + __DMB(); + if (bytes == 2 && reading && subaddress_sent) { // rx 2 bytes - EV6_1 + I2C_AcknowledgeConfig(I2Cx, DISABLE); // turn off ACK + I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to fill + } else if (bytes == 3 && reading && subaddress_sent) // rx 3 bytes + I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // make sure RXNE disabled so we get a BTF in two bytes time + else // receiving greater than three bytes, sending subaddress, or transmitting + I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); + } + } else if (SReg_1 & 0x004) { // Byte transfer finished - EV7_2, EV7_3 or EV8_2 + final_stop = 1; + if (reading && subaddress_sent) { // EV7_2, EV7_3 + if (bytes > 2) { // EV7_2 + I2C_AcknowledgeConfig(I2Cx, DISABLE); // turn off ACK + read_p[index++] = (uint8_t)I2Cx->DR; // read data N-2 + I2C_GenerateSTOP(I2Cx, ENABLE); // program the Stop + final_stop = 1; // required to fix hardware + read_p[index++] = (uint8_t)I2Cx->DR; // read data N - 1 + I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); // enable TXE to allow the final EV7 + } else { // EV7_3 + if (final_stop) + I2C_GenerateSTOP(I2Cx, ENABLE); // program the Stop + else + I2C_GenerateSTART(I2Cx, ENABLE); // program a rep start + read_p[index++] = (uint8_t)I2Cx->DR; // read data N - 1 + read_p[index++] = (uint8_t)I2Cx->DR; // read data N + index++; // to show job completed + } + } else { // EV8_2, which may be due to a subaddress sent or a write completion + if (subaddress_sent || (writing)) { + if (final_stop) + I2C_GenerateSTOP(I2Cx, ENABLE); // program the Stop + else + I2C_GenerateSTART(I2Cx, ENABLE); // program a rep start + index++; // to show that the job is complete + } else { // We need to send a subaddress + I2C_GenerateSTART(I2Cx, ENABLE); // program the repeated Start + subaddress_sent = 1; // this is set back to zero upon completion of the current task + } + } + // we must wait for the start to clear, otherwise we get constant BTF + while (I2Cx->CR1 & 0x0100) { ; } + } else if (SReg_1 & 0x0040) { // Byte received - EV7 + read_p[index++] = (uint8_t)I2Cx->DR; + if (bytes == (index + 3)) + I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to flush so we can get an EV7_2 + if (bytes == index) // We have completed a final EV7 + index++; // to show job is complete + } else if (SReg_1 & 0x0080) { // Byte transmitted EV8 / EV8_1 + if (index != -1) { // we dont have a subaddress to send + I2Cx->DR = write_p[index++]; + if (bytes == index) // we have sent all the data + I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to flush + } else { + index++; + I2Cx->DR = reg; // send the subaddress + if (reading || !bytes) // if receiving or sending 0 bytes, flush now + I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to flush + } + } + if (index == bytes + 1) { // we have completed the current job + subaddress_sent = 0; // reset this here + if (final_stop) // If there is a final stop and no more jobs, bus is inactive, disable interrupts to prevent BTF + I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); // Disable EVT and ERR interrupts while bus inactive busy = 0; } } void i2cInit(I2C_TypeDef *I2C) { - NVIC_InitTypeDef NVIC_InitStructure; - I2C_InitTypeDef I2C_InitStructure; + NVIC_InitTypeDef nvic; + I2C_InitTypeDef i2c; gpio_config_t gpio; // Init pins @@ -275,29 +269,27 @@ void i2cInit(I2C_TypeDef *I2C) // Init I2C I2C_DeInit(I2Cx); - I2C_StructInit(&I2C_InitStructure); + I2C_StructInit(&i2c); - I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); //Enable EVT and ERR interrupts - they are enabled by the first request - I2C_InitStructure.I2C_Mode = I2C_Mode_I2C; - I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2; - I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; - I2C_InitStructure.I2C_ClockSpeed = 400000; + I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); // Enable EVT and ERR interrupts - they are enabled by the first request + i2c.I2C_Mode = I2C_Mode_I2C; + i2c.I2C_DutyCycle = I2C_DutyCycle_2; + i2c.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; + i2c.I2C_ClockSpeed = 400000; I2C_Cmd(I2Cx, ENABLE); - I2C_Init(I2Cx, &I2C_InitStructure); - - NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); + I2C_Init(I2Cx, &i2c); // I2C ER Interrupt - NVIC_InitStructure.NVIC_IRQChannel = I2C2_ER_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); + nvic.NVIC_IRQChannel = I2C2_ER_IRQn; + nvic.NVIC_IRQChannelPreemptionPriority = 0; + nvic.NVIC_IRQChannelSubPriority = 0; + nvic.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&nvic); // I2C EV Interrupt - NVIC_InitStructure.NVIC_IRQChannel = I2C2_EV_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; - NVIC_Init(&NVIC_InitStructure); + nvic.NVIC_IRQChannel = I2C2_EV_IRQn; + nvic.NVIC_IRQChannelPreemptionPriority = 0; + NVIC_Init(&nvic); } uint16_t i2cGetErrorCounter(void) diff --git a/src/drv_system.c b/src/drv_system.c index 9fb2456968..6d92d81beb 100755 --- a/src/drv_system.c +++ b/src/drv_system.c @@ -76,6 +76,8 @@ void systemInit(bool overclock) // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers // Configure the Flash Latency cycles and enable prefetch buffer SetSysClock(overclock); + // Configure NVIC preempt/priority groups + NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // Turn on clocks for stuff we use RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4 | RCC_APB1Periph_I2C2, ENABLE); From 3d39ece50fbad377dfc6fd952f4d162f138b1dcf Mon Sep 17 00:00:00 2001 From: dongie Date: Thu, 29 May 2014 11:38:20 +0900 Subject: [PATCH 3/5] Remove FY90Q support. Dead hardware, not even available anymore, and doesn't offer anything over already cheap acroafro. --- Makefile | 9 +- baseflight.uvproj | 1400 +---------------- .../startup_stm32f10x_md_fy90q.s | 359 ----- src/board.h | 26 - src/drv_adc.h | 3 - src/drv_adc_fy90q.c | 145 -- src/drv_pwm_fy90q.c | 344 ---- src/drv_system.c | 2 - src/sensors.c | 10 - 9 files changed, 7 insertions(+), 2291 deletions(-) delete mode 100644 src/baseflight_startups/startup_stm32f10x_md_fy90q.s delete mode 100644 src/drv_adc_fy90q.c delete mode 100644 src/drv_pwm_fy90q.c diff --git a/Makefile b/Makefile index 3bab5cbfe4..95c2af8862 100644 --- a/Makefile +++ b/Makefile @@ -14,7 +14,7 @@ # Things that the user might override on the commandline # -# The target to build, must be one of NAZE, FY90Q OR OLIMEXINO +# The target to build, must be one of NAZE OR OLIMEXINO TARGET ?= NAZE # Compile-time options @@ -30,7 +30,7 @@ SERIAL_DEVICE ?= /dev/ttyUSB0 # Things that need to be maintained as the source changes # -VALID_TARGETS = NAZE FY90Q OLIMEXINO +VALID_TARGETS = NAZE OLIMEXINO # Working directories ROOT = $(dir $(lastword $(MAKEFILE_LIST))) @@ -89,11 +89,6 @@ NAZE_SRC = drv_adc.c \ drv_timer.c \ $(COMMON_SRC) -# Source files for the FY90Q target -FY90Q_SRC = drv_adc_fy90q.c \ - drv_pwm_fy90q.c \ - $(COMMON_SRC) - # Source files for the OLIMEXINO target OLIMEXINO_SRC = drv_spi.c \ drv_adc.c \ diff --git a/baseflight.uvproj b/baseflight.uvproj index c17889b29e..765943e863 100755 --- a/baseflight.uvproj +++ b/baseflight.uvproj @@ -139,11 +139,9 @@ 0 1 0 - 1 - 1 0 - 1 + 7 @@ -157,7 +155,7 @@ - BIN\UL2CM3.DLL + Segger\JL2CM3.dll @@ -560,98 +558,6 @@ 1 .\src\drv_mpu6050.c - - drv_adc_fy90q.c - 1 - .\src\drv_adc_fy90q.c - - - 2 - 0 - 0 - 0 - 0 - 0 - 2 - 2 - 2 - 2 - 11 - - - 1 - - - - 2 - 0 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 0 - 2 - 2 - - - - - - - - - - - - drv_pwm_fy90q.c - 1 - .\src\drv_pwm_fy90q.c - - - 2 - 0 - 0 - 0 - 0 - 0 - 2 - 2 - 2 - 2 - 11 - - - 1 - - - - 2 - 0 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 0 - 2 - 2 - - - - - - - - - - drv_mma845x.c 1 @@ -829,47 +735,6 @@ 2 .\src\baseflight_startups\startup_stm32f10x_md.s - - startup_stm32f10x_md_fy90q.s - 2 - .\src\baseflight_startups\startup_stm32f10x_md_fy90q.s - - - 2 - 0 - 0 - 0 - 0 - 0 - 2 - 2 - 2 - 2 - 11 - - - 1 - - - - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - - - - - - - - - - printf.c 1 @@ -1019,7 +884,7 @@ 0 0 - 1 + 7 @@ -1033,7 +898,7 @@ - BIN\UL2CM3.DLL + Segger\JL2CM3.dll @@ -1045,7 +910,7 @@ 1 4096 - 0 + 1 BIN\UL2CM3.DLL "" () @@ -1436,98 +1301,6 @@ 1 .\src\drv_mpu6050.c - - drv_adc_fy90q.c - 1 - .\src\drv_adc_fy90q.c - - - 2 - 0 - 0 - 0 - 0 - 0 - 2 - 2 - 2 - 2 - 11 - - - 1 - - - - 2 - 0 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 0 - 2 - 2 - - - - - - - - - - - - drv_pwm_fy90q.c - 1 - .\src\drv_pwm_fy90q.c - - - 2 - 0 - 0 - 0 - 0 - 0 - 2 - 2 - 2 - 2 - 11 - - - 1 - - - - 2 - 0 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 0 - 2 - 2 - - - - - - - - - - drv_mma845x.c 1 @@ -1648,1169 +1421,6 @@ 2 .\src\baseflight_startups\startup_stm32f10x_md.s - - startup_stm32f10x_md_fy90q.s - 2 - .\src\baseflight_startups\startup_stm32f10x_md_fy90q.s - - - 2 - 0 - 0 - 0 - 0 - 0 - 2 - 2 - 2 - 2 - 11 - - - 1 - - - - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - - - - - - - - - - - - printf.c - 1 - .\src\printf.c - - - stm32f10x_spi.c - 1 - .\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_spi.c - - - - - - - STM32-FY90Q - 0x4 - ARM-ADS - - - STM32F103RB - STMicroelectronics - IRAM(0x20000000-0x20004FFF) IROM(0x8000000-0x801FFFF) CLOCK(8000000) CPUTYPE("Cortex-M3") - - "STARTUP\ST\STM32F10x\startup_stm32f10x_md.s" ("STM32 Medium Density Line Startup Code") - UL2CM3(-O14 -S0 -C0 -N00("ARM Cortex-M3") -D00(1BA00477) -L00(4) -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F10x_128 -FS08000000 -FL020000) - 4231 - stm32f10x_md.h - - - - - - - - - - SFD\ST\STM32F10xx\STM32F10xxB.sfr - 0 - 0 - - - - ST\STM32F10x\ - ST\STM32F10x\ - - 0 - 0 - 0 - 0 - 1 - - .\obj\ - baseflight_fy90q - 1 - 0 - 1 - 1 - 1 - .\obj\ - 1 - 0 - 0 - - 0 - 0 - - - 0 - 0 - 0 - 0 - - - 0 - 0 - - - 0 - 0 - - - 0 - 0 - - - 0 - 0 - - 0 - - - - 0 - 0 - 0 - 0 - 0 - 1 - 0 - 0 - 0 - 0 - 3 - - - 1 - - - SARMCM3.DLL - - DARMSTM.DLL - -pSTM32F103RB - SARMCM3.DLL - - TARMSTM.DLL - -pSTM32F103RB - - - - 1 - 0 - 0 - 0 - 16 - - - 0 - 1 - 1 - 1 - 1 - 1 - 1 - 1 - 0 - - - 1 - 1 - 1 - 1 - 1 - 1 - 0 - 1 - 0 - - 0 - 1 - - - - - - - - - - - - - - BIN\UL2CM3.DLL - - - - - 1 - 0 - 0 - 1 - 1 - 4096 - - 0 - BIN\UL2CM3.DLL - "" () - - - - - 0 - - - - 0 - 1 - 1 - 1 - 1 - 1 - 1 - 1 - 0 - 1 - 1 - 0 - 1 - 1 - 0 - 0 - 1 - 1 - 1 - 1 - 1 - 1 - 1 - 1 - 1 - 0 - 0 - "Cortex-M3" - - 0 - 0 - 0 - 1 - 1 - 0 - 0 - 0 - 0 - 0 - 8 - 1 - 0 - 0 - 3 - 3 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 1 - 0 - 0 - 0 - 0 - 1 - 0 - - - 0 - 0x0 - 0x0 - - - 0 - 0x0 - 0x0 - - - 0 - 0x0 - 0x0 - - - 0 - 0x0 - 0x0 - - - 0 - 0x0 - 0x0 - - - 0 - 0x0 - 0x0 - - - 0 - 0x20000000 - 0x5000 - - - 1 - 0x8000000 - 0x20000 - - - 0 - 0x0 - 0x0 - - - 1 - 0x0 - 0x0 - - - 1 - 0x0 - 0x0 - - - 1 - 0x0 - 0x0 - - - 1 - 0x8000000 - 0x20000 - - - 1 - 0x0 - 0x0 - - - 0 - 0x0 - 0x0 - - - 0 - 0x0 - 0x0 - - - 0 - 0x0 - 0x0 - - - 0 - 0x20000000 - 0x5000 - - - 0 - 0x0 - 0x0 - - - - - - 1 - 1 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - - - STM32F10X_MD,USE_STDPERIPH_DRIVER,FY90Q - - lib\CMSIS\CM3\CoreSupport;lib\CMSIS\CM3\DeviceSupport\ST\STM32F10x;lib\STM32F10x_StdPeriph_Driver\inc - - - - 1 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - - - - - - - - - 1 - 0 - 0 - 0 - 1 - 0 - 0x08000000 - 0x20000000 - - - - - - - - - - - - App - - - cli.c - 1 - .\src\cli.c - - - config.c - 1 - .\src\config.c - - - imu.c - 1 - .\src\imu.c - - - main.c - 1 - .\src\main.c - - - mixer.c - 1 - .\src\mixer.c - - - mw.c - 1 - .\src\mw.c - - - sensors.c - 1 - .\src\sensors.c - - - serial.c - 1 - .\src\serial.c - - - board.h - 5 - .\src\board.h - - - mw.h - 5 - .\src\mw.h - - - gps.c - 1 - .\src\gps.c - - - spektrum.c - 1 - .\src\spektrum.c - - - buzzer.c - 1 - .\src\buzzer.c - - - utils.c - 1 - .\src\utils.c - - - sbus.c - 1 - .\src\sbus.c - - - sumd.c - 1 - .\src\sumd.c - - - telemetry_frsky.c - 1 - .\src\telemetry_frsky.c - - - telemetry_common.c - 1 - .\src\telemetry_common.c - - - telemetry_hott.c - 1 - .\src\telemetry_hott.c - - - rxmsp.c - 1 - .\src\rxmsp.c - - - - - Drivers - - - drv_adc.c - 1 - .\src\drv_adc.c - - - 2 - 0 - 0 - 0 - 0 - 0 - 2 - 2 - 2 - 2 - 11 - - - 1 - - - - 2 - 0 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 0 - 2 - 2 - - - - - - - - - - - - drv_adxl345.c - 1 - .\src\drv_adxl345.c - - - 2 - 0 - 0 - 0 - 0 - 0 - 2 - 2 - 2 - 2 - 11 - - - 1 - - - - 2 - 0 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 0 - 2 - 2 - - - - - - - - - - - - drv_bmp085.c - 1 - .\src\drv_bmp085.c - - - 2 - 0 - 0 - 0 - 0 - 0 - 2 - 2 - 2 - 2 - 11 - - - 1 - - - - 2 - 0 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 0 - 2 - 2 - - - - - - - - - - - - drv_hmc5883l.c - 1 - .\src\drv_hmc5883l.c - - - 2 - 0 - 0 - 0 - 0 - 0 - 2 - 2 - 2 - 2 - 11 - - - 1 - - - - 2 - 0 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 0 - 2 - 2 - - - - - - - - - - - - drv_i2c.c - 1 - .\src\drv_i2c.c - - - drv_mpu3050.c - 1 - .\src\drv_mpu3050.c - - - 2 - 0 - 0 - 0 - 0 - 0 - 2 - 2 - 2 - 2 - 11 - - - 1 - - - - 2 - 0 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 0 - 2 - 2 - - - - - - - - - - - - drv_pwm.c - 1 - .\src\drv_pwm.c - - - 2 - 0 - 0 - 0 - 0 - 0 - 2 - 2 - 2 - 2 - 11 - - - 1 - - - - 2 - 0 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 0 - 2 - 2 - - - - - - - - - - - - drv_system.c - 1 - .\src\drv_system.c - - - drv_uart.c - 1 - .\src\drv_uart.c - - - drv_ledring.c - 1 - .\src\drv_ledring.c - - - 2 - 0 - 0 - 0 - 0 - 0 - 2 - 2 - 2 - 2 - 11 - - - 1 - - - - 2 - 0 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 0 - 2 - 2 - - - - - - - - - - - - drv_mpu6050.c - 1 - .\src\drv_mpu6050.c - - - 2 - 0 - 0 - 0 - 0 - 0 - 2 - 2 - 2 - 2 - 11 - - - 1 - - - - 2 - 0 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 0 - 2 - 2 - - - - - - - - - - - - drv_adc_fy90q.c - 1 - .\src\drv_adc_fy90q.c - - - drv_pwm_fy90q.c - 1 - .\src\drv_pwm_fy90q.c - - - drv_mma845x.c - 1 - .\src\drv_mma845x.c - - - drv_hcsr04.c - 1 - .\src\drv_hcsr04.c - - - drv_i2c_soft.c - 1 - .\src\drv_i2c_soft.c - - - drv_l3g4200d.c - 1 - .\src\drv_l3g4200d.c - - - drv_ms5611.c - 1 - .\src\drv_ms5611.c - - - drv_gpio.c - 1 - .\src\drv_gpio.c - - - drv_spi.c - 1 - .\src\drv_spi.c - - - drv_timer.c - 1 - .\src\drv_timer.c - - - drv_softserial.c - 1 - .\src\drv_softserial.c - - - drv_bma280.c - 1 - .\src\drv_bma280.c - - - drv_serial.c - 1 - .\src\drv_serial.c - - - - - System - - - 0 - 0 - 0 - 0 - 0 - 2 - 2 - 2 - 2 - 2 - 11 - - - 0 - - - - 2 - 4 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 0 - 2 - 2 - - - - - - - - - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - - - - - - - - - - - - core_cm3.c - 1 - .\lib\CMSIS\CM3\CoreSupport\core_cm3.c - - - system_stm32f10x.c - 1 - .\lib\CMSIS\CM3\DeviceSupport\ST\STM32F10x\system_stm32f10x.c - - - misc.c - 1 - .\lib\STM32F10x_StdPeriph_Driver\src\misc.c - - - stm32f10x_adc.c - 1 - .\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_adc.c - - - stm32f10x_dma.c - 1 - .\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_dma.c - - - stm32f10x_exti.c - 1 - .\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_exti.c - - - stm32f10x_i2c.c - 1 - .\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_i2c.c - - - stm32f10x_rcc.c - 1 - .\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_rcc.c - - - stm32f10x_tim.c - 1 - .\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_tim.c - - - stm32f10x_usart.c - 1 - .\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_usart.c - - - stm32f10x_flash.c - 1 - .\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_flash.c - - - startup_stm32f10x_md.s - 2 - .\src\baseflight_startups\startup_stm32f10x_md.s - - - 2 - 0 - 0 - 0 - 0 - 0 - 2 - 2 - 2 - 2 - 11 - - - 1 - - - - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - - - - - - - - - - - - startup_stm32f10x_md_fy90q.s - 2 - .\src\baseflight_startups\startup_stm32f10x_md_fy90q.s - printf.c 1 diff --git a/src/baseflight_startups/startup_stm32f10x_md_fy90q.s b/src/baseflight_startups/startup_stm32f10x_md_fy90q.s deleted file mode 100644 index dd7e1376d8..0000000000 --- a/src/baseflight_startups/startup_stm32f10x_md_fy90q.s +++ /dev/null @@ -1,359 +0,0 @@ -;******************** (C) COPYRIGHT 2011 STMicroelectronics ******************** -;* File Name : startup_stm32f10x_md.s -;* Author : MCD Application Team -;* Version : V3.5.0 -;* Date : 11-March-2011 -;* Description : STM32F10x Medium Density Devices vector table for MDK-ARM -;* toolchain. -;* This module performs: -;* - Set the initial SP -;* - Set the initial PC == Reset_Handler -;* - Set the vector table entries with the exceptions ISR address -;* - Configure the clock system -;* - Branches to __main in the C library (which eventually -;* calls main()). -;* After Reset the CortexM3 processor is in Thread mode, -;* priority is Privileged, and the Stack is set to Main. -;* <<< Use Configuration Wizard in Context Menu >>> -;******************************************************************************* -; THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS -; WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME. -; AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT, -; INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE -; CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING -; INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. -;******************************************************************************* - -; Amount of memory (in bytes) allocated for Stack -; Tailor this value to your application needs -; Stack Configuration -; Stack Size (in Bytes) <0x0-0xFFFFFFFF:8> -; - -Stack_Size EQU 0x00001000 - - AREA STACK, NOINIT, READWRITE, ALIGN=3 -Stack_Mem SPACE Stack_Size -__initial_sp - - -; Heap Configuration -; Heap Size (in Bytes) <0x0-0xFFFFFFFF:8> -; - -Heap_Size EQU 0x00000200 - - AREA HEAP, NOINIT, READWRITE, ALIGN=3 -__heap_base -Heap_Mem SPACE Heap_Size -__heap_limit - - PRESERVE8 - THUMB - - -; Vector Table Mapped to Address 0 at Reset - AREA RESET, DATA, READONLY - EXPORT __Vectors - EXPORT __Vectors_End - EXPORT __Vectors_Size - -__Vectors DCD __initial_sp ; Top of Stack - DCD Reset_Handler ; Reset Handler - DCD NMI_Handler ; NMI Handler - DCD HardFault_Handler ; Hard Fault Handler - DCD MemManage_Handler ; MPU Fault Handler - DCD BusFault_Handler ; Bus Fault Handler - DCD UsageFault_Handler ; Usage Fault Handler - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD SVC_Handler ; SVCall Handler - DCD DebugMon_Handler ; Debug Monitor Handler - DCD 0 ; Reserved - DCD PendSV_Handler ; PendSV Handler - DCD SysTick_Handler ; SysTick Handler - - ; External Interrupts - DCD WWDG_IRQHandler ; Window Watchdog - DCD PVD_IRQHandler ; PVD through EXTI Line detect - DCD TAMPER_IRQHandler ; Tamper - DCD RTC_IRQHandler ; RTC - DCD FLASH_IRQHandler ; Flash - DCD RCC_IRQHandler ; RCC - DCD EXTI0_IRQHandler ; EXTI Line 0 - DCD EXTI1_IRQHandler ; EXTI Line 1 - DCD EXTI2_IRQHandler ; EXTI Line 2 - DCD EXTI3_IRQHandler ; EXTI Line 3 - DCD EXTI4_IRQHandler ; EXTI Line 4 - DCD DMA1_Channel1_IRQHandler ; DMA1 Channel 1 - DCD DMA1_Channel2_IRQHandler ; DMA1 Channel 2 - DCD DMA1_Channel3_IRQHandler ; DMA1 Channel 3 - DCD DMA1_Channel4_IRQHandler ; DMA1 Channel 4 - DCD DMA1_Channel5_IRQHandler ; DMA1 Channel 5 - DCD DMA1_Channel6_IRQHandler ; DMA1 Channel 6 - DCD DMA1_Channel7_IRQHandler ; DMA1 Channel 7 - DCD ADC1_2_IRQHandler ; ADC1_2 - DCD USB_HP_CAN1_TX_IRQHandler ; USB High Priority or CAN1 TX - DCD USB_LP_CAN1_RX0_IRQHandler ; USB Low Priority or CAN1 RX0 - DCD CAN1_RX1_IRQHandler ; CAN1 RX1 - DCD CAN1_SCE_IRQHandler ; CAN1 SCE - DCD EXTI9_5_IRQHandler ; EXTI Line 9..5 - DCD TIM1_BRK_IRQHandler ; TIM1 Break - DCD TIM1_UP_IRQHandler ; TIM1 Update - DCD TIM1_TRG_COM_IRQHandler ; TIM1 Trigger and Commutation - DCD TIM1_CC_IRQHandler ; TIM1 Capture Compare - DCD TIM2_IRQHandler ; TIM2 - DCD TIM3_IRQHandler ; TIM3 - DCD TIM4_IRQHandler ; TIM4 - DCD I2C1_EV_IRQHandler ; I2C1 Event - DCD I2C1_ER_IRQHandler ; I2C1 Error - DCD I2C2_EV_IRQHandler ; I2C2 Event - DCD I2C2_ER_IRQHandler ; I2C2 Error - DCD SPI1_IRQHandler ; SPI1 - DCD SPI2_IRQHandler ; SPI2 - DCD USART1_IRQHandler ; USART1 - DCD USART2_IRQHandler ; USART2 - DCD USART3_IRQHandler ; USART3 - DCD EXTI15_10_IRQHandler ; EXTI Line 15..10 - DCD RTCAlarm_IRQHandler ; RTC Alarm through EXTI Line - DCD USBWakeUp_IRQHandler ; USB Wakeup from suspend -__Vectors_End - -__Vectors_Size EQU __Vectors_End - __Vectors - - AREA |.text|, CODE, READONLY - -; Reset handler -Reset_Handler PROC - EXPORT Reset_Handler [WEAK] - IMPORT __main - IMPORT SystemInit - LDR R0, =0x20004FF0 - LDR R1, =0xDEADBEEF - LDR R2, [R0, #0] - STR R0, [R0, #0] ; Invalidate - CMP R2, R1 - BEQ Reboot_Loader - LDR R0, =SystemInit - BLX R0 - LDR R0, =__main - BX R0 - ENDP - -RCC_APB2ENR EQU 0x40021018 -GPIO_AFIO_MASK EQU 0x00000015 -GPIOA_CRH EQU 0x40010804 -GPIOA_BRR EQU 0x40010814 -GPIOC_CRH EQU 0x40011004 -GPIOC_BRR EQU 0x40011014 -AFIO_MAPR EQU 0x40010004 - -Reboot_Loader PROC - EXPORT Reboot_Loader - - ; RCC Enable GPIOA+C+AFIO - LDR R6, =RCC_APB2ENR - LDR R0, =GPIO_AFIO_MASK - STR R0, [R6] - ; MAPR pt1 - LDR R0, =AFIO_MAPR - LDR R1, [R0] - BIC R1, R1, #0xF000000 - STR R1, [R0] - ; MAPR pt2 - LSLS R1, R0, #9 - STR R1, [R0] - ; GPIO A BRR - LDR R4, =GPIOA_BRR - MOVS R0, #0x8000 - STR R0, [R4] - ; GPIO A CRL - LDR R1, =GPIOA_CRH - LDR R0, =0x34444444 - STR R0, [R1] - ; GPIO C BRR - LDR R4, =GPIOC_BRR - MOVS R0, #0x1000 - STR R0, [R4] - ; GPIO C CRL - LDR R1, =GPIOC_CRH - LDR R0, =0x44434444 - STR R0, [R1] - ; Reboot to ROM - LDR R0, =0x1FFFF000 - LDR SP,[R0, #0] - LDR R0,[R0, #4] - BX R0 - ENDP - -; Dummy Exception Handlers (infinite loops which can be modified) - -NMI_Handler PROC - EXPORT NMI_Handler [WEAK] - B . - ENDP -HardFault_Handler\ - PROC - EXPORT HardFault_Handler [WEAK] - B . - ENDP -MemManage_Handler\ - PROC - EXPORT MemManage_Handler [WEAK] - B . - ENDP -BusFault_Handler\ - PROC - EXPORT BusFault_Handler [WEAK] - B . - ENDP -UsageFault_Handler\ - PROC - EXPORT UsageFault_Handler [WEAK] - B . - ENDP -SVC_Handler PROC - EXPORT SVC_Handler [WEAK] - B . - ENDP -DebugMon_Handler\ - PROC - EXPORT DebugMon_Handler [WEAK] - B . - ENDP -PendSV_Handler PROC - EXPORT PendSV_Handler [WEAK] - B . - ENDP -SysTick_Handler PROC - EXPORT SysTick_Handler [WEAK] - B . - ENDP - -Default_Handler PROC - - EXPORT WWDG_IRQHandler [WEAK] - EXPORT PVD_IRQHandler [WEAK] - EXPORT TAMPER_IRQHandler [WEAK] - EXPORT RTC_IRQHandler [WEAK] - EXPORT FLASH_IRQHandler [WEAK] - EXPORT RCC_IRQHandler [WEAK] - EXPORT EXTI0_IRQHandler [WEAK] - EXPORT EXTI1_IRQHandler [WEAK] - EXPORT EXTI2_IRQHandler [WEAK] - EXPORT EXTI3_IRQHandler [WEAK] - EXPORT EXTI4_IRQHandler [WEAK] - EXPORT DMA1_Channel1_IRQHandler [WEAK] - EXPORT DMA1_Channel2_IRQHandler [WEAK] - EXPORT DMA1_Channel3_IRQHandler [WEAK] - EXPORT DMA1_Channel4_IRQHandler [WEAK] - EXPORT DMA1_Channel5_IRQHandler [WEAK] - EXPORT DMA1_Channel6_IRQHandler [WEAK] - EXPORT DMA1_Channel7_IRQHandler [WEAK] - EXPORT ADC1_2_IRQHandler [WEAK] - EXPORT USB_HP_CAN1_TX_IRQHandler [WEAK] - EXPORT USB_LP_CAN1_RX0_IRQHandler [WEAK] - EXPORT CAN1_RX1_IRQHandler [WEAK] - EXPORT CAN1_SCE_IRQHandler [WEAK] - EXPORT EXTI9_5_IRQHandler [WEAK] - EXPORT TIM1_BRK_IRQHandler [WEAK] - EXPORT TIM1_UP_IRQHandler [WEAK] - EXPORT TIM1_TRG_COM_IRQHandler [WEAK] - EXPORT TIM1_CC_IRQHandler [WEAK] - EXPORT TIM2_IRQHandler [WEAK] - EXPORT TIM3_IRQHandler [WEAK] - EXPORT TIM4_IRQHandler [WEAK] - EXPORT I2C1_EV_IRQHandler [WEAK] - EXPORT I2C1_ER_IRQHandler [WEAK] - EXPORT I2C2_EV_IRQHandler [WEAK] - EXPORT I2C2_ER_IRQHandler [WEAK] - EXPORT SPI1_IRQHandler [WEAK] - EXPORT SPI2_IRQHandler [WEAK] - EXPORT USART1_IRQHandler [WEAK] - EXPORT USART2_IRQHandler [WEAK] - EXPORT USART3_IRQHandler [WEAK] - EXPORT EXTI15_10_IRQHandler [WEAK] - EXPORT RTCAlarm_IRQHandler [WEAK] - EXPORT USBWakeUp_IRQHandler [WEAK] - -WWDG_IRQHandler -PVD_IRQHandler -TAMPER_IRQHandler -RTC_IRQHandler -FLASH_IRQHandler -RCC_IRQHandler -EXTI0_IRQHandler -EXTI1_IRQHandler -EXTI2_IRQHandler -EXTI3_IRQHandler -EXTI4_IRQHandler -DMA1_Channel1_IRQHandler -DMA1_Channel2_IRQHandler -DMA1_Channel3_IRQHandler -DMA1_Channel4_IRQHandler -DMA1_Channel5_IRQHandler -DMA1_Channel6_IRQHandler -DMA1_Channel7_IRQHandler -ADC1_2_IRQHandler -USB_HP_CAN1_TX_IRQHandler -USB_LP_CAN1_RX0_IRQHandler -CAN1_RX1_IRQHandler -CAN1_SCE_IRQHandler -EXTI9_5_IRQHandler -TIM1_BRK_IRQHandler -TIM1_UP_IRQHandler -TIM1_TRG_COM_IRQHandler -TIM1_CC_IRQHandler -TIM2_IRQHandler -TIM3_IRQHandler -TIM4_IRQHandler -I2C1_EV_IRQHandler -I2C1_ER_IRQHandler -I2C2_EV_IRQHandler -I2C2_ER_IRQHandler -SPI1_IRQHandler -SPI2_IRQHandler -USART1_IRQHandler -USART2_IRQHandler -USART3_IRQHandler -EXTI15_10_IRQHandler -RTCAlarm_IRQHandler -USBWakeUp_IRQHandler - - B . - - ENDP - - ALIGN - -;******************************************************************************* -; User Stack and Heap initialization -;******************************************************************************* - IF :DEF:__MICROLIB - - EXPORT __initial_sp - EXPORT __heap_base - EXPORT __heap_limit - - ELSE - - IMPORT __use_two_region_memory - EXPORT __user_initial_stackheap - -__user_initial_stackheap - - LDR R0, = Heap_Mem - LDR R1, =(Stack_Mem + Stack_Size) - LDR R2, = (Heap_Mem + Heap_Size) - LDR R3, = Stack_Mem - BX LR - - ALIGN - - ENDIF - - END - -;******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE***** diff --git a/src/board.h b/src/board.h index 077a6201de..cfc16333dd 100755 --- a/src/board.h +++ b/src/board.h @@ -176,22 +176,6 @@ typedef struct baro_t { } baro_t; // Hardware definitions and GPIO -#ifdef FY90Q - // FY90Q -#define LED0_GPIO GPIOC -#define LED0_PIN Pin_12 -#define LED1_GPIO GPIOA -#define LED1_PIN Pin_15 - -#define GYRO -#define ACC -#define LED0 -#define LED1 - -#define SENSORS_SET (SENSOR_ACC) - -#else - #ifdef OLIMEXINO // OLIMEXINO @@ -238,7 +222,6 @@ typedef struct baro_t { #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) // #define PROD_DEBUG -#endif #endif // Helpful macros @@ -276,14 +259,6 @@ typedef struct baro_t { #include "utils.h" -#ifdef FY90Q - // FY90Q -#include "drv_adc.h" -#include "drv_i2c.h" -#include "drv_pwm.h" -#include "drv_uart.h" -#else - #ifdef OLIMEXINO // OLIMEXINO #include "drv_adc.h" @@ -322,4 +297,3 @@ typedef struct baro_t { #include "drv_hcsr04.h" #endif -#endif diff --git a/src/drv_adc.h b/src/drv_adc.h index ad43533076..788a74dea5 100755 --- a/src/drv_adc.h +++ b/src/drv_adc.h @@ -13,6 +13,3 @@ typedef struct drv_adc_config_t { void adcInit(drv_adc_config_t *init); uint16_t adcGetChannel(uint8_t channel); -#ifdef FY90Q -void adcSensorInit(sensor_t *acc, sensor_t *gyro); -#endif diff --git a/src/drv_adc_fy90q.c b/src/drv_adc_fy90q.c deleted file mode 100644 index 07689ce0e7..0000000000 --- a/src/drv_adc_fy90q.c +++ /dev/null @@ -1,145 +0,0 @@ -#ifdef FY90Q -#include "board.h" - -#define ADC_CHANNELS 9 - -volatile uint16_t adcData[ADC_CHANNELS] = { 0, }; -extern uint16_t acc_1G; - -static void adcAccRead(int16_t *accelData); -static void adcAccAlign(int16_t *accelData); -static void adcGyroRead(int16_t *gyroData); -static void adcGyroAlign(int16_t *gyroData); -static void adcDummyInit(void); - -void adcSensorInit(sensor_t *acc, sensor_t *gyro) -{ - acc->init = adcDummyInit; - acc->read = adcAccRead; - acc->align = adcAccAlign; - - gyro->init = adcDummyInit; - gyro->read = adcGyroRead; - gyro->align = adcGyroAlign; - gyro->scale = 1.0f; - - acc_1G = 376; -} - -void adcCalibrateADC(ADC_TypeDef *ADCx, int n) -{ - while (n > 0) { - delay(5); - // Enable ADC reset calibration register - ADC_ResetCalibration(ADCx); - // Check the end of ADC reset calibration register - while(ADC_GetResetCalibrationStatus(ADCx)); - // Start ADC calibration - ADC_StartCalibration(ADCx); - // Check the end of ADC calibration - while(ADC_GetCalibrationStatus(ADCx)); - n--; - } -} - -void adcInit(void) -{ - ADC_InitTypeDef ADC_InitStructure; - DMA_InitTypeDef DMA_InitStructure; - - // ADC assumes all the GPIO was already placed in 'AIN' mode - DMA_DeInit(DMA1_Channel1); - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR; - DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&adcData; - DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; - DMA_InitStructure.DMA_BufferSize = ADC_CHANNELS; - DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; - DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; - DMA_InitStructure.DMA_Priority = DMA_Priority_High; - DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; - DMA_Init(DMA1_Channel1, &DMA_InitStructure); - /* Enable DMA1 channel1 */ - DMA_Cmd(DMA1_Channel1, ENABLE); - - ADC_InitStructure.ADC_Mode = ADC_Mode_Independent; - ADC_InitStructure.ADC_ScanConvMode = ENABLE; - ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; - ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None; - ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; - ADC_InitStructure.ADC_NbrOfChannel = ADC_CHANNELS; - ADC_Init(ADC1, &ADC_InitStructure); - - ADC_RegularChannelConfig(ADC1, ADC_Channel_10, 1, ADC_SampleTime_28Cycles5); // GY_X - ADC_RegularChannelConfig(ADC1, ADC_Channel_11, 2, ADC_SampleTime_28Cycles5); // GY_Y - ADC_RegularChannelConfig(ADC1, ADC_Channel_12, 3, ADC_SampleTime_28Cycles5); // GY_Z - - ADC_RegularChannelConfig(ADC1, ADC_Channel_13, 4, ADC_SampleTime_28Cycles5); // ACC_X - ADC_RegularChannelConfig(ADC1, ADC_Channel_14, 5, ADC_SampleTime_28Cycles5); // ACC_Y - ADC_RegularChannelConfig(ADC1, ADC_Channel_15, 6, ADC_SampleTime_28Cycles5); // ACC_Z - - ADC_RegularChannelConfig(ADC1, ADC_Channel_5, 7, ADC_SampleTime_28Cycles5); // POT_ELE - ADC_RegularChannelConfig(ADC1, ADC_Channel_6, 8, ADC_SampleTime_28Cycles5); // POT_AIL - ADC_RegularChannelConfig(ADC1, ADC_Channel_7, 9, ADC_SampleTime_28Cycles5); // POT_RUD - - ADC_DMACmd(ADC1, ENABLE); - - ADC_Cmd(ADC1, ENABLE); - - // Calibrate ADC - adcCalibrateADC(ADC1, 2); - - // Fire off ADC - ADC_SoftwareStartConvCmd(ADC1, ENABLE); -} - -static void adcAccRead(int16_t *accelData) -{ - // ADXL335 - // 300mV/g - // Vcc 3.0V - accelData[0] = adcData[3]; - accelData[1] = adcData[4]; - accelData[2] = adcData[5]; -} - -static void adcAccAlign(int16_t *accelData) -{ - // align OK -} - -static void adcGyroRead(int16_t *gyroData) -{ - // Vcc: 3.0V - // Pitch/Roll: LPR550AL, 2000dps mode. - // 0.5mV/dps - // Zero-rate: 1.23V - // Yaw: LPY550AL, 2000dps mode. - // 0.5mV/dps - // Zero-rate: 1.23V - - // Need to match with: 14.375lsb per dps - // 12-bit ADC - - gyroData[0] = adcData[0] * 2; - gyroData[1] = adcData[1] * 2; - gyroData[2] = adcData[2] * 2; -} - -static void adcGyroAlign(int16_t *gyroData) -{ - // align OK -} - -static void adcDummyInit(void) -{ - // nothing to init here -} - -uint16_t adcGetBattery(void) -{ - return 0; -} -#endif diff --git a/src/drv_pwm_fy90q.c b/src/drv_pwm_fy90q.c deleted file mode 100644 index b1236f9865..0000000000 --- a/src/drv_pwm_fy90q.c +++ /dev/null @@ -1,344 +0,0 @@ -#ifdef FY90Q -#include "board.h" - -#define PULSE_1MS (1000) // 1ms pulse width -// #define PULSE_PERIOD (2500) // pulse period (400Hz) -// #define PULSE_PERIOD_SERVO_DIGITAL (5000) // pulse period for digital servo (200Hz) -// #define PULSE_PERIOD_SERVO_ANALOG (20000) // pulse period for analog servo (50Hz) - -// Forward declaration -static void pwmIRQHandler(TIM_TypeDef *tim); -static void ppmIRQHandler(TIM_TypeDef *tim); - -// external vars (ugh) -extern int16_t failsafeCnt; - -// local vars -static struct TIM_Channel { - TIM_TypeDef *tim; - uint16_t channel; - uint16_t cc; -} Channels[] = { - { TIM2, TIM_Channel_1, TIM_IT_CC1 }, - { TIM2, TIM_Channel_2, TIM_IT_CC2 }, - { TIM2, TIM_Channel_3, TIM_IT_CC3 }, - { TIM2, TIM_Channel_4, TIM_IT_CC4 }, - { TIM3, TIM_Channel_1, TIM_IT_CC1 }, - { TIM3, TIM_Channel_2, TIM_IT_CC2 }, - { TIM3, TIM_Channel_3, TIM_IT_CC3 }, - { TIM3, TIM_Channel_4, TIM_IT_CC4 }, -}; - -static volatile uint16_t *OutputChannels[] = { - &(TIM4->CCR1), - &(TIM4->CCR2), - &(TIM4->CCR3), - &(TIM4->CCR4), - // Extended use during CPPM input (TODO) - &(TIM3->CCR1), - &(TIM3->CCR2), - &(TIM3->CCR3), - &(TIM3->CCR4), -}; - -static struct PWM_State { - uint8_t state; - uint16_t rise; - uint16_t fall; - uint16_t capture; -} Inputs[8] = { { 0, } }; - -static TIM_ICInitTypeDef TIM_ICInitStructure = { 0, }; -static bool usePPMFlag = false; -static uint8_t numOutputChannels = 0; - -void TIM2_IRQHandler(void) -{ - if (usePPMFlag) - ppmIRQHandler(TIM2); - else - pwmIRQHandler(TIM2); -} - -static void ppmIRQHandler(TIM_TypeDef *tim) -{ - uint16_t diff; - static uint16_t now; - static uint16_t last = 0; - static uint8_t chan = 0; - static uint8_t GoodPulses; - - if (TIM_GetITStatus(tim, TIM_IT_CC1) == SET) { - last = now; - now = TIM_GetCapture1(tim); - rcActive = true; - } - - TIM_ClearITPendingBit(tim, TIM_IT_CC1); - - if (now > last) { - diff = (now - last); - } else { - diff = ((0xFFFF - last) + now); - } - - if (diff > 4000) { - chan = 0; - } else { - if (diff > PULSE_MIN && diff < PULSE_MAX && chan < 8) { // 750 to 2250 ms is our 'valid' channel range - Inputs[chan].capture = diff; - if (chan < 4 && diff > FAILSAFE_DETECT_TRESHOLD) - GoodPulses |= (1 << chan); // if signal is valid - mark channel as OK - if (GoodPulses == 0x0F) { // If first four chanells have good pulses, clear FailSafe counter - GoodPulses = 0; - if (failsafeCnt > 20) - failsafeCnt -= 20; - else - failsafeCnt = 0; - } - } - chan++; - failsafeCnt = 0; - } -} - -static void pwmIRQHandler(TIM_TypeDef *tim) -{ - uint8_t i; - uint16_t val = 0; - - for (i = 0; i < 8; i++) { - struct TIM_Channel channel = Channels[i]; - struct PWM_State *state = &Inputs[i]; - - if (channel.tim == tim && (TIM_GetITStatus(tim, channel.cc) == SET)) { - TIM_ClearITPendingBit(channel.tim, channel.cc); - - switch (channel.channel) { - case TIM_Channel_1: - val = TIM_GetCapture1(channel.tim); - break; - case TIM_Channel_2: - val = TIM_GetCapture2(channel.tim); - break; - case TIM_Channel_3: - val = TIM_GetCapture3(channel.tim); - break; - case TIM_Channel_4: - val = TIM_GetCapture4(channel.tim); - break; - } - - if (state->state == 0) - state->rise = val; - else - state->fall = val; - - if (state->state == 0) { - // switch states - state->state = 1; - - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; - TIM_ICInitStructure.TIM_Channel = channel.channel; - TIM_ICInit(channel.tim, &TIM_ICInitStructure); - } else { - // compute capture - if (state->fall > state->rise) - state->capture = (state->fall - state->rise); - else - state->capture = ((0xffff - state->rise) + state->fall); - - // switch state - state->state = 0; - - // ping failsafe - failsafeCnt = 0; - - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; - TIM_ICInitStructure.TIM_Channel = channel.channel; - TIM_ICInit(channel.tim, &TIM_ICInitStructure); - } - } - } -} - -static void pwmInitializeInput(bool usePPM) -{ - GPIO_InitTypeDef GPIO_InitStructure = { 0, }; - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = { 0, }; - NVIC_InitTypeDef NVIC_InitStructure = { 0, }; - uint8_t i; - - // Input pins - if (usePPM) { - // Configure TIM2_CH1 for PPM input - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; - GPIO_Init(GPIOA, &GPIO_InitStructure); - - // Input timer on TIM2 only for PPM - NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); - - // TIM2 timebase - TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); - TIM_TimeBaseStructure.TIM_Prescaler = (72 - 1); - TIM_TimeBaseStructure.TIM_Period = 0xffff; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); - - // Input capture on TIM2_CH1 for PPM - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; - TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; - TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; - TIM_ICInitStructure.TIM_ICFilter = 0x0; - TIM_ICInitStructure.TIM_Channel = TIM_Channel_1; - TIM_ICInit(TIM2, &TIM_ICInitStructure); - - // TIM2_CH1 capture compare interrupt enable - TIM_ITConfig(TIM2, TIM_IT_CC1, ENABLE); - TIM_Cmd(TIM2, ENABLE); - - // configure number of PWM outputs, in PPM mode, we use bottom 4 channels more more motors - numOutputChannels = 10; - } else { - // Configure TIM2 all 4 channels - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_4; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; - GPIO_Init(GPIOA, &GPIO_InitStructure); - - // TODO Configure EXTI4 1 channel - - // Input timers on TIM2 for PWM - NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); - - // TIM2 timebase - TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); - TIM_TimeBaseStructure.TIM_Prescaler = (72 - 1); - TIM_TimeBaseStructure.TIM_Period = 0xffff; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); - - // PWM Input capture - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; - TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; - TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; - TIM_ICInitStructure.TIM_ICFilter = 0x0; - - for (i = 0; i < 4; i++) { - TIM_ICInitStructure.TIM_Channel = Channels[i].channel; - TIM_ICInit(Channels[i].tim, &TIM_ICInitStructure); - } - - // TODO EXTI4 - - TIM_ITConfig(TIM2, TIM_IT_CC1 | TIM_IT_CC2 | TIM_IT_CC3 | TIM_IT_CC4, ENABLE); - // TODO EXTI4 - TIM_Cmd(TIM2, ENABLE); - - // In PWM input mode, all 4 channels are wasted - numOutputChannels = 4; - } -} - -bool pwmInit(drv_pwm_config_t *init) -{ - GPIO_InitTypeDef GPIO_InitStructure = { 0, }; - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = { 0, }; - TIM_OCInitTypeDef TIM_OCInitStructure = { 0, }; - - uint8_t i; - - // Inputs - - // RX1 TIM2_CH1 PA0 [also PPM] [also used for throttle calibration] - // RX2 TIM2_CH2 PA1 - // RX3 TIM2_CH3 PA2 [also UART2_TX] - // RX4 TIM2_CH4 PA3 [also UART2_RX] - // RX5 TIM3_CH1 PA6 [also ADC_IN6] - // RX6 TIM3_CH2 PA7 [also ADC_IN7] - // RX7 TIM3_CH3 PB0 [also ADC_IN8] - // RX8 TIM3_CH4 PB1 [also ADC_IN9] - - // Outputs - // PWM1 TIM1_CH1 PA8 - // PWM2 TIM1_CH4 PA11 - // PWM3 TIM4_CH1 PB6 [also I2C1_SCL] - // PWM4 TIM4_CH2 PB7 [also I2C1_SDA] - // PWM5 TIM4_CH3 PB8 - // PWM6 TIM4_CH4 PB9 - - // use PPM or PWM input - usePPMFlag = init->usePPM; - - // preset channels to center - for (i = 0; i < 8; i++) - Inputs[i].capture = 1500; - - // Timers run at 1mhz. - // TODO: clean this shit up. Make it all dynamic etc. - if (init->enableInput) - pwmInitializeInput(usePPMFlag); - - // Output pins (4x) - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; - GPIO_Init(GPIOB, &GPIO_InitStructure); - - // Output timer - TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); - TIM_TimeBaseStructure.TIM_Prescaler = (72 - 1); - - TIM_TimeBaseStructure.TIM_Period = (1000000 / init->motorPwmRate) - 1; - TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); - - TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; - TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; - TIM_OCInitStructure.TIM_Pulse = PULSE_1MS; - TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; - TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; - - // PWM1,2,3,4 - TIM_OC1Init(TIM4, &TIM_OCInitStructure); - TIM_OC2Init(TIM4, &TIM_OCInitStructure); - TIM_OC3Init(TIM4, &TIM_OCInitStructure); - TIM_OC4Init(TIM4, &TIM_OCInitStructure); - TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable); - TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable); - TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable); - TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable); - - TIM_Cmd(TIM4, ENABLE); - TIM_CtrlPWMOutputs(TIM4, ENABLE); - TIM_OC1PreloadConfig (TIM4, TIM_OCPreload_Enable); - - // turn on more motor outputs if we're using ppm / not using pwm input - if (!init->enableInput || init->usePPM) { - // TODO - } - - return false; -} - -void pwmWrite(uint8_t channel, uint16_t value) -{ - if (channel < numOutputChannels) - *OutputChannels[channel] = value; -} - -uint16_t pwmRead(uint8_t channel) -{ - return Inputs[channel].capture; -} -#endif diff --git a/src/drv_system.c b/src/drv_system.c index 6d92d81beb..4607170921 100755 --- a/src/drv_system.c +++ b/src/drv_system.c @@ -121,9 +121,7 @@ void systemInit(bool overclock) SysTick_Config(SystemCoreClock / 1000); // Configure the rest of the stuff -#ifndef FY90Q i2cInit(I2C2); -#endif spiInit(); // sleep for 100ms diff --git a/src/sensors.c b/src/sensors.c index b2e0385bce..4a952adb69 100755 --- a/src/sensors.c +++ b/src/sensors.c @@ -20,15 +20,6 @@ sensor_t gyro; // gyro access functions baro_t baro; // barometer access functions uint8_t accHardware = ACC_DEFAULT; // which accel chip is used/detected -#ifdef FY90Q -// FY90Q analog gyro/acc -bool sensorsAutodetect(void) -{ - adcSensorInit(&acc, &gyro); - return true; -} -#else -// AfroFlight32 i2c sensors bool sensorsAutodetect(void) { int16_t deg, min; @@ -133,7 +124,6 @@ retry: return true; } -#endif uint16_t batteryAdcToVoltage(uint16_t src) { From d050c7df0c05fac810b1a696ed47bbcae5340106 Mon Sep 17 00:00:00 2001 From: dongie Date: Fri, 30 May 2014 10:05:38 +0900 Subject: [PATCH 4/5] fixed ADC initialization with skipped channels - spotted by Lux. Also increased ADC sample time to 239.5 cycles (~10uS at 24MHz ADC clock). --- src/drv_adc.c | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/drv_adc.c b/src/drv_adc.c index 98da373a76..779e763816 100755 --- a/src/drv_adc.c +++ b/src/drv_adc.c @@ -18,7 +18,7 @@ void adcInit(drv_adc_config_t *init) { ADC_InitTypeDef adc; DMA_InitTypeDef dma; - int numChannels = 1, i; + int numChannels = 1, i, rank = 1; // configure always-present battery index (ADC4) adcConfig[ADC_BATTERY].adcChannel = ADC_Channel_4; @@ -61,11 +61,10 @@ void adcInit(drv_adc_config_t *init) adc.ADC_NbrOfChannel = numChannels; ADC_Init(ADC1, &adc); - // fixed ADC4 - ADC_RegularChannelConfig(ADC1, ADC_Channel_4, 1, ADC_SampleTime_28Cycles5); - // configure any additional ADC channels (2 + n) - for (i = 1; i < numChannels; i++) - ADC_RegularChannelConfig(ADC1, adcConfig[i].adcChannel, i + 1, ADC_SampleTime_28Cycles5); + // Configure ADC channels. Skip if channel is zero, means can't use PA0, but OK for this situation. + for (i = 0; i < ADC_CHANNEL_MAX; i++) + if (adcConfig[i].adcChannel > 0) + ADC_RegularChannelConfig(ADC1, adcConfig[i].adcChannel, rank++, ADC_SampleTime_239Cycles5); ADC_DMACmd(ADC1, ENABLE); ADC_Cmd(ADC1, ENABLE); From 5cc9750d123c786ededfb9e69b3f8e2033181b81 Mon Sep 17 00:00:00 2001 From: luggi Date: Thu, 29 May 2014 02:57:01 +0200 Subject: [PATCH 5/5] current sensor support added also optimized the vbat code a bit --- src/cli.c | 3 +++ src/config.c | 3 ++- src/drv_adc.c | 4 ++-- src/drv_adc.h | 2 +- src/mw.c | 27 ++++++++++++++++++++------- src/mw.h | 8 +++++++- src/sensors.c | 11 +++++++++++ src/serial.c | 9 ++++++--- 8 files changed, 52 insertions(+), 15 deletions(-) diff --git a/src/cli.c b/src/cli.c index fbd20e19b6..a8f9141928 100644 --- a/src/cli.c +++ b/src/cli.c @@ -141,6 +141,9 @@ const clivalue_t valueTable[] = { { "telemetry_port", VAR_UINT8, &mcfg.telemetry_port, 0, TELEMETRY_PORT_MAX }, { "telemetry_switch", VAR_UINT8, &mcfg.telemetry_switch, 0, 1 }, { "vbatscale", VAR_UINT8, &mcfg.vbatscale, 10, 200 }, + { "currentscale", VAR_UINT16, &mcfg.currentscale, 1, 10000 }, + { "currentoffset", VAR_UINT16, &mcfg.currentoffset, 0, 1650 }, + { "multiwiicurrentoutput", VAR_UINT8, &mcfg.multiwiicurrentoutput, 0, 1 }, { "vbatmaxcellvoltage", VAR_UINT8, &mcfg.vbatmaxcellvoltage, 10, 50 }, { "vbatmincellvoltage", VAR_UINT8, &mcfg.vbatmincellvoltage, 10, 50 }, { "power_adc_channel", VAR_UINT8, &mcfg.power_adc_channel, 0, 9 }, diff --git a/src/config.c b/src/config.c index 5b17ab8674..7703528826 100755 --- a/src/config.c +++ b/src/config.c @@ -13,7 +13,7 @@ master_t mcfg; // master config struct with data independent from profiles config_t cfg; // profile config struct const char rcChannelLetters[] = "AERT1234"; -static const uint8_t EEPROM_CONF_VERSION = 63; +static const uint8_t EEPROM_CONF_VERSION = 64; static uint32_t enabledSensors = 0; static void resetConf(void); @@ -187,6 +187,7 @@ static void resetConf(void) mcfg.max_angle_inclination = 500; // 50 degrees mcfg.yaw_control_direction = 1; mcfg.moron_threshold = 32; + mcfg.currentscale = 400; // for Allegro ACS758LCB-100U (40mV/A) mcfg.vbatscale = 110; mcfg.vbatmaxcellvoltage = 43; mcfg.vbatmincellvoltage = 33; diff --git a/src/drv_adc.c b/src/drv_adc.c index 779e763816..965522c777 100755 --- a/src/drv_adc.c +++ b/src/drv_adc.c @@ -33,8 +33,8 @@ void adcInit(drv_adc_config_t *init) // another channel can be stolen from PWM for current measurement or other things if (init->powerAdcChannel > 0) { numChannels++; - adcConfig[ADC_EXTERNAL2].adcChannel = init->powerAdcChannel; - adcConfig[ADC_EXTERNAL2].dmaIndex = numChannels - 1; + adcConfig[ADC_EXTERNAL_CURRENT].adcChannel = init->powerAdcChannel; + adcConfig[ADC_EXTERNAL_CURRENT].dmaIndex = numChannels - 1; } // ADC driver assumes all the GPIO was already placed in 'AIN' mode diff --git a/src/drv_adc.h b/src/drv_adc.h index 788a74dea5..1f09b2e794 100755 --- a/src/drv_adc.h +++ b/src/drv_adc.h @@ -3,7 +3,7 @@ typedef enum { ADC_BATTERY = 0, ADC_EXTERNAL1 = 1, - ADC_EXTERNAL2 = 2, + ADC_EXTERNAL_CURRENT = 2, ADC_CHANNEL_MAX = 3 } AdcChannel; diff --git a/src/mw.c b/src/mw.c index ac64393cbe..f9fdceac36 100755 --- a/src/mw.c +++ b/src/mw.c @@ -12,7 +12,9 @@ uint32_t previousTime = 0; uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop int16_t headFreeModeHold; -uint8_t vbat; // battery voltage in 0.1V steps +uint16_t vbat; // battery voltage in 0.1V steps +int32_t amperage; // amperage read by current sensor in centiampere (1/100th A) +uint32_t mAhdrawn; // milliampere hours drawn from the battery since start int16_t telemTemperature1; // gyro sensor temperature int16_t failsafeCnt = 0; @@ -90,9 +92,10 @@ void annexCode(void) // vbat shit static uint8_t vbatTimer = 0; - static uint8_t ind = 0; - uint16_t vbatRaw = 0; - static uint16_t vbatRawArray[8]; + static int32_t vbatRaw = 0; + static int32_t amperageRaw = 0; + static uint32_t mAhdrawnRaw = 0; + static uint32_t vbatCycleTime = 0; int i; @@ -155,11 +158,21 @@ void annexCode(void) } if (feature(FEATURE_VBAT)) { + vbatCycleTime += cycleTime; if (!(++vbatTimer % VBATFREQ)) { - vbatRawArray[(ind++) % 8] = adcGetChannel(ADC_BATTERY); - for (i = 0; i < 8; i++) - vbatRaw += vbatRawArray[i]; + vbatRaw -= vbatRaw / 8; + vbatRaw += adcGetChannel(ADC_BATTERY); vbat = batteryAdcToVoltage(vbatRaw / 8); + + if (mcfg.power_adc_channel > 0) { + amperageRaw -= amperageRaw / 8; + amperageRaw += adcGetChannel(ADC_EXTERNAL_CURRENT); + amperage = currentSensorToCentiamps(amperageRaw / 8); + mAhdrawnRaw += (amperage * vbatCycleTime) / 1000; // will overflow at ~11000mAh + mAhdrawn = mAhdrawnRaw / (3600 * 100); + vbatCycleTime = 0; + } + } if ((vbat > batteryWarningVoltage) || (vbat < mcfg.vbatmincellvoltage)) { // VBAT ok, buzzer off buzzerFreq = 0; diff --git a/src/mw.h b/src/mw.h index c368cdc5f3..add967baa0 100755 --- a/src/mw.h +++ b/src/mw.h @@ -256,6 +256,9 @@ typedef struct master_t { int16_t magZero[3]; // Battery/ADC stuff + uint16_t currentscale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A + uint16_t currentoffset; // offset of the current sensor in millivolt steps + uint8_t multiwiicurrentoutput; // if set to 1 output the amperage in milliamp steps instead of 0.01A steps via msp uint8_t vbatscale; // adjust this to match battery voltage to reported value uint8_t vbatmaxcellvoltage; // maximum voltage per cell, used for auto-detecting battery voltage in 0.1V units, default is 43 (4.3V) uint8_t vbatmincellvoltage; // minimum voltage per cell, this triggers battery out alarms, in 0.1V units, default is 33 (3.3V) @@ -363,8 +366,10 @@ extern int16_t motor[MAX_MOTORS]; extern int16_t servo[MAX_SERVOS]; extern int16_t rcData[RC_CHANS]; extern uint16_t rssi; // range: [0;1023] -extern uint8_t vbat; +extern uint16_t vbat; // battery voltage in 0.1V steps extern int16_t telemTemperature1; // gyro sensor temperature +extern int32_t amperage; // amperage read by current sensor in 0.01A steps +extern uint32_t mAhdrawn; // milli ampere hours drawn from battery since start extern uint8_t toggleBeep; #define PITCH_LOOKUP_LENGTH 7 @@ -415,6 +420,7 @@ int getEstimatedAltitude(void); bool sensorsAutodetect(void); void batteryInit(void); uint16_t batteryAdcToVoltage(uint16_t src); +int32_t currentSensorToCentiamps(uint16_t src); void ACC_getADC(void); int Baro_update(void); void Gyro_getADC(void); diff --git a/src/sensors.c b/src/sensors.c index 4a952adb69..b9453c8692 100755 --- a/src/sensors.c +++ b/src/sensors.c @@ -132,6 +132,17 @@ uint16_t batteryAdcToVoltage(uint16_t src) return (((src) * 3.3f) / 4095) * mcfg.vbatscale; } +#define ADCVREF 33L +int32_t currentSensorToCentiamps(uint16_t src) +{ + int32_t millivolts; + + millivolts = ((uint32_t)src * ADCVREF * 100) / 4095; + millivolts -= mcfg.currentoffset; + + return (millivolts * 1000) / (int32_t)mcfg.currentscale; // current in 0.01A steps +} + void batteryInit(void) { uint32_t i; diff --git a/src/serial.c b/src/serial.c index e6b212f88e..99ba54495d 100755 --- a/src/serial.c +++ b/src/serial.c @@ -471,10 +471,13 @@ static void evaluateCommand(void) break; case MSP_ANALOG: headSerialReply(7); - serialize8(vbat); - serialize16(0); // power meter trash + serialize8((uint8_t)constrain(vbat, 0, 255)); + serialize16(mAhdrawn); // milliamphours drawn from battery serialize16(rssi); - serialize16(0); // amperage + if(mcfg.multiwiicurrentoutput) { + serialize16((uint16_t)constrain((abs(amperage)*10), 0, 0xFFFF)); // send amperage in 0.001 A steps + } else + serialize16((uint16_t)abs(amperage)); // send amperage in 0.01 A steps break; case MSP_RC_TUNING: headSerialReply(7);