Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

ak7451ctrl.cpp@25:76c11ab5060e, 2017-04-14 (annotated)

Committer:
tkstreet
Date:
Fri Apr 14 20:13:38 2017 +0000
Revision:
25:76c11ab5060e
Parent:
16:d85be9bafb80
Child:
34:1ea3357c8d9a
Added partially implemented AP1017 control class, compiles successfully.

Who changed what in which revision?

UserRevisionLine numberNew 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){
tkstreet 25:76c11ab5060e 27 SPI* mSpi; // SPI connection
tkstreet 25:76c11ab5060e 28 DigitalOut* mCs; // Chip select pin
tkstreet 25:76c11ab5060e 29
tkstreet 25:76c11ab5060e 30 // Initialize SPI connection
masahikofukasawa 11:cef8dc1cf010 31 AK7451::Status status = AK7451::ERROR;
masahikofukasawa 11:cef8dc1cf010 32 mSpi = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK);
masahikofukasawa 11:cef8dc1cf010 33 mSpi->format(8,1); // 8bit, Mode=1
masahikofukasawa 11:cef8dc1cf010 34 mSpi->frequency(1000000);
tkstreet 25:76c11ab5060e 35
tkstreet 25:76c11ab5060e 36 // Initialize chip select pin
masahikofukasawa 11:cef8dc1cf010 37 mCs = new DigitalOut(SPI_CS);
masahikofukasawa 11:cef8dc1cf010 38 ak7451 = new AK7451();
masahikofukasawa 11:cef8dc1cf010 39 ak7451->begin(mSpi, mCs);
masahikofukasawa 13:d008249f0359 40 sensorName = "AK7451";
masahikofukasawa 11:cef8dc1cf010 41
tkstreet 25:76c11ab5060e 42 // Set to User Mode
masahikofukasawa 11:cef8dc1cf010 43 status = ak7451->setOperationMode(AK7451::AK7451_USER_MODE);
masahikofukasawa 11:cef8dc1cf010 44 if( status != AK7451::SUCCESS ){
masahikofukasawa 11:cef8dc1cf010 45 MSG("#AK7451 user mode failed.\r\n");
masahikofukasawa 11:cef8dc1cf010 46 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 47 }
masahikofukasawa 11:cef8dc1cf010 48 MSG("#AK7451 user mode succeed.\r\n");
tkstreet 25:76c11ab5060e 49
tkstreet 25:76c11ab5060e 50 // Set clockwise rotation
tkstreet 25:76c11ab5060e 51 char data[2] = {0x02,0x00};
masahikofukasawa 11:cef8dc1cf010 52 status = ak7451->writeEEPROM(0x07,data); // set clockwise
masahikofukasawa 11:cef8dc1cf010 53 if( status != AK7451::SUCCESS ){
masahikofukasawa 11:cef8dc1cf010 54 MSG("#AK7451 write EEPROM failed.\r\n");
masahikofukasawa 11:cef8dc1cf010 55 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 56 }
masahikofukasawa 11:cef8dc1cf010 57 MSG("#AK7451 write EEPROM succeed.\r\n");
masahikofukasawa 11:cef8dc1cf010 58
tkstreet 25:76c11ab5060e 59 // Set to Normal Mode
masahikofukasawa 11:cef8dc1cf010 60 status = ak7451->setOperationMode(AK7451::AK7451_NORMAL_MODE);
masahikofukasawa 11:cef8dc1cf010 61 if( status != AK7451::SUCCESS ){
masahikofukasawa 11:cef8dc1cf010 62 MSG("#AK7451 normal mode failed.\r\n");
masahikofukasawa 11:cef8dc1cf010 63 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 64 }
masahikofukasawa 11:cef8dc1cf010 65 MSG("#AK7451 normal mode succeed.\r\n");
masahikofukasawa 11:cef8dc1cf010 66
masahikofukasawa 11:cef8dc1cf010 67 interval = SENSOR_SAMPLING_RATE; // 10Hz
masahikofukasawa 11:cef8dc1cf010 68 }
masahikofukasawa 11:cef8dc1cf010 69 else{
masahikofukasawa 11:cef8dc1cf010 70 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 71 }
masahikofukasawa 11:cef8dc1cf010 72
masahikofukasawa 11:cef8dc1cf010 73 return AkmSensor::SUCCESS;
masahikofukasawa 11:cef8dc1cf010 74 }
masahikofukasawa 11:cef8dc1cf010 75
masahikofukasawa 11:cef8dc1cf010 76 void Ak7451Ctrl::eventCallback(){
masahikofukasawa 11:cef8dc1cf010 77 event = true;
masahikofukasawa 11:cef8dc1cf010 78 }
masahikofukasawa 11:cef8dc1cf010 79
masahikofukasawa 11:cef8dc1cf010 80 bool Ak7451Ctrl::isEvent(){
masahikofukasawa 11:cef8dc1cf010 81 return event;
masahikofukasawa 11:cef8dc1cf010 82 }
masahikofukasawa 11:cef8dc1cf010 83
masahikofukasawa 11:cef8dc1cf010 84 AkmSensor::Status Ak7451Ctrl::startSensor(){
masahikofukasawa 16:d85be9bafb80 85 ticker.attach(callback(this, &Ak7451Ctrl::eventCallback), interval);
masahikofukasawa 11:cef8dc1cf010 86 return AkmSensor::SUCCESS;
masahikofukasawa 11:cef8dc1cf010 87 }
masahikofukasawa 11:cef8dc1cf010 88
masahikofukasawa 11:cef8dc1cf010 89 AkmSensor::Status Ak7451Ctrl::startSensor(const float sec){
masahikofukasawa 11:cef8dc1cf010 90 interval = sec;
masahikofukasawa 16:d85be9bafb80 91 ticker.attach(callback(this, &Ak7451Ctrl::eventCallback), interval);
masahikofukasawa 11:cef8dc1cf010 92 return AkmSensor::SUCCESS;
masahikofukasawa 11:cef8dc1cf010 93 }
masahikofukasawa 11:cef8dc1cf010 94
masahikofukasawa 11:cef8dc1cf010 95 AkmSensor::Status Ak7451Ctrl::stopSensor(){
masahikofukasawa 11:cef8dc1cf010 96 ticker.detach();
masahikofukasawa 11:cef8dc1cf010 97 event = false;
masahikofukasawa 11:cef8dc1cf010 98 return AkmSensor::SUCCESS;
masahikofukasawa 11:cef8dc1cf010 99 }
masahikofukasawa 11:cef8dc1cf010 100
masahikofukasawa 11:cef8dc1cf010 101 AkmSensor::Status Ak7451Ctrl::readSensorData(Message* msg){
masahikofukasawa 11:cef8dc1cf010 102 event = false;
masahikofukasawa 11:cef8dc1cf010 103 char angle[2] = {0x00,0x00};
masahikofukasawa 11:cef8dc1cf010 104 AK7451::Status status = ak7451->readAngle(angle);
masahikofukasawa 11:cef8dc1cf010 105
masahikofukasawa 11:cef8dc1cf010 106 msg->setCommand(Message::CMD_START_MEASUREMENT);
masahikofukasawa 11:cef8dc1cf010 107 msg->setArgument( 0, status );
masahikofukasawa 11:cef8dc1cf010 108 msg->setArgument( 1, angle[0] );
masahikofukasawa 11:cef8dc1cf010 109 msg->setArgument( 2, angle[1] );
masahikofukasawa 11:cef8dc1cf010 110
masahikofukasawa 11:cef8dc1cf010 111 if( status != SUCCESS){
masahikofukasawa 11:cef8dc1cf010 112 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 113 }
masahikofukasawa 11:cef8dc1cf010 114 return AkmSensor::SUCCESS;
masahikofukasawa 11:cef8dc1cf010 115 }
masahikofukasawa 11:cef8dc1cf010 116
masahikofukasawa 11:cef8dc1cf010 117 AkmSensor::Status Ak7451Ctrl::requestCommand(Message* in, Message* out){
masahikofukasawa 11:cef8dc1cf010 118 AkmSensor::Status status = AkmSensor::SUCCESS;
masahikofukasawa 11:cef8dc1cf010 119
masahikofukasawa 11:cef8dc1cf010 120 Message::Command cmd = in->getCommand();
masahikofukasawa 11:cef8dc1cf010 121
masahikofukasawa 11:cef8dc1cf010 122 switch(cmd){
masahikofukasawa 11:cef8dc1cf010 123 case Message::CMD_ANGLE_ZERO_RESET:
masahikofukasawa 11:cef8dc1cf010 124 {
masahikofukasawa 11:cef8dc1cf010 125 AK7451::Status st;
masahikofukasawa 11:cef8dc1cf010 126
masahikofukasawa 11:cef8dc1cf010 127 st = ak7451->setOperationMode(AK7451::AK7451_USER_MODE);
masahikofukasawa 11:cef8dc1cf010 128 if( st != AK7451::SUCCESS ){
masahikofukasawa 11:cef8dc1cf010 129 MSG("#Error when set user mode\r\n");
masahikofukasawa 11:cef8dc1cf010 130 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 131 }
masahikofukasawa 11:cef8dc1cf010 132 st = ak7451->setAngleZero(); // reset ZP data
masahikofukasawa 11:cef8dc1cf010 133 if( st != AK7451::SUCCESS ){
masahikofukasawa 11:cef8dc1cf010 134 MSG("#Error setAngleZero: code=%d\r\n",st);
masahikofukasawa 11:cef8dc1cf010 135 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 136 }
masahikofukasawa 11:cef8dc1cf010 137 st = ak7451->setOperationMode(AK7451::AK7451_NORMAL_MODE);
masahikofukasawa 11:cef8dc1cf010 138 if( st != AK7451::SUCCESS ){
masahikofukasawa 11:cef8dc1cf010 139 MSG("#Error when set normal mode\r\n");
masahikofukasawa 11:cef8dc1cf010 140 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 141 }
masahikofukasawa 14:21e177fc308a 142
masahikofukasawa 11:cef8dc1cf010 143 if( status == AkmSensor::ERROR ){
masahikofukasawa 11:cef8dc1cf010 144 out->setArgument(0,1);
masahikofukasawa 11:cef8dc1cf010 145 }else{
masahikofukasawa 11:cef8dc1cf010 146 out->setArgument(0,0);
masahikofukasawa 11:cef8dc1cf010 147 }
masahikofukasawa 11:cef8dc1cf010 148 break;
masahikofukasawa 11:cef8dc1cf010 149 }
masahikofukasawa 11:cef8dc1cf010 150 default:
masahikofukasawa 11:cef8dc1cf010 151 {
masahikofukasawa 11:cef8dc1cf010 152 MSG("#Error no command.\r\n");
masahikofukasawa 11:cef8dc1cf010 153 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 154 break;
masahikofukasawa 11:cef8dc1cf010 155 }
masahikofukasawa 11:cef8dc1cf010 156 }
masahikofukasawa 11:cef8dc1cf010 157
masahikofukasawa 11:cef8dc1cf010 158 return status;
masahikofukasawa 11:cef8dc1cf010 159 }
masahikofukasawa 13:d008249f0359 160 char* Ak7451Ctrl::getSensorName(){
masahikofukasawa 13:d008249f0359 161 return sensorName;
masahikofukasawa 13:d008249f0359 162 }
masahikofukasawa 11:cef8dc1cf010 163