Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

ak7401ctrl.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?

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 13:d008249f0359 11 sensorName = "";
masahikofukasawa 10:5c69b067d88a 12 }
masahikofukasawa 10:5c69b067d88a 13
masahikofukasawa 10:5c69b067d88a 14 /**
masahikofukasawa 10:5c69b067d88a 15 * Destructor.
masahikofukasawa 10:5c69b067d88a 16 *
masahikofukasawa 10:5c69b067d88a 17 */
masahikofukasawa 10:5c69b067d88a 18 Ak7401Ctrl::~Ak7401Ctrl(){
masahikofukasawa 10:5c69b067d88a 19 if (ak7401) delete ak7401;
masahikofukasawa 10:5c69b067d88a 20 }
masahikofukasawa 10:5c69b067d88a 21
masahikofukasawa 10:5c69b067d88a 22 AkmSensor::Status Ak7401Ctrl::init(const uint8_t id, const uint8_t subid){
masahikofukasawa 10:5c69b067d88a 23 primaryId = id;
masahikofukasawa 10:5c69b067d88a 24 subId = subid;
masahikofukasawa 10:5c69b067d88a 25
masahikofukasawa 10:5c69b067d88a 26 if(subId == SUB_ID_AK7401){
masahikofukasawa 10:5c69b067d88a 27 SPI* mSpi;
masahikofukasawa 10:5c69b067d88a 28 DigitalOut* mCs;
masahikofukasawa 10:5c69b067d88a 29 AK7401::Status status = AK7401::ERROR;
masahikofukasawa 10:5c69b067d88a 30 mSpi = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK);
masahikofukasawa 10:5c69b067d88a 31 mSpi->format(8,1); // 8bit, Mode=1
masahikofukasawa 10:5c69b067d88a 32 mSpi->frequency(1000000);
masahikofukasawa 10:5c69b067d88a 33 mCs = new DigitalOut(SPI_CS);
masahikofukasawa 10:5c69b067d88a 34 ak7401 = new AK7401();
masahikofukasawa 10:5c69b067d88a 35 ak7401->begin(mSpi, mCs);
masahikofukasawa 13:d008249f0359 36 sensorName = "AK7401";
masahikofukasawa 10:5c69b067d88a 37
masahikofukasawa 10:5c69b067d88a 38 status = ak7401->setOperationMode(AK7401::AK7401_USER_MODE);
masahikofukasawa 10:5c69b067d88a 39 if( status != AK7401::SUCCESS ){
masahikofukasawa 14:21e177fc308a 40 MSG("#AK7401 set USER mode failed.\r\n");
masahikofukasawa 10:5c69b067d88a 41 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 42 }
masahikofukasawa 14:21e177fc308a 43
masahikofukasawa 14:21e177fc308a 44 // check connection
masahikofukasawa 14:21e177fc308a 45 status = ak7401->checkConnection();
masahikofukasawa 14:21e177fc308a 46 if( status != AK7401::SUCCESS ){
masahikofukasawa 14:21e177fc308a 47 MSG("#AK7401 check connection failed.\r\n");
masahikofukasawa 14:21e177fc308a 48 return AkmSensor::ERROR;
masahikofukasawa 14:21e177fc308a 49 }
masahikofukasawa 14:21e177fc308a 50
masahikofukasawa 14:21e177fc308a 51 // change rotation direction to CW from CCW
masahikofukasawa 10:5c69b067d88a 52 char data[2] = {0x00,0x07}; // set clockwise rotation
masahikofukasawa 10:5c69b067d88a 53 status = ak7401->writeEEPROM(0x05,data); // set clockwise
masahikofukasawa 10:5c69b067d88a 54 if( status != AK7401::SUCCESS ){
masahikofukasawa 14:21e177fc308a 55 MSG("#AK7401 set rotation failed.\r\n");
masahikofukasawa 10:5c69b067d88a 56 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 57 }
masahikofukasawa 10:5c69b067d88a 58
masahikofukasawa 10:5c69b067d88a 59 status = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE);
masahikofukasawa 10:5c69b067d88a 60 if( status != AK7401::SUCCESS ){
masahikofukasawa 14:21e177fc308a 61 MSG("#AK7401 set NORMAL mode failed.\r\n");
masahikofukasawa 10:5c69b067d88a 62 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 63 }
masahikofukasawa 14:21e177fc308a 64
masahikofukasawa 14:21e177fc308a 65 MSG("#AK7401 init succeed.\r\n");
masahikofukasawa 11:cef8dc1cf010 66
masahikofukasawa 11:cef8dc1cf010 67 interval = SENSOR_SAMPLING_RATE; // 10Hz
masahikofukasawa 10:5c69b067d88a 68 }
masahikofukasawa 10:5c69b067d88a 69 else{
masahikofukasawa 10:5c69b067d88a 70 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 71 }
masahikofukasawa 10:5c69b067d88a 72
masahikofukasawa 10:5c69b067d88a 73 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 74 }
masahikofukasawa 10:5c69b067d88a 75
masahikofukasawa 10:5c69b067d88a 76 void Ak7401Ctrl::eventCallback(){
masahikofukasawa 10:5c69b067d88a 77 event = true;
masahikofukasawa 10:5c69b067d88a 78 }
masahikofukasawa 10:5c69b067d88a 79
masahikofukasawa 10:5c69b067d88a 80 bool Ak7401Ctrl::isEvent(){
masahikofukasawa 10:5c69b067d88a 81 return event;
masahikofukasawa 10:5c69b067d88a 82 }
masahikofukasawa 10:5c69b067d88a 83
masahikofukasawa 10:5c69b067d88a 84 AkmSensor::Status Ak7401Ctrl::startSensor(){
masahikofukasawa 10:5c69b067d88a 85 ticker.attach(this, &Ak7401Ctrl::eventCallback, interval);
masahikofukasawa 10:5c69b067d88a 86 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 87 }
masahikofukasawa 10:5c69b067d88a 88
masahikofukasawa 10:5c69b067d88a 89 AkmSensor::Status Ak7401Ctrl::startSensor(const float sec){
masahikofukasawa 10:5c69b067d88a 90 interval = sec;
masahikofukasawa 10:5c69b067d88a 91 ticker.attach(this, &Ak7401Ctrl::eventCallback, interval);
masahikofukasawa 10:5c69b067d88a 92 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 93 }
masahikofukasawa 10:5c69b067d88a 94
masahikofukasawa 10:5c69b067d88a 95 AkmSensor::Status Ak7401Ctrl::stopSensor(){
masahikofukasawa 10:5c69b067d88a 96 ticker.detach();
masahikofukasawa 10:5c69b067d88a 97 event = false;
masahikofukasawa 10:5c69b067d88a 98 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 99 }
masahikofukasawa 10:5c69b067d88a 100
masahikofukasawa 10:5c69b067d88a 101 AkmSensor::Status Ak7401Ctrl::readSensorData(Message* msg){
masahikofukasawa 10:5c69b067d88a 102 event = false;
masahikofukasawa 10:5c69b067d88a 103 char angle[2] = {0x00,0x00};
masahikofukasawa 10:5c69b067d88a 104 AK7401::Status status = ak7401->readAngle(angle);
masahikofukasawa 10:5c69b067d88a 105
masahikofukasawa 10:5c69b067d88a 106 msg->setCommand(Message::CMD_START_MEASUREMENT);
masahikofukasawa 10:5c69b067d88a 107 msg->setArgument( 0, status );
masahikofukasawa 10:5c69b067d88a 108 msg->setArgument( 1, angle[0] );
masahikofukasawa 10:5c69b067d88a 109 msg->setArgument( 2, angle[1] );
masahikofukasawa 10:5c69b067d88a 110
masahikofukasawa 10:5c69b067d88a 111 if( status != SUCCESS){
masahikofukasawa 10:5c69b067d88a 112 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 113 }
masahikofukasawa 10:5c69b067d88a 114 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 115 }
masahikofukasawa 10:5c69b067d88a 116
masahikofukasawa 10:5c69b067d88a 117 AkmSensor::Status Ak7401Ctrl::requestCommand(Message* in, Message* out){
masahikofukasawa 10:5c69b067d88a 118 AkmSensor::Status status = AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 119 Message::Command cmd = in->getCommand();
masahikofukasawa 10:5c69b067d88a 120 switch(cmd){
masahikofukasawa 10:5c69b067d88a 121 case Message::CMD_ANGLE_ZERO_RESET:
masahikofukasawa 10:5c69b067d88a 122 {
masahikofukasawa 14:21e177fc308a 123 if( ak7401->setAngleZero() != AK7401::SUCCESS ){
masahikofukasawa 14:21e177fc308a 124 MSG("#Error set angle zero\r\n");
masahikofukasawa 10:5c69b067d88a 125 status = AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 126 }
masahikofukasawa 10:5c69b067d88a 127 if( status == AkmSensor::ERROR ){
masahikofukasawa 10:5c69b067d88a 128 out->setArgument(0,1);
masahikofukasawa 10:5c69b067d88a 129 }else{
masahikofukasawa 10:5c69b067d88a 130 out->setArgument(0,0);
masahikofukasawa 10:5c69b067d88a 131 }
masahikofukasawa 10:5c69b067d88a 132 break;
masahikofukasawa 10:5c69b067d88a 133 }
masahikofukasawa 10:5c69b067d88a 134 default:
masahikofukasawa 10:5c69b067d88a 135 {
masahikofukasawa 11:cef8dc1cf010 136 MSG("#Error no command.\r\n");
masahikofukasawa 10:5c69b067d88a 137 status = AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 138 break;
masahikofukasawa 10:5c69b067d88a 139 }
masahikofukasawa 10:5c69b067d88a 140 }
masahikofukasawa 10:5c69b067d88a 141
masahikofukasawa 10:5c69b067d88a 142 return status;
masahikofukasawa 10:5c69b067d88a 143 }
masahikofukasawa 13:d008249f0359 144 char* Ak7401Ctrl::getSensorName(){
masahikofukasawa 13:d008249f0359 145 return sensorName;
masahikofukasawa 13:d008249f0359 146 }
masahikofukasawa 10:5c69b067d88a 147
masahikofukasawa 13:d008249f0359 148