Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
ak7451ctrl.cpp@14:21e177fc308a, 2016-09-12 (annotated)
- Committer:
- masahikofukasawa
- Date:
- Mon Sep 12 17:24:11 2016 +0000
- Revision:
- 14:21e177fc308a
- Parent:
- 13:d008249f0359
- Child:
- 16:d85be9bafb80
Rev.009 Added AK7401
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
masahikofukasawa | 11:cef8dc1cf010 | 1 | #include "ak7451ctrl.h" |
masahikofukasawa | 11:cef8dc1cf010 | 2 | #include "debug.h" |
masahikofukasawa | 11:cef8dc1cf010 | 3 | |
masahikofukasawa | 11:cef8dc1cf010 | 4 | /** |
masahikofukasawa | 11:cef8dc1cf010 | 5 | * Constructor. |
masahikofukasawa | 11:cef8dc1cf010 | 6 | * |
masahikofukasawa | 11:cef8dc1cf010 | 7 | */ |
masahikofukasawa | 11:cef8dc1cf010 | 8 | Ak7451Ctrl::Ak7451Ctrl(){ |
masahikofukasawa | 11:cef8dc1cf010 | 9 | ak7451 = NULL; |
masahikofukasawa | 11:cef8dc1cf010 | 10 | event = false; |
masahikofukasawa | 13:d008249f0359 | 11 | sensorName = ""; |
masahikofukasawa | 11:cef8dc1cf010 | 12 | } |
masahikofukasawa | 11:cef8dc1cf010 | 13 | |
masahikofukasawa | 11:cef8dc1cf010 | 14 | /** |
masahikofukasawa | 11:cef8dc1cf010 | 15 | * Destructor. |
masahikofukasawa | 11:cef8dc1cf010 | 16 | * |
masahikofukasawa | 11:cef8dc1cf010 | 17 | */ |
masahikofukasawa | 11:cef8dc1cf010 | 18 | Ak7451Ctrl::~Ak7451Ctrl(){ |
masahikofukasawa | 11:cef8dc1cf010 | 19 | if (ak7451) delete ak7451; |
masahikofukasawa | 11:cef8dc1cf010 | 20 | } |
masahikofukasawa | 11:cef8dc1cf010 | 21 | |
masahikofukasawa | 11:cef8dc1cf010 | 22 | AkmSensor::Status Ak7451Ctrl::init(const uint8_t id, const uint8_t subid){ |
masahikofukasawa | 11:cef8dc1cf010 | 23 | primaryId = id; |
masahikofukasawa | 11:cef8dc1cf010 | 24 | subId = subid; |
masahikofukasawa | 11:cef8dc1cf010 | 25 | |
masahikofukasawa | 11:cef8dc1cf010 | 26 | if(subId == SUB_ID_AK7451){ |
masahikofukasawa | 11:cef8dc1cf010 | 27 | SPI* mSpi; |
masahikofukasawa | 11:cef8dc1cf010 | 28 | DigitalOut* mCs; |
masahikofukasawa | 11:cef8dc1cf010 | 29 | AK7451::Status status = AK7451::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 30 | mSpi = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK); |
masahikofukasawa | 11:cef8dc1cf010 | 31 | mSpi->format(8,1); // 8bit, Mode=1 |
masahikofukasawa | 11:cef8dc1cf010 | 32 | mSpi->frequency(1000000); |
masahikofukasawa | 11:cef8dc1cf010 | 33 | mCs = new DigitalOut(SPI_CS); |
masahikofukasawa | 11:cef8dc1cf010 | 34 | ak7451 = new AK7451(); |
masahikofukasawa | 11:cef8dc1cf010 | 35 | ak7451->begin(mSpi, mCs); |
masahikofukasawa | 13:d008249f0359 | 36 | sensorName = "AK7451"; |
masahikofukasawa | 11:cef8dc1cf010 | 37 | |
masahikofukasawa | 11:cef8dc1cf010 | 38 | status = ak7451->setOperationMode(AK7451::AK7451_USER_MODE); |
masahikofukasawa | 11:cef8dc1cf010 | 39 | if( status != AK7451::SUCCESS ){ |
masahikofukasawa | 11:cef8dc1cf010 | 40 | MSG("#AK7451 user mode failed.\r\n"); |
masahikofukasawa | 11:cef8dc1cf010 | 41 | return AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 42 | } |
masahikofukasawa | 11:cef8dc1cf010 | 43 | MSG("#AK7451 user mode succeed.\r\n"); |
masahikofukasawa | 11:cef8dc1cf010 | 44 | |
masahikofukasawa | 11:cef8dc1cf010 | 45 | char data[2] = {0x02,0x00}; // set clockwise rotation |
masahikofukasawa | 11:cef8dc1cf010 | 46 | status = ak7451->writeEEPROM(0x07,data); // set clockwise |
masahikofukasawa | 11:cef8dc1cf010 | 47 | if( status != AK7451::SUCCESS ){ |
masahikofukasawa | 11:cef8dc1cf010 | 48 | MSG("#AK7451 write EEPROM failed.\r\n"); |
masahikofukasawa | 11:cef8dc1cf010 | 49 | return AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 50 | } |
masahikofukasawa | 11:cef8dc1cf010 | 51 | MSG("#AK7451 write EEPROM succeed.\r\n"); |
masahikofukasawa | 11:cef8dc1cf010 | 52 | |
masahikofukasawa | 11:cef8dc1cf010 | 53 | status = ak7451->setOperationMode(AK7451::AK7451_NORMAL_MODE); |
masahikofukasawa | 11:cef8dc1cf010 | 54 | if( status != AK7451::SUCCESS ){ |
masahikofukasawa | 11:cef8dc1cf010 | 55 | MSG("#AK7451 normal mode failed.\r\n"); |
masahikofukasawa | 11:cef8dc1cf010 | 56 | return AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 57 | } |
masahikofukasawa | 11:cef8dc1cf010 | 58 | MSG("#AK7451 normal mode succeed.\r\n"); |
masahikofukasawa | 11:cef8dc1cf010 | 59 | |
masahikofukasawa | 11:cef8dc1cf010 | 60 | interval = SENSOR_SAMPLING_RATE; // 10Hz |
masahikofukasawa | 11:cef8dc1cf010 | 61 | } |
masahikofukasawa | 11:cef8dc1cf010 | 62 | else{ |
masahikofukasawa | 11:cef8dc1cf010 | 63 | return AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 64 | } |
masahikofukasawa | 11:cef8dc1cf010 | 65 | |
masahikofukasawa | 11:cef8dc1cf010 | 66 | return AkmSensor::SUCCESS; |
masahikofukasawa | 11:cef8dc1cf010 | 67 | } |
masahikofukasawa | 11:cef8dc1cf010 | 68 | |
masahikofukasawa | 11:cef8dc1cf010 | 69 | void Ak7451Ctrl::eventCallback(){ |
masahikofukasawa | 11:cef8dc1cf010 | 70 | event = true; |
masahikofukasawa | 11:cef8dc1cf010 | 71 | } |
masahikofukasawa | 11:cef8dc1cf010 | 72 | |
masahikofukasawa | 11:cef8dc1cf010 | 73 | bool Ak7451Ctrl::isEvent(){ |
masahikofukasawa | 11:cef8dc1cf010 | 74 | return event; |
masahikofukasawa | 11:cef8dc1cf010 | 75 | } |
masahikofukasawa | 11:cef8dc1cf010 | 76 | |
masahikofukasawa | 11:cef8dc1cf010 | 77 | AkmSensor::Status Ak7451Ctrl::startSensor(){ |
masahikofukasawa | 11:cef8dc1cf010 | 78 | ticker.attach(this, &Ak7451Ctrl::eventCallback, interval); |
masahikofukasawa | 11:cef8dc1cf010 | 79 | return AkmSensor::SUCCESS; |
masahikofukasawa | 11:cef8dc1cf010 | 80 | } |
masahikofukasawa | 11:cef8dc1cf010 | 81 | |
masahikofukasawa | 11:cef8dc1cf010 | 82 | AkmSensor::Status Ak7451Ctrl::startSensor(const float sec){ |
masahikofukasawa | 11:cef8dc1cf010 | 83 | interval = sec; |
masahikofukasawa | 11:cef8dc1cf010 | 84 | ticker.attach(this, &Ak7451Ctrl::eventCallback, interval); |
masahikofukasawa | 11:cef8dc1cf010 | 85 | return AkmSensor::SUCCESS; |
masahikofukasawa | 11:cef8dc1cf010 | 86 | } |
masahikofukasawa | 11:cef8dc1cf010 | 87 | |
masahikofukasawa | 11:cef8dc1cf010 | 88 | AkmSensor::Status Ak7451Ctrl::stopSensor(){ |
masahikofukasawa | 11:cef8dc1cf010 | 89 | ticker.detach(); |
masahikofukasawa | 11:cef8dc1cf010 | 90 | event = false; |
masahikofukasawa | 11:cef8dc1cf010 | 91 | return AkmSensor::SUCCESS; |
masahikofukasawa | 11:cef8dc1cf010 | 92 | } |
masahikofukasawa | 11:cef8dc1cf010 | 93 | |
masahikofukasawa | 11:cef8dc1cf010 | 94 | AkmSensor::Status Ak7451Ctrl::readSensorData(Message* msg){ |
masahikofukasawa | 11:cef8dc1cf010 | 95 | event = false; |
masahikofukasawa | 11:cef8dc1cf010 | 96 | char angle[2] = {0x00,0x00}; |
masahikofukasawa | 11:cef8dc1cf010 | 97 | AK7451::Status status = ak7451->readAngle(angle); |
masahikofukasawa | 11:cef8dc1cf010 | 98 | |
masahikofukasawa | 11:cef8dc1cf010 | 99 | msg->setCommand(Message::CMD_START_MEASUREMENT); |
masahikofukasawa | 11:cef8dc1cf010 | 100 | msg->setArgument( 0, status ); |
masahikofukasawa | 11:cef8dc1cf010 | 101 | msg->setArgument( 1, angle[0] ); |
masahikofukasawa | 11:cef8dc1cf010 | 102 | msg->setArgument( 2, angle[1] ); |
masahikofukasawa | 11:cef8dc1cf010 | 103 | |
masahikofukasawa | 11:cef8dc1cf010 | 104 | if( status != SUCCESS){ |
masahikofukasawa | 11:cef8dc1cf010 | 105 | return AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 106 | } |
masahikofukasawa | 11:cef8dc1cf010 | 107 | return AkmSensor::SUCCESS; |
masahikofukasawa | 11:cef8dc1cf010 | 108 | } |
masahikofukasawa | 11:cef8dc1cf010 | 109 | |
masahikofukasawa | 11:cef8dc1cf010 | 110 | AkmSensor::Status Ak7451Ctrl::requestCommand(Message* in, Message* out){ |
masahikofukasawa | 11:cef8dc1cf010 | 111 | AkmSensor::Status status = AkmSensor::SUCCESS; |
masahikofukasawa | 11:cef8dc1cf010 | 112 | |
masahikofukasawa | 11:cef8dc1cf010 | 113 | Message::Command cmd = in->getCommand(); |
masahikofukasawa | 11:cef8dc1cf010 | 114 | |
masahikofukasawa | 11:cef8dc1cf010 | 115 | switch(cmd){ |
masahikofukasawa | 11:cef8dc1cf010 | 116 | case Message::CMD_ANGLE_ZERO_RESET: |
masahikofukasawa | 11:cef8dc1cf010 | 117 | { |
masahikofukasawa | 11:cef8dc1cf010 | 118 | AK7451::Status st; |
masahikofukasawa | 11:cef8dc1cf010 | 119 | |
masahikofukasawa | 11:cef8dc1cf010 | 120 | st = ak7451->setOperationMode(AK7451::AK7451_USER_MODE); |
masahikofukasawa | 11:cef8dc1cf010 | 121 | if( st != AK7451::SUCCESS ){ |
masahikofukasawa | 11:cef8dc1cf010 | 122 | MSG("#Error when set user mode\r\n"); |
masahikofukasawa | 11:cef8dc1cf010 | 123 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 124 | } |
masahikofukasawa | 11:cef8dc1cf010 | 125 | st = ak7451->setAngleZero(); // reset ZP data |
masahikofukasawa | 11:cef8dc1cf010 | 126 | if( st != AK7451::SUCCESS ){ |
masahikofukasawa | 11:cef8dc1cf010 | 127 | MSG("#Error setAngleZero: code=%d\r\n",st); |
masahikofukasawa | 11:cef8dc1cf010 | 128 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 129 | } |
masahikofukasawa | 11:cef8dc1cf010 | 130 | st = ak7451->setOperationMode(AK7451::AK7451_NORMAL_MODE); |
masahikofukasawa | 11:cef8dc1cf010 | 131 | if( st != AK7451::SUCCESS ){ |
masahikofukasawa | 11:cef8dc1cf010 | 132 | MSG("#Error when set normal mode\r\n"); |
masahikofukasawa | 11:cef8dc1cf010 | 133 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 134 | } |
masahikofukasawa | 14:21e177fc308a | 135 | |
masahikofukasawa | 11:cef8dc1cf010 | 136 | if( status == AkmSensor::ERROR ){ |
masahikofukasawa | 11:cef8dc1cf010 | 137 | out->setArgument(0,1); |
masahikofukasawa | 11:cef8dc1cf010 | 138 | }else{ |
masahikofukasawa | 11:cef8dc1cf010 | 139 | out->setArgument(0,0); |
masahikofukasawa | 11:cef8dc1cf010 | 140 | } |
masahikofukasawa | 11:cef8dc1cf010 | 141 | break; |
masahikofukasawa | 11:cef8dc1cf010 | 142 | } |
masahikofukasawa | 11:cef8dc1cf010 | 143 | default: |
masahikofukasawa | 11:cef8dc1cf010 | 144 | { |
masahikofukasawa | 11:cef8dc1cf010 | 145 | MSG("#Error no command.\r\n"); |
masahikofukasawa | 11:cef8dc1cf010 | 146 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 147 | break; |
masahikofukasawa | 11:cef8dc1cf010 | 148 | } |
masahikofukasawa | 11:cef8dc1cf010 | 149 | } |
masahikofukasawa | 11:cef8dc1cf010 | 150 | |
masahikofukasawa | 11:cef8dc1cf010 | 151 | return status; |
masahikofukasawa | 11:cef8dc1cf010 | 152 | } |
masahikofukasawa | 13:d008249f0359 | 153 | char* Ak7451Ctrl::getSensorName(){ |
masahikofukasawa | 13:d008249f0359 | 154 | return sensorName; |
masahikofukasawa | 13:d008249f0359 | 155 | } |
masahikofukasawa | 11:cef8dc1cf010 | 156 |