Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

ak7401ctrl.cpp@10:5c69b067d88a, 2016-07-08 (annotated)

Committer:
masahikofukasawa
Date:
Fri Jul 08 22:26:26 2016 +0000
Revision:
10:5c69b067d88a
Child:
11:cef8dc1cf010
RevD with AK09970 Release to Japan.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
masahikofukasawa 10:5c69b067d88a 1 #include "ak7401ctrl.h"
masahikofukasawa 10:5c69b067d88a 2 #include "debug.h"
masahikofukasawa 10:5c69b067d88a 3
masahikofukasawa 10:5c69b067d88a 4 /**
masahikofukasawa 10:5c69b067d88a 5 * Constructor.
masahikofukasawa 10:5c69b067d88a 6 *
masahikofukasawa 10:5c69b067d88a 7 */
masahikofukasawa 10:5c69b067d88a 8 Ak7401Ctrl::Ak7401Ctrl(){
masahikofukasawa 10:5c69b067d88a 9 ak7401 = NULL;
masahikofukasawa 10:5c69b067d88a 10 event = false;
masahikofukasawa 10:5c69b067d88a 11 }
masahikofukasawa 10:5c69b067d88a 12
masahikofukasawa 10:5c69b067d88a 13 /**
masahikofukasawa 10:5c69b067d88a 14 * Destructor.
masahikofukasawa 10:5c69b067d88a 15 *
masahikofukasawa 10:5c69b067d88a 16 */
masahikofukasawa 10:5c69b067d88a 17 Ak7401Ctrl::~Ak7401Ctrl(){
masahikofukasawa 10:5c69b067d88a 18 if (ak7401) delete ak7401;
masahikofukasawa 10:5c69b067d88a 19 }
masahikofukasawa 10:5c69b067d88a 20
masahikofukasawa 10:5c69b067d88a 21 AkmSensor::Status Ak7401Ctrl::init(const uint8_t id, const uint8_t subid){
masahikofukasawa 10:5c69b067d88a 22 primaryId = id;
masahikofukasawa 10:5c69b067d88a 23 subId = subid;
masahikofukasawa 10:5c69b067d88a 24
masahikofukasawa 10:5c69b067d88a 25 if(subId == SUB_ID_AK7401){
masahikofukasawa 10:5c69b067d88a 26 SPI* mSpi;
masahikofukasawa 10:5c69b067d88a 27 DigitalOut* mCs;
masahikofukasawa 10:5c69b067d88a 28 AK7401::Status status = AK7401::ERROR;
masahikofukasawa 10:5c69b067d88a 29 mSpi = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK);
masahikofukasawa 10:5c69b067d88a 30 mSpi->format(8,1); // 8bit, Mode=1
masahikofukasawa 10:5c69b067d88a 31 mSpi->frequency(1000000);
masahikofukasawa 10:5c69b067d88a 32 mCs = new DigitalOut(SPI_CS);
masahikofukasawa 10:5c69b067d88a 33 ak7401 = new AK7401();
masahikofukasawa 10:5c69b067d88a 34 ak7401->begin(mSpi, mCs);
masahikofukasawa 10:5c69b067d88a 35
masahikofukasawa 10:5c69b067d88a 36 status = ak7401->setOperationMode(AK7401::AK7401_USER_MODE);
masahikofukasawa 10:5c69b067d88a 37 if( status != AK7401::SUCCESS ){
masahikofukasawa 10:5c69b067d88a 38 MSG("#AK7401 user mode failed.\n");
masahikofukasawa 10:5c69b067d88a 39 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 40 }
masahikofukasawa 10:5c69b067d88a 41 MSG("#AK7401 user mode succeed.\n");
masahikofukasawa 10:5c69b067d88a 42
masahikofukasawa 10:5c69b067d88a 43 char data[2] = {0x00,0x07}; // set clockwise rotation
masahikofukasawa 10:5c69b067d88a 44 status = ak7401->writeEEPROM(0x05,data); // set clockwise
masahikofukasawa 10:5c69b067d88a 45 if( status != AK7401::SUCCESS ){
masahikofukasawa 10:5c69b067d88a 46 MSG("#AK7401 write EEPROM failed.\n");
masahikofukasawa 10:5c69b067d88a 47 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 48 }
masahikofukasawa 10:5c69b067d88a 49 MSG("#AK7401 write EEPROM succeed.\n");
masahikofukasawa 10:5c69b067d88a 50
masahikofukasawa 10:5c69b067d88a 51 status = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE);
masahikofukasawa 10:5c69b067d88a 52 if( status != AK7401::SUCCESS ){
masahikofukasawa 10:5c69b067d88a 53 MSG("#AK7401 normal mode failed.\n");
masahikofukasawa 10:5c69b067d88a 54 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 55 }
masahikofukasawa 10:5c69b067d88a 56 MSG("#AK7401 normal mode succeed.\n");
masahikofukasawa 10:5c69b067d88a 57 }
masahikofukasawa 10:5c69b067d88a 58 else{
masahikofukasawa 10:5c69b067d88a 59 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 60 }
masahikofukasawa 10:5c69b067d88a 61
masahikofukasawa 10:5c69b067d88a 62 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 63 }
masahikofukasawa 10:5c69b067d88a 64
masahikofukasawa 10:5c69b067d88a 65 void Ak7401Ctrl::eventCallback(){
masahikofukasawa 10:5c69b067d88a 66 event = true;
masahikofukasawa 10:5c69b067d88a 67 }
masahikofukasawa 10:5c69b067d88a 68
masahikofukasawa 10:5c69b067d88a 69 bool Ak7401Ctrl::isEvent(){
masahikofukasawa 10:5c69b067d88a 70 return event;
masahikofukasawa 10:5c69b067d88a 71 }
masahikofukasawa 10:5c69b067d88a 72
masahikofukasawa 10:5c69b067d88a 73 AkmSensor::Status Ak7401Ctrl::startSensor(){
masahikofukasawa 10:5c69b067d88a 74 interval = 0.1; // 10Hz
masahikofukasawa 10:5c69b067d88a 75 ticker.attach(this, &Ak7401Ctrl::eventCallback, interval);
masahikofukasawa 10:5c69b067d88a 76 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 77 }
masahikofukasawa 10:5c69b067d88a 78
masahikofukasawa 10:5c69b067d88a 79 AkmSensor::Status Ak7401Ctrl::startSensor(const float sec){
masahikofukasawa 10:5c69b067d88a 80 interval = sec;
masahikofukasawa 10:5c69b067d88a 81 ticker.attach(this, &Ak7401Ctrl::eventCallback, interval);
masahikofukasawa 10:5c69b067d88a 82 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 83 }
masahikofukasawa 10:5c69b067d88a 84
masahikofukasawa 10:5c69b067d88a 85 AkmSensor::Status Ak7401Ctrl::stopSensor(){
masahikofukasawa 10:5c69b067d88a 86 ticker.detach();
masahikofukasawa 10:5c69b067d88a 87 event = false;
masahikofukasawa 10:5c69b067d88a 88 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 89 }
masahikofukasawa 10:5c69b067d88a 90
masahikofukasawa 10:5c69b067d88a 91 AkmSensor::Status Ak7401Ctrl::readSensorData(Message* msg){
masahikofukasawa 10:5c69b067d88a 92 event = false;
masahikofukasawa 10:5c69b067d88a 93 char angle[2] = {0x00,0x00};
masahikofukasawa 10:5c69b067d88a 94 AK7401::Status status = ak7401->readAngle(angle);
masahikofukasawa 10:5c69b067d88a 95
masahikofukasawa 10:5c69b067d88a 96 msg->setCommand(Message::CMD_START_MEASUREMENT);
masahikofukasawa 10:5c69b067d88a 97 msg->setArgument( 0, status );
masahikofukasawa 10:5c69b067d88a 98 msg->setArgument( 1, angle[0] );
masahikofukasawa 10:5c69b067d88a 99 msg->setArgument( 2, angle[1] );
masahikofukasawa 10:5c69b067d88a 100
masahikofukasawa 10:5c69b067d88a 101 if( status != SUCCESS){
masahikofukasawa 10:5c69b067d88a 102 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 103 }
masahikofukasawa 10:5c69b067d88a 104 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 105 }
masahikofukasawa 10:5c69b067d88a 106
masahikofukasawa 10:5c69b067d88a 107 AkmSensor::Status Ak7401Ctrl::requestCommand(Message* in, Message* out){
masahikofukasawa 10:5c69b067d88a 108 AkmSensor::Status status = AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 109
masahikofukasawa 10:5c69b067d88a 110 Message::Command cmd = in->getCommand();
masahikofukasawa 10:5c69b067d88a 111
masahikofukasawa 10:5c69b067d88a 112 switch(cmd){
masahikofukasawa 10:5c69b067d88a 113 case Message::CMD_ANGLE_ZERO_RESET:
masahikofukasawa 10:5c69b067d88a 114 {
masahikofukasawa 10:5c69b067d88a 115 AK7401::Status st;
masahikofukasawa 10:5c69b067d88a 116 if( Ak7401Ctrl::stopSensor() != AkmSensor::SUCCESS ){
masahikofukasawa 10:5c69b067d88a 117 MSG("#Error stop sensor\n");
masahikofukasawa 10:5c69b067d88a 118 status = AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 119 }
masahikofukasawa 10:5c69b067d88a 120
masahikofukasawa 10:5c69b067d88a 121 st = ak7401->setOperationMode(AK7401::AK7401_USER_MODE);
masahikofukasawa 10:5c69b067d88a 122 if( st != AK7401::SUCCESS ){
masahikofukasawa 10:5c69b067d88a 123 MSG("#Error when set user mode\n");
masahikofukasawa 10:5c69b067d88a 124 status = AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 125 }
masahikofukasawa 10:5c69b067d88a 126 char temp[2] = {0x00,0x00};
masahikofukasawa 10:5c69b067d88a 127 st = ak7401->writeRegister(0x06,temp);
masahikofukasawa 10:5c69b067d88a 128 if( st != AK7401::SUCCESS ){
masahikofukasawa 10:5c69b067d88a 129 MSG("#Error temp read: code=%d\n",st);
masahikofukasawa 10:5c69b067d88a 130 status = AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 131 }
masahikofukasawa 10:5c69b067d88a 132 st = ak7401->setAngleZero(); // reset ZP data
masahikofukasawa 10:5c69b067d88a 133 if( st != AK7401::SUCCESS ){
masahikofukasawa 10:5c69b067d88a 134 MSG("#Error setAngleZero: code=%d\n",st);
masahikofukasawa 10:5c69b067d88a 135 status = AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 136 }
masahikofukasawa 10:5c69b067d88a 137 st = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE);
masahikofukasawa 10:5c69b067d88a 138 if( st != AK7401::SUCCESS ){
masahikofukasawa 10:5c69b067d88a 139 MSG("#Error when set normal mode\n");
masahikofukasawa 10:5c69b067d88a 140 status = AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 141 }
masahikofukasawa 10:5c69b067d88a 142 if( Ak7401Ctrl::startSensor(interval) != AkmSensor::SUCCESS ){
masahikofukasawa 10:5c69b067d88a 143 MSG("#Error start sensor\n");
masahikofukasawa 10:5c69b067d88a 144 status = AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 145 }
masahikofukasawa 10:5c69b067d88a 146 if( status == AkmSensor::ERROR ){
masahikofukasawa 10:5c69b067d88a 147 out->setArgument(0,1);
masahikofukasawa 10:5c69b067d88a 148 }else{
masahikofukasawa 10:5c69b067d88a 149 out->setArgument(0,0);
masahikofukasawa 10:5c69b067d88a 150 }
masahikofukasawa 10:5c69b067d88a 151 break;
masahikofukasawa 10:5c69b067d88a 152 }
masahikofukasawa 10:5c69b067d88a 153 default:
masahikofukasawa 10:5c69b067d88a 154 {
masahikofukasawa 10:5c69b067d88a 155 MSG("#Error no command.\n");
masahikofukasawa 10:5c69b067d88a 156 status = AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 157 break;
masahikofukasawa 10:5c69b067d88a 158 }
masahikofukasawa 10:5c69b067d88a 159 }
masahikofukasawa 10:5c69b067d88a 160
masahikofukasawa 10:5c69b067d88a 161 return status;
masahikofukasawa 10:5c69b067d88a 162 }
masahikofukasawa 10:5c69b067d88a 163