Erik van de Coevering / Mbed 2 deprecated Multicopter_2018

Latest version of my quadcopter controller with an LPC1768 and MPU9250.

Dependencies:   mbed

Currently running on a custom PCB with 30.5 x 30.5mm mounts. There are also 2 PC apps that go with the software; one to set up the PID controller and one to balance the motors and props. If anyone is interested, send me a message and I'll upload them.

IAP/IAP.cpp@8:981f7e2365b6, 2018-07-31 (annotated)

Committer:
Anaesthetix
Date:
Tue Jul 31 20:36:57 2018 +0000
Revision:
8:981f7e2365b6
Parent:
0:0929d3d566cf 
Switched from Madgwick to Mahony as I'm having trouble with slow oscillations caused by the madgwick filter. Fixed an error on the PID algorithm also.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Anaesthetix 0:0929d3d566cf 1 /** IAP : internal Flash memory access library
Anaesthetix 0:0929d3d566cf 2 *
Anaesthetix 0:0929d3d566cf 3 * The internal Flash memory access is described in the LPC1768 and LPC11U24 usermanual.
Anaesthetix 0:0929d3d566cf 4 * http://www.nxp.com/documents/user_manual/UM10360.pdf
Anaesthetix 0:0929d3d566cf 5 * http://www.nxp.com/documents/user_manual/UM10462.pdf
Anaesthetix 0:0929d3d566cf 6 *
Anaesthetix 0:0929d3d566cf 7 * LPC1768 --
Anaesthetix 0:0929d3d566cf 8 * Chapter 2: "LPC17xx Memory map"
Anaesthetix 0:0929d3d566cf 9 * Chapter 32: "LPC17xx Flash memory interface and programming"
Anaesthetix 0:0929d3d566cf 10 * refering Rev. 01 - 4 January 2010
Anaesthetix 0:0929d3d566cf 11 *
Anaesthetix 0:0929d3d566cf 12 * LPC11U24 --
Anaesthetix 0:0929d3d566cf 13 * Chapter 2: "LPC11Uxx Memory mapping"
Anaesthetix 0:0929d3d566cf 14 * Chapter 20: "LPC11Uxx Flash programming firmware"
Anaesthetix 0:0929d3d566cf 15 * refering Rev. 03 - 16 July 2012
Anaesthetix 0:0929d3d566cf 16 *
Anaesthetix 0:0929d3d566cf 17 * Released under the MIT License: http://mbed.org/license/mit
Anaesthetix 0:0929d3d566cf 18 *
Anaesthetix 0:0929d3d566cf 19 * revision 1.0 09-Mar-2010 1st release
Anaesthetix 0:0929d3d566cf 20 * revision 1.1 12-Mar-2010 chaged: to make possible to reserve flash area for user
Anaesthetix 0:0929d3d566cf 21 * it can be set by USER_FLASH_AREA_START and USER_FLASH_AREA_SIZE in IAP.h
Anaesthetix 0:0929d3d566cf 22 * revision 2.0 26-Nov-2012 LPC11U24 code added
Anaesthetix 0:0929d3d566cf 23 * revision 2.1 26-Nov-2012 EEPROM access code imported from Suga koubou san's (http://mbed.org/users/okini3939/) library
Anaesthetix 0:0929d3d566cf 24 * http://mbed.org/users/okini3939/code/M0_EEPROM_test/
Anaesthetix 0:0929d3d566cf 25 * revision 3.0 09-Jan-2015 LPC812 and LPC824 support added
Anaesthetix 0:0929d3d566cf 26 * revision 3.1 13-Jan-2015 LPC1114 support added
Anaesthetix 0:0929d3d566cf 27 * revision 3.1.1 16-Jan-2015 Target MCU name changed for better compatibility across the platforms
Anaesthetix 0:0929d3d566cf 28 */
Anaesthetix 0:0929d3d566cf 29
Anaesthetix 0:0929d3d566cf 30 #include "mbed.h"
Anaesthetix 0:0929d3d566cf 31 #include "IAP.h"
Anaesthetix 0:0929d3d566cf 32
Anaesthetix 0:0929d3d566cf 33 #define USER_FLASH_AREA_START_STR( x ) STR( x )
Anaesthetix 0:0929d3d566cf 34 #define STR( x ) #x
Anaesthetix 0:0929d3d566cf 35
Anaesthetix 0:0929d3d566cf 36 unsigned char user_area[ USER_FLASH_AREA_SIZE ] __attribute__((section( ".ARM.__at_" USER_FLASH_AREA_START_STR( USER_FLASH_AREA_START ) ), zero_init));
Anaesthetix 0:0929d3d566cf 37
Anaesthetix 0:0929d3d566cf 38 /*
Anaesthetix 0:0929d3d566cf 39 * Reserve of flash area is explained by Igor. Please refer next URL
Anaesthetix 0:0929d3d566cf 40 * http://mbed.org/users/okano/notebook/iap-in-application-programming-internal-flash-eras/?page=1#comment-271
Anaesthetix 0:0929d3d566cf 41 */
Anaesthetix 0:0929d3d566cf 42
Anaesthetix 0:0929d3d566cf 43 //unsigned char user_area[ size ] __attribute__((section(".ARM.__at_0x78000"), zero_init));
Anaesthetix 0:0929d3d566cf 44
Anaesthetix 0:0929d3d566cf 45 /*
Anaesthetix 0:0929d3d566cf 46 * IAP command codes
Anaesthetix 0:0929d3d566cf 47 * Table 589. "IAP Command Summary", Chapter 8. "IAP commands", usermanual
Anaesthetix 0:0929d3d566cf 48 */
Anaesthetix 0:0929d3d566cf 49
Anaesthetix 0:0929d3d566cf 50 enum command_code {
Anaesthetix 0:0929d3d566cf 51 IAPCommand_Prepare_sector_for_write_operation = 50,
Anaesthetix 0:0929d3d566cf 52 IAPCommand_Copy_RAM_to_Flash,
Anaesthetix 0:0929d3d566cf 53 IAPCommand_Erase_sector,
Anaesthetix 0:0929d3d566cf 54 IAPCommand_Blank_check_sector,
Anaesthetix 0:0929d3d566cf 55 IAPCommand_Read_part_ID,
Anaesthetix 0:0929d3d566cf 56 IAPCommand_Read_Boot_Code_version,
Anaesthetix 0:0929d3d566cf 57 IAPCommand_Compare,
Anaesthetix 0:0929d3d566cf 58 IAPCommand_Reinvoke_ISP,
Anaesthetix 0:0929d3d566cf 59 IAPCommand_Read_device_serial_number,
Anaesthetix 0:0929d3d566cf 60 #if defined(TARGET_LPC11UXX)
Anaesthetix 0:0929d3d566cf 61 IAPCommand_EEPROM_Write = 61,
Anaesthetix 0:0929d3d566cf 62 IAPCommand_EEPROM_Read,
Anaesthetix 0:0929d3d566cf 63 #elif defined(TARGET_LPC81X) || defined(TARGET_LPC82X)
Anaesthetix 0:0929d3d566cf 64 IAPCommand_Erase_page = 59,
Anaesthetix 0:0929d3d566cf 65 #endif
Anaesthetix 0:0929d3d566cf 66 };
Anaesthetix 0:0929d3d566cf 67
Anaesthetix 0:0929d3d566cf 68 int IAP::reinvoke_isp( void ) {
Anaesthetix 0:0929d3d566cf 69 __disable_irq();
Anaesthetix 0:0929d3d566cf 70
Anaesthetix 0:0929d3d566cf 71 IAP_command[ 0 ] = IAPCommand_Reinvoke_ISP;
Anaesthetix 0:0929d3d566cf 72
Anaesthetix 0:0929d3d566cf 73 iap_entry( IAP_command, IAP_result );
Anaesthetix 0:0929d3d566cf 74
Anaesthetix 0:0929d3d566cf 75 return ( (int)IAP_result[ 0 ] );
Anaesthetix 0:0929d3d566cf 76 }
Anaesthetix 0:0929d3d566cf 77
Anaesthetix 0:0929d3d566cf 78 /** Read part identification number
Anaesthetix 0:0929d3d566cf 79 *
Anaesthetix 0:0929d3d566cf 80 * @return device ID
Anaesthetix 0:0929d3d566cf 81 * @see read_serial()
Anaesthetix 0:0929d3d566cf 82 */
Anaesthetix 0:0929d3d566cf 83 int IAP::read_ID( void )
Anaesthetix 0:0929d3d566cf 84 {
Anaesthetix 0:0929d3d566cf 85 IAP_command[ 0 ] = IAPCommand_Read_part_ID;
Anaesthetix 0:0929d3d566cf 86
Anaesthetix 0:0929d3d566cf 87 iap_entry( IAP_command, IAP_result );
Anaesthetix 0:0929d3d566cf 88
Anaesthetix 0:0929d3d566cf 89 // return ( (int)IAP_result[ 0 ] );
Anaesthetix 0:0929d3d566cf 90 return ( (int)IAP_result[ 1 ] ); // to return the number itself (this command always returns CMD_SUCCESS)
Anaesthetix 0:0929d3d566cf 91 }
Anaesthetix 0:0929d3d566cf 92
Anaesthetix 0:0929d3d566cf 93 int *IAP::read_serial( void ) {
Anaesthetix 0:0929d3d566cf 94 IAP_command[ 0 ] = IAPCommand_Read_device_serial_number;
Anaesthetix 0:0929d3d566cf 95
Anaesthetix 0:0929d3d566cf 96 iap_entry( IAP_command, IAP_result );
Anaesthetix 0:0929d3d566cf 97
Anaesthetix 0:0929d3d566cf 98 // return ( (int)IAP_result[ 0 ] );
Anaesthetix 0:0929d3d566cf 99 return ( (int *)&IAP_result[ 1 ] ); // to return the number itself (this command always returns CMD_SUCCESS)
Anaesthetix 0:0929d3d566cf 100 }
Anaesthetix 0:0929d3d566cf 101
Anaesthetix 0:0929d3d566cf 102 int IAP::blank_check( int start, int end )
Anaesthetix 0:0929d3d566cf 103 {
Anaesthetix 0:0929d3d566cf 104 IAP_command[ 0 ] = IAPCommand_Blank_check_sector;
Anaesthetix 0:0929d3d566cf 105 IAP_command[ 1 ] = (unsigned int)start; // Start Sector Number
Anaesthetix 0:0929d3d566cf 106 IAP_command[ 2 ] = (unsigned int)end; // End Sector Number (should be greater than or equal to start sector number)
Anaesthetix 0:0929d3d566cf 107
Anaesthetix 0:0929d3d566cf 108 iap_entry( IAP_command, IAP_result );
Anaesthetix 0:0929d3d566cf 109
Anaesthetix 0:0929d3d566cf 110 return ( (int)IAP_result[ 0 ] );
Anaesthetix 0:0929d3d566cf 111 }
Anaesthetix 0:0929d3d566cf 112
Anaesthetix 0:0929d3d566cf 113 int IAP::erase( int start, int end )
Anaesthetix 0:0929d3d566cf 114 {
Anaesthetix 0:0929d3d566cf 115 IAP_command[ 0 ] = IAPCommand_Erase_sector;
Anaesthetix 0:0929d3d566cf 116 IAP_command[ 1 ] = (unsigned int)start; // Start Sector Number
Anaesthetix 0:0929d3d566cf 117 IAP_command[ 2 ] = (unsigned int)end; // End Sector Number (should be greater than or equal to start sector number)
Anaesthetix 0:0929d3d566cf 118 IAP_command[ 3 ] = cclk_kHz; // CPU Clock Frequency (CCLK) in kHz
Anaesthetix 0:0929d3d566cf 119
Anaesthetix 0:0929d3d566cf 120 iap_entry( IAP_command, IAP_result );
Anaesthetix 0:0929d3d566cf 121
Anaesthetix 0:0929d3d566cf 122 return ( (int)IAP_result[ 0 ] );
Anaesthetix 0:0929d3d566cf 123 }
Anaesthetix 0:0929d3d566cf 124
Anaesthetix 0:0929d3d566cf 125 int IAP::prepare( int start, int end )
Anaesthetix 0:0929d3d566cf 126 {
Anaesthetix 0:0929d3d566cf 127 IAP_command[ 0 ] = IAPCommand_Prepare_sector_for_write_operation;
Anaesthetix 0:0929d3d566cf 128 IAP_command[ 1 ] = (unsigned int)start; // Start Sector Number
Anaesthetix 0:0929d3d566cf 129 IAP_command[ 2 ] = (unsigned int)end; // End Sector Number (should be greater than or equal to start sector number).
Anaesthetix 0:0929d3d566cf 130
Anaesthetix 0:0929d3d566cf 131 iap_entry( IAP_command, IAP_result );
Anaesthetix 0:0929d3d566cf 132
Anaesthetix 0:0929d3d566cf 133 return ( (int)IAP_result[ 0 ] );
Anaesthetix 0:0929d3d566cf 134 }
Anaesthetix 0:0929d3d566cf 135
Anaesthetix 0:0929d3d566cf 136 int IAP::write( char *source_addr, char *target_addr, int size )
Anaesthetix 0:0929d3d566cf 137 {
Anaesthetix 0:0929d3d566cf 138 IAP_command[ 0 ] = IAPCommand_Copy_RAM_to_Flash;
Anaesthetix 0:0929d3d566cf 139 IAP_command[ 1 ] = (unsigned int)target_addr; // Destination flash address where data bytes are to be written. This address should be a 256 byte boundary.
Anaesthetix 0:0929d3d566cf 140 IAP_command[ 2 ] = (unsigned int)source_addr; // Source RAM address from which data bytes are to be read. This address should be a word boundary.
Anaesthetix 0:0929d3d566cf 141 IAP_command[ 3 ] = size; // Number of bytes to be written. Should be 256 | 512 | 1024 | 4096.
Anaesthetix 0:0929d3d566cf 142 IAP_command[ 4 ] = cclk_kHz; // CPU Clock Frequency (CCLK) in kHz.
Anaesthetix 0:0929d3d566cf 143
Anaesthetix 0:0929d3d566cf 144 iap_entry( IAP_command, IAP_result );
Anaesthetix 0:0929d3d566cf 145
Anaesthetix 0:0929d3d566cf 146 return ( (int)IAP_result[ 0 ] );
Anaesthetix 0:0929d3d566cf 147 }
Anaesthetix 0:0929d3d566cf 148
Anaesthetix 0:0929d3d566cf 149 int IAP::compare( char *source_addr, char *target_addr, int size )
Anaesthetix 0:0929d3d566cf 150 {
Anaesthetix 0:0929d3d566cf 151 IAP_command[ 0 ] = IAPCommand_Compare;
Anaesthetix 0:0929d3d566cf 152 IAP_command[ 1 ] = (unsigned int)target_addr; // Starting flash or RAM address of data bytes to be compared. This address should be a word boundary.
Anaesthetix 0:0929d3d566cf 153 IAP_command[ 2 ] = (unsigned int)source_addr; // Starting flash or RAM address of data bytes to be compared. This address should be a word boundary.
Anaesthetix 0:0929d3d566cf 154 IAP_command[ 3 ] = size; // Number of bytes to be compared; should be a multiple of 4.
Anaesthetix 0:0929d3d566cf 155
Anaesthetix 0:0929d3d566cf 156 iap_entry( IAP_command, IAP_result );
Anaesthetix 0:0929d3d566cf 157
Anaesthetix 0:0929d3d566cf 158 return ( (int)IAP_result[ 0 ] );
Anaesthetix 0:0929d3d566cf 159 }
Anaesthetix 0:0929d3d566cf 160
Anaesthetix 0:0929d3d566cf 161 int IAP::read_BootVer(void)
Anaesthetix 0:0929d3d566cf 162 {
Anaesthetix 0:0929d3d566cf 163 IAP_command[0] = IAPCommand_Read_Boot_Code_version;
Anaesthetix 0:0929d3d566cf 164 IAP_result[1] = 0; // not sure if in high or low bits.
Anaesthetix 0:0929d3d566cf 165 iap_entry(IAP_command, IAP_result);
Anaesthetix 0:0929d3d566cf 166 return ((int)IAP_result[1]);
Anaesthetix 0:0929d3d566cf 167 }
Anaesthetix 0:0929d3d566cf 168
Anaesthetix 0:0929d3d566cf 169 char * IAP::reserved_flash_area_start( void )
Anaesthetix 0:0929d3d566cf 170 {
Anaesthetix 0:0929d3d566cf 171 return ( (char *)USER_FLASH_AREA_START );
Anaesthetix 0:0929d3d566cf 172 }
Anaesthetix 0:0929d3d566cf 173
Anaesthetix 0:0929d3d566cf 174 int IAP::reserved_flash_area_size( void )
Anaesthetix 0:0929d3d566cf 175 {
Anaesthetix 0:0929d3d566cf 176 return ( USER_FLASH_AREA_SIZE );
Anaesthetix 0:0929d3d566cf 177 }
Anaesthetix 0:0929d3d566cf 178
Anaesthetix 0:0929d3d566cf 179 #if defined(TARGET_LPC11UXX)
Anaesthetix 0:0929d3d566cf 180
Anaesthetix 0:0929d3d566cf 181 int IAP::write_eeprom( char *source_addr, char *target_addr, int size )
Anaesthetix 0:0929d3d566cf 182 {
Anaesthetix 0:0929d3d566cf 183 IAP_command[ 0 ] = IAPCommand_EEPROM_Write;
Anaesthetix 0:0929d3d566cf 184 IAP_command[ 1 ] = (unsigned int)target_addr; // Destination EEPROM address where data bytes are to be written. This address should be a 256 byte boundary.
Anaesthetix 0:0929d3d566cf 185 IAP_command[ 2 ] = (unsigned int)source_addr; // Source RAM address from which data bytes are to be read. This address should be a word boundary.
Anaesthetix 0:0929d3d566cf 186 IAP_command[ 3 ] = size; // Number of bytes to be written. Should be 256 | 512 | 1024 | 4096.
Anaesthetix 0:0929d3d566cf 187 IAP_command[ 4 ] = cclk_kHz; // CPU Clock Frequency (CCLK) in kHz.
Anaesthetix 0:0929d3d566cf 188
Anaesthetix 0:0929d3d566cf 189 iap_entry( IAP_command, IAP_result );
Anaesthetix 0:0929d3d566cf 190
Anaesthetix 0:0929d3d566cf 191 return ( (int)IAP_result[ 0 ] );
Anaesthetix 0:0929d3d566cf 192 }
Anaesthetix 0:0929d3d566cf 193
Anaesthetix 0:0929d3d566cf 194 int IAP::read_eeprom( char *source_addr, char *target_addr, int size )
Anaesthetix 0:0929d3d566cf 195 {
Anaesthetix 0:0929d3d566cf 196 IAP_command[ 0 ] = IAPCommand_EEPROM_Read;
Anaesthetix 0:0929d3d566cf 197 IAP_command[ 1 ] = (unsigned int)source_addr; // Source EEPROM address from which data bytes are to be read. This address should be a word boundary.
Anaesthetix 0:0929d3d566cf 198 IAP_command[ 2 ] = (unsigned int)target_addr; // Destination RAM address where data bytes are to be written. This address should be a 256 byte boundary.
Anaesthetix 0:0929d3d566cf 199 IAP_command[ 3 ] = size; // Number of bytes to be written. Should be 256 | 512 | 1024 | 4096.
Anaesthetix 0:0929d3d566cf 200 IAP_command[ 4 ] = cclk_kHz; // CPU Clock Frequency (CCLK) in kHz.
Anaesthetix 0:0929d3d566cf 201
Anaesthetix 0:0929d3d566cf 202 iap_entry( IAP_command, IAP_result );
Anaesthetix 0:0929d3d566cf 203
Anaesthetix 0:0929d3d566cf 204 return ( (int)IAP_result[ 0 ] );
Anaesthetix 0:0929d3d566cf 205 }
Anaesthetix 0:0929d3d566cf 206
Anaesthetix 0:0929d3d566cf 207 #elif defined(TARGET_LPC81X) || defined(TARGET_LPC82X)
Anaesthetix 0:0929d3d566cf 208
Anaesthetix 0:0929d3d566cf 209 int IAP::erase_page( int start, int end )
Anaesthetix 0:0929d3d566cf 210 {
Anaesthetix 0:0929d3d566cf 211 IAP_command[ 0 ] = IAPCommand_Erase_page;
Anaesthetix 0:0929d3d566cf 212 IAP_command[ 1 ] = (unsigned int)start; // Start Sector Number
Anaesthetix 0:0929d3d566cf 213 IAP_command[ 2 ] = (unsigned int)end; // End Sector Number (should be greater than or equal to start sector number)
Anaesthetix 0:0929d3d566cf 214 IAP_command[ 3 ] = cclk_kHz; // CPU Clock Frequency (CCLK) in kHz
Anaesthetix 0:0929d3d566cf 215
Anaesthetix 0:0929d3d566cf 216 iap_entry( IAP_command, IAP_result );
Anaesthetix 0:0929d3d566cf 217
Anaesthetix 0:0929d3d566cf 218 return ( (int)IAP_result[ 0 ] );
Anaesthetix 0:0929d3d566cf 219 }
Anaesthetix 0:0929d3d566cf 220
Anaesthetix 0:0929d3d566cf 221 #endif
Anaesthetix 0:0929d3d566cf 222