Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
ak7401ctrl.cpp@46:5938ad2039b0, 2017-12-13 (annotated)
- Committer:
- masahikofukasawa
- Date:
- Wed Dec 13 23:35:23 2017 +0000
- Revision:
- 46:5938ad2039b0
- Parent:
- 43:45225713cd58
- Child:
- 47:221ec4b404ec
debugged for warnings
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
masahikofukasawa | 10:5c69b067d88a | 1 | #include "ak7401ctrl.h" |
masahikofukasawa | 10:5c69b067d88a | 2 | |
masahikofukasawa | 10:5c69b067d88a | 3 | /** |
masahikofukasawa | 10:5c69b067d88a | 4 | * Constructor. |
masahikofukasawa | 10:5c69b067d88a | 5 | * |
masahikofukasawa | 10:5c69b067d88a | 6 | */ |
masahikofukasawa | 27:41aa9fb23a2f | 7 | Ak7401Ctrl::Ak7401Ctrl() : AkmSensor(){ |
masahikofukasawa | 10:5c69b067d88a | 8 | ak7401 = NULL; |
masahikofukasawa | 10:5c69b067d88a | 9 | } |
masahikofukasawa | 10:5c69b067d88a | 10 | |
masahikofukasawa | 10:5c69b067d88a | 11 | /** |
masahikofukasawa | 10:5c69b067d88a | 12 | * Destructor. |
masahikofukasawa | 10:5c69b067d88a | 13 | * |
masahikofukasawa | 10:5c69b067d88a | 14 | */ |
masahikofukasawa | 10:5c69b067d88a | 15 | Ak7401Ctrl::~Ak7401Ctrl(){ |
masahikofukasawa | 10:5c69b067d88a | 16 | if (ak7401) delete ak7401; |
masahikofukasawa | 10:5c69b067d88a | 17 | } |
masahikofukasawa | 10:5c69b067d88a | 18 | |
masahikofukasawa | 10:5c69b067d88a | 19 | AkmSensor::Status Ak7401Ctrl::init(const uint8_t id, const uint8_t subid){ |
masahikofukasawa | 10:5c69b067d88a | 20 | primaryId = id; |
masahikofukasawa | 10:5c69b067d88a | 21 | subId = subid; |
masahikofukasawa | 10:5c69b067d88a | 22 | |
masahikofukasawa | 10:5c69b067d88a | 23 | if(subId == SUB_ID_AK7401){ |
masahikofukasawa | 10:5c69b067d88a | 24 | SPI* mSpi; |
masahikofukasawa | 10:5c69b067d88a | 25 | DigitalOut* mCs; |
masahikofukasawa | 10:5c69b067d88a | 26 | AK7401::Status status = AK7401::ERROR; |
masahikofukasawa | 10:5c69b067d88a | 27 | mSpi = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK); |
masahikofukasawa | 10:5c69b067d88a | 28 | mSpi->format(8,1); // 8bit, Mode=1 |
masahikofukasawa | 10:5c69b067d88a | 29 | mSpi->frequency(1000000); |
masahikofukasawa | 10:5c69b067d88a | 30 | mCs = new DigitalOut(SPI_CS); |
masahikofukasawa | 10:5c69b067d88a | 31 | ak7401 = new AK7401(); |
masahikofukasawa | 10:5c69b067d88a | 32 | ak7401->begin(mSpi, mCs); |
masahikofukasawa | 13:d008249f0359 | 33 | sensorName = "AK7401"; |
masahikofukasawa | 10:5c69b067d88a | 34 | |
masahikofukasawa | 10:5c69b067d88a | 35 | status = ak7401->setOperationMode(AK7401::AK7401_USER_MODE); |
masahikofukasawa | 10:5c69b067d88a | 36 | if( status != AK7401::SUCCESS ){ |
masahikofukasawa | 14:21e177fc308a | 37 | MSG("#AK7401 set USER mode failed.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 38 | return AkmSensor::ERROR; |
masahikofukasawa | 10:5c69b067d88a | 39 | } |
masahikofukasawa | 14:21e177fc308a | 40 | |
masahikofukasawa | 14:21e177fc308a | 41 | // check connection |
masahikofukasawa | 14:21e177fc308a | 42 | status = ak7401->checkConnection(); |
masahikofukasawa | 14:21e177fc308a | 43 | if( status != AK7401::SUCCESS ){ |
masahikofukasawa | 14:21e177fc308a | 44 | MSG("#AK7401 check connection failed.\r\n"); |
masahikofukasawa | 14:21e177fc308a | 45 | return AkmSensor::ERROR; |
masahikofukasawa | 14:21e177fc308a | 46 | } |
masahikofukasawa | 14:21e177fc308a | 47 | |
masahikofukasawa | 14:21e177fc308a | 48 | // change rotation direction to CW from CCW |
masahikofukasawa | 10:5c69b067d88a | 49 | char data[2] = {0x00,0x07}; // set clockwise rotation |
masahikofukasawa | 10:5c69b067d88a | 50 | status = ak7401->writeEEPROM(0x05,data); // set clockwise |
masahikofukasawa | 10:5c69b067d88a | 51 | if( status != AK7401::SUCCESS ){ |
masahikofukasawa | 14:21e177fc308a | 52 | MSG("#AK7401 set rotation failed.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 53 | return AkmSensor::ERROR; |
masahikofukasawa | 10:5c69b067d88a | 54 | } |
masahikofukasawa | 30:5a241d9b3262 | 55 | /* |
masahikofukasawa | 10:5c69b067d88a | 56 | status = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE); |
masahikofukasawa | 10:5c69b067d88a | 57 | if( status != AK7401::SUCCESS ){ |
masahikofukasawa | 14:21e177fc308a | 58 | MSG("#AK7401 set NORMAL mode failed.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 59 | return AkmSensor::ERROR; |
masahikofukasawa | 10:5c69b067d88a | 60 | } |
masahikofukasawa | 30:5a241d9b3262 | 61 | */ |
masahikofukasawa | 14:21e177fc308a | 62 | MSG("#AK7401 init succeed.\r\n"); |
masahikofukasawa | 11:cef8dc1cf010 | 63 | |
masahikofukasawa | 11:cef8dc1cf010 | 64 | interval = SENSOR_SAMPLING_RATE; // 10Hz |
masahikofukasawa | 10:5c69b067d88a | 65 | } |
masahikofukasawa | 10:5c69b067d88a | 66 | else{ |
masahikofukasawa | 10:5c69b067d88a | 67 | return AkmSensor::ERROR; |
masahikofukasawa | 10:5c69b067d88a | 68 | } |
masahikofukasawa | 10:5c69b067d88a | 69 | |
masahikofukasawa | 10:5c69b067d88a | 70 | return AkmSensor::SUCCESS; |
masahikofukasawa | 10:5c69b067d88a | 71 | } |
masahikofukasawa | 10:5c69b067d88a | 72 | |
masahikofukasawa | 10:5c69b067d88a | 73 | AkmSensor::Status Ak7401Ctrl::startSensor(){ |
masahikofukasawa | 30:5a241d9b3262 | 74 | AK7401::Status status = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE); |
masahikofukasawa | 30:5a241d9b3262 | 75 | if( status != AK7401::SUCCESS ){ |
masahikofukasawa | 30:5a241d9b3262 | 76 | MSG("#AK7401 set NORMAL mode failed.\r\n"); |
masahikofukasawa | 30:5a241d9b3262 | 77 | return AkmSensor::ERROR; |
masahikofukasawa | 30:5a241d9b3262 | 78 | } |
masahikofukasawa | 29:b488d2c89fba | 79 | ticker.attach(callback(this, &base::setEvent), interval); |
masahikofukasawa | 10:5c69b067d88a | 80 | return AkmSensor::SUCCESS; |
masahikofukasawa | 10:5c69b067d88a | 81 | } |
masahikofukasawa | 10:5c69b067d88a | 82 | |
masahikofukasawa | 10:5c69b067d88a | 83 | AkmSensor::Status Ak7401Ctrl::startSensor(const float sec){ |
masahikofukasawa | 30:5a241d9b3262 | 84 | AK7401::Status status = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE); |
masahikofukasawa | 30:5a241d9b3262 | 85 | if( status != AK7401::SUCCESS ){ |
masahikofukasawa | 30:5a241d9b3262 | 86 | MSG("#AK7401 set NORMAL mode failed.\r\n"); |
masahikofukasawa | 30:5a241d9b3262 | 87 | return AkmSensor::ERROR; |
masahikofukasawa | 30:5a241d9b3262 | 88 | } |
masahikofukasawa | 10:5c69b067d88a | 89 | interval = sec; |
masahikofukasawa | 29:b488d2c89fba | 90 | ticker.attach(callback(this, &base::setEvent), interval); |
masahikofukasawa | 29:b488d2c89fba | 91 | MSG("#Start sensor %s.\r\n",sensorName); |
masahikofukasawa | 10:5c69b067d88a | 92 | return AkmSensor::SUCCESS; |
masahikofukasawa | 10:5c69b067d88a | 93 | } |
masahikofukasawa | 10:5c69b067d88a | 94 | |
masahikofukasawa | 10:5c69b067d88a | 95 | AkmSensor::Status Ak7401Ctrl::stopSensor(){ |
masahikofukasawa | 30:5a241d9b3262 | 96 | AK7401::Status status = ak7401->setOperationMode(AK7401::AK7401_USER_MODE); |
masahikofukasawa | 30:5a241d9b3262 | 97 | if( status != AK7401::SUCCESS ){ |
masahikofukasawa | 30:5a241d9b3262 | 98 | MSG("#AK7401 set USER mode failed.\r\n"); |
masahikofukasawa | 30:5a241d9b3262 | 99 | return AkmSensor::ERROR; |
masahikofukasawa | 30:5a241d9b3262 | 100 | } |
masahikofukasawa | 10:5c69b067d88a | 101 | ticker.detach(); |
masahikofukasawa | 29:b488d2c89fba | 102 | AkmSensor::clearEvent(); |
masahikofukasawa | 10:5c69b067d88a | 103 | return AkmSensor::SUCCESS; |
masahikofukasawa | 10:5c69b067d88a | 104 | } |
masahikofukasawa | 10:5c69b067d88a | 105 | |
masahikofukasawa | 10:5c69b067d88a | 106 | AkmSensor::Status Ak7401Ctrl::readSensorData(Message* msg){ |
masahikofukasawa | 29:b488d2c89fba | 107 | AkmSensor::clearEvent(); |
masahikofukasawa | 29:b488d2c89fba | 108 | |
masahikofukasawa | 10:5c69b067d88a | 109 | char angle[2] = {0x00,0x00}; |
masahikofukasawa | 10:5c69b067d88a | 110 | AK7401::Status status = ak7401->readAngle(angle); |
masahikofukasawa | 10:5c69b067d88a | 111 | |
masahikofukasawa | 10:5c69b067d88a | 112 | msg->setCommand(Message::CMD_START_MEASUREMENT); |
masahikofukasawa | 10:5c69b067d88a | 113 | msg->setArgument( 0, status ); |
masahikofukasawa | 10:5c69b067d88a | 114 | msg->setArgument( 1, angle[0] ); |
masahikofukasawa | 10:5c69b067d88a | 115 | msg->setArgument( 2, angle[1] ); |
masahikofukasawa | 10:5c69b067d88a | 116 | |
masahikofukasawa | 10:5c69b067d88a | 117 | if( status != SUCCESS){ |
masahikofukasawa | 10:5c69b067d88a | 118 | return AkmSensor::ERROR; |
masahikofukasawa | 10:5c69b067d88a | 119 | } |
masahikofukasawa | 10:5c69b067d88a | 120 | return AkmSensor::SUCCESS; |
masahikofukasawa | 10:5c69b067d88a | 121 | } |
masahikofukasawa | 10:5c69b067d88a | 122 | |
masahikofukasawa | 10:5c69b067d88a | 123 | AkmSensor::Status Ak7401Ctrl::requestCommand(Message* in, Message* out){ |
masahikofukasawa | 10:5c69b067d88a | 124 | AkmSensor::Status status = AkmSensor::SUCCESS; |
masahikofukasawa | 10:5c69b067d88a | 125 | Message::Command cmd = in->getCommand(); |
masahikofukasawa | 10:5c69b067d88a | 126 | switch(cmd){ |
masahikofukasawa | 30:5a241d9b3262 | 127 | case Message::CMD_ANGLE_READ: |
masahikofukasawa | 30:5a241d9b3262 | 128 | { |
masahikofukasawa | 30:5a241d9b3262 | 129 | // read angle |
masahikofukasawa | 30:5a241d9b3262 | 130 | char angle[2] = {0x00,0x00}; |
masahikofukasawa | 30:5a241d9b3262 | 131 | char density = 0x00; |
masahikofukasawa | 30:5a241d9b3262 | 132 | char abnormal = 0x00; |
masahikofukasawa | 30:5a241d9b3262 | 133 | |
masahikofukasawa | 30:5a241d9b3262 | 134 | if( ak7401->readAngleMeasureCommand(angle, &density, &abnormal) != AK7401::SUCCESS ){ |
masahikofukasawa | 39:3821886c902e | 135 | MSG("#Error: Read angle\r\n"); |
masahikofukasawa | 30:5a241d9b3262 | 136 | status = AkmSensor::ERROR; |
masahikofukasawa | 30:5a241d9b3262 | 137 | } |
masahikofukasawa | 30:5a241d9b3262 | 138 | out->setCommand(Message::CMD_ANGLE_READ); |
masahikofukasawa | 30:5a241d9b3262 | 139 | out->setArgument( 0, ((abnormal&0x01)==0x00)|(status==AkmSensor::ERROR) ? 0x01 : 0x00 ); |
masahikofukasawa | 30:5a241d9b3262 | 140 | out->setArgument( 1, angle[0] ); |
masahikofukasawa | 30:5a241d9b3262 | 141 | out->setArgument( 2, angle[1] ); |
masahikofukasawa | 30:5a241d9b3262 | 142 | out->setArgument( 3, density ); |
masahikofukasawa | 30:5a241d9b3262 | 143 | break; |
masahikofukasawa | 30:5a241d9b3262 | 144 | } |
masahikofukasawa | 10:5c69b067d88a | 145 | case Message::CMD_ANGLE_ZERO_RESET: |
masahikofukasawa | 10:5c69b067d88a | 146 | { |
masahikofukasawa | 14:21e177fc308a | 147 | if( ak7401->setAngleZero() != AK7401::SUCCESS ){ |
masahikofukasawa | 39:3821886c902e | 148 | MSG("#Error: Set angle zero\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 149 | status = AkmSensor::ERROR; |
masahikofukasawa | 10:5c69b067d88a | 150 | } |
masahikofukasawa | 10:5c69b067d88a | 151 | if( status == AkmSensor::ERROR ){ |
masahikofukasawa | 10:5c69b067d88a | 152 | out->setArgument(0,1); |
masahikofukasawa | 10:5c69b067d88a | 153 | }else{ |
masahikofukasawa | 10:5c69b067d88a | 154 | out->setArgument(0,0); |
masahikofukasawa | 10:5c69b067d88a | 155 | } |
masahikofukasawa | 10:5c69b067d88a | 156 | break; |
masahikofukasawa | 10:5c69b067d88a | 157 | } |
masahikofukasawa | 30:5a241d9b3262 | 158 | case Message::CMD_REG_WRITE: |
masahikofukasawa | 30:5a241d9b3262 | 159 | case Message::CMD_REG_WRITEN: |
masahikofukasawa | 30:5a241d9b3262 | 160 | { |
masahikofukasawa | 30:5a241d9b3262 | 161 | char address = in->getArgument(0); |
masahikofukasawa | 46:5938ad2039b0 | 162 | const int len = in->getArgument(1); |
masahikofukasawa | 30:5a241d9b3262 | 163 | if(len != 2){ |
masahikofukasawa | 39:3821886c902e | 164 | MSG("#Error: length=%d. Only support 2byte length\r\n",len); |
masahikofukasawa | 30:5a241d9b3262 | 165 | status = AkmSensor::ERROR; |
masahikofukasawa | 30:5a241d9b3262 | 166 | return status; |
masahikofukasawa | 30:5a241d9b3262 | 167 | } |
masahikofukasawa | 30:5a241d9b3262 | 168 | if(in->getArgNum() != len+2){ |
masahikofukasawa | 39:3821886c902e | 169 | MSG("#Error: Argument num. Args=%d\r\n",in->getArgNum()); |
masahikofukasawa | 30:5a241d9b3262 | 170 | status = AkmSensor::ERROR; |
masahikofukasawa | 30:5a241d9b3262 | 171 | out->setArgument(0,(char)status); |
masahikofukasawa | 30:5a241d9b3262 | 172 | return status; |
masahikofukasawa | 30:5a241d9b3262 | 173 | } |
masahikofukasawa | 30:5a241d9b3262 | 174 | |
masahikofukasawa | 30:5a241d9b3262 | 175 | char data[len]; |
masahikofukasawa | 30:5a241d9b3262 | 176 | for(int i=0; i<len; i++){ |
masahikofukasawa | 30:5a241d9b3262 | 177 | data[i] = in->getArgument(i+2); |
masahikofukasawa | 30:5a241d9b3262 | 178 | } |
masahikofukasawa | 30:5a241d9b3262 | 179 | if( ak7401->writeRegister(address, data) != AK7401::SUCCESS) { |
masahikofukasawa | 30:5a241d9b3262 | 180 | status = AkmSensor::ERROR; |
masahikofukasawa | 39:3821886c902e | 181 | MSG("#Error: Register write.\r\n"); |
masahikofukasawa | 30:5a241d9b3262 | 182 | } |
masahikofukasawa | 30:5a241d9b3262 | 183 | if( ak7401->writeEEPROM(address, data) != AK7401::SUCCESS) { |
masahikofukasawa | 30:5a241d9b3262 | 184 | status = AkmSensor::ERROR; |
masahikofukasawa | 39:3821886c902e | 185 | MSG("#Error: EEPROM write.\r\n"); |
masahikofukasawa | 30:5a241d9b3262 | 186 | } |
masahikofukasawa | 30:5a241d9b3262 | 187 | out->setArgument(0,(char)status); |
masahikofukasawa | 30:5a241d9b3262 | 188 | break; |
masahikofukasawa | 30:5a241d9b3262 | 189 | } |
masahikofukasawa | 30:5a241d9b3262 | 190 | case Message::CMD_REG_READ: |
masahikofukasawa | 30:5a241d9b3262 | 191 | case Message::CMD_REG_READN: |
masahikofukasawa | 30:5a241d9b3262 | 192 | { |
masahikofukasawa | 30:5a241d9b3262 | 193 | if(in->getArgNum() != 2){ |
masahikofukasawa | 39:3821886c902e | 194 | MSG("#Error: argument num. Args=%d\r\n",in->getArgNum()); |
masahikofukasawa | 30:5a241d9b3262 | 195 | status = AkmSensor::ERROR; |
masahikofukasawa | 30:5a241d9b3262 | 196 | return status; |
masahikofukasawa | 30:5a241d9b3262 | 197 | } |
masahikofukasawa | 30:5a241d9b3262 | 198 | |
masahikofukasawa | 30:5a241d9b3262 | 199 | char address = in->getArgument(0); |
masahikofukasawa | 46:5938ad2039b0 | 200 | const int len = in->getArgument(1); |
masahikofukasawa | 30:5a241d9b3262 | 201 | if(len != 2){ |
masahikofukasawa | 39:3821886c902e | 202 | MSG("#Error: length=%d. Only support 2byte length\r\n",len); |
masahikofukasawa | 30:5a241d9b3262 | 203 | status = AkmSensor::ERROR; |
masahikofukasawa | 30:5a241d9b3262 | 204 | return status; |
masahikofukasawa | 30:5a241d9b3262 | 205 | } |
masahikofukasawa | 30:5a241d9b3262 | 206 | char data[len]; |
masahikofukasawa | 30:5a241d9b3262 | 207 | if( ak7401->readRegister(address, data) != AK7401::SUCCESS) { |
masahikofukasawa | 30:5a241d9b3262 | 208 | status = AkmSensor::ERROR; |
masahikofukasawa | 39:3821886c902e | 209 | MSG("#Error: register read.\r\n"); |
masahikofukasawa | 30:5a241d9b3262 | 210 | } |
masahikofukasawa | 30:5a241d9b3262 | 211 | for(int i=0; i<len; i++){ |
masahikofukasawa | 30:5a241d9b3262 | 212 | out->setArgument(i, data[i]); |
masahikofukasawa | 30:5a241d9b3262 | 213 | } |
masahikofukasawa | 30:5a241d9b3262 | 214 | break; |
masahikofukasawa | 30:5a241d9b3262 | 215 | } |
masahikofukasawa | 10:5c69b067d88a | 216 | default: |
masahikofukasawa | 10:5c69b067d88a | 217 | { |
masahikofukasawa | 39:3821886c902e | 218 | MSG("#Error: no command.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 219 | status = AkmSensor::ERROR; |
masahikofukasawa | 10:5c69b067d88a | 220 | break; |
masahikofukasawa | 10:5c69b067d88a | 221 | } |
masahikofukasawa | 10:5c69b067d88a | 222 | } |
masahikofukasawa | 10:5c69b067d88a | 223 | |
masahikofukasawa | 10:5c69b067d88a | 224 | return status; |
masahikofukasawa | 10:5c69b067d88a | 225 | } |
masahikofukasawa | 10:5c69b067d88a | 226 | |
masahikofukasawa | 13:d008249f0359 | 227 |