sensor.cpp

00001 /*
00002  * Author    : Garudago KRAI ITB 2019
00003  * Developer : Calmantara Sumpono Putra 
00004  * Version   : 1.0.0 
00005  */
00006 
00007 #include "mbed.h"
00008 #include "sensor.h"
00009 #include <garudago_conf/config.h>
00010 
00011 
00012 Compass::Compass(){
00013     // Constructor
00014 }
00015 
00016 void Compass::compass_reset(float _value){
00017     _offset_compass_value = _value;
00018 }
00019 
00020 void Compass::compass_update(float _value){
00021     _theta_origin = _value;
00022     _theta_offset = _value - _offset_compass_value;
00023 }
00024 
00025 float Compass::compass_value(){
00026     float theta_transformed;
00027 
00028     if(_theta_offset > 180.0 && _theta_offset <= 360.0)
00029         theta_transformed = (_theta_origin - 360.0 - _offset_compass_value)/-RAD_TO_DEG;
00030     else if(_theta_offset < -180.0 && _theta_offset >= -360.0)
00031         theta_transformed = (_theta_origin  + 360.0 - _offset_compass_value)/-RAD_TO_DEG;
00032     else
00033         theta_transformed = (_theta_origin - _offset_compass_value)/-RAD_TO_DEG;  
00034 
00035     return theta_transformed; 
00036 }