...
 
Commits (4)
<!-- Chordata.xml -->
<Chordata version="0.1.0">
<Configuration>
<KC_revision> 1 </KC_revision>
<Communication>
<Adapter>
/dev/i2c-1
......@@ -41,13 +42,19 @@
</Configuration>
<Armature>
<Mux Name="main" id="0">
0x73
0x77
<Branch Name="left" id="1">
CH_1
<K_Ceptor Name="Unico" id="2">
4
1
<K_Ceptor Name="Unico" id="2">
1
<K_Ceptor Name="Unico" id="2">
1
</K_Ceptor>
</K_Ceptor>
</K_Ceptor>
</Branch>
</Mux>
</Armature>
</Chordata>
\ No newline at end of file
</Chordata>
......@@ -48,6 +48,12 @@
</xs:annotation>
</xs:element>
<xs:simpleType name="versionType">
<xs:restriction base="xs:normalizedString">
<xs:whiteSpace value='collapse'/>
<xs:pattern value="[0-9]{1,3}\.[0-9]{1,3}\.[0-9]{1,3}" />
</xs:restriction>
</xs:simpleType>
<xs:complexType name="root_type">
<xs:all>
......@@ -56,13 +62,35 @@
<xs:element type="ArmatureType" name="Armature"
minOccurs="1" maxOccurs="1"/>
</xs:all>
<xs:attribute type="xs:float" name="version"/>
<xs:attribute type="versionType" name="version"/>
</xs:complexType>
<!-- CONFIGURATION -->
<xs:simpleType name="CollapsedFloatType">
<xs:restriction base="xs:float">
<xs:whiteSpace value='collapse'/>
</xs:restriction>
</xs:simpleType>
<xs:simpleType name="CollapsedIntType">
<xs:restriction base="xs:integer">
<xs:whiteSpace value='collapse'/>
</xs:restriction>
</xs:simpleType>
<xs:simpleType name="revisionType">
<xs:restriction base="CollapsedIntType">
<xs:minInclusive value="1"/>
<xs:maxInclusive value="2"/>
</xs:restriction>
</xs:simpleType>
<xs:complexType name="ConfigurationType">
<xs:all>
<xs:element type="revisionType" name="KC_revision"
minOccurs="0" maxOccurs="1"/>
<xs:element type="CommunicationType" name="Communication"
minOccurs="0" maxOccurs="1"/>
......@@ -77,29 +105,16 @@
<xs:complexType name="FusionType">
<xs:all>
<xs:element type="FusionValueType" name="Beta_final"
<xs:element type="CollapsedFloatType" name="Beta_final"
minOccurs="0" maxOccurs="1"/>
<xs:element type="FusionValueType" name="Beta_start"
<xs:element type="CollapsedFloatType" name="Beta_start"
minOccurs="0" maxOccurs="1"/>
<xs:element type="FusionValueType" name="Time"
<xs:element type="CollapsedIntType" name="Time"
minOccurs="0" maxOccurs="1"/>
</xs:all>
</xs:complexType>
<xs:simpleType name="FusionValueType">
<xs:restriction base="xs:float">
<xs:whiteSpace value='collapse'/>
</xs:restriction>
</xs:simpleType>
<xs:simpleType name="FusionTimeType">
<xs:restriction base="xs:integer">
<xs:whiteSpace value='collapse'/>
</xs:restriction>
</xs:simpleType>
<xs:complexType name="OscType">
<xs:all>
<xs:element type="OscAddressType" name="Base"
......
This diff is collapsed.
<!-- Chordata.xml -->
<Chordata version="0.1.0">
<Configuration>
<KC_revision> 1 </KC_revision>
<Communication>
<Adapter>
/dev/i2c-1
</Adapter>
<Ip>
192.168.85.3
</Ip>
<Port>
6565
</Port>
<Log>
stdout,file
</Log>
<Transmit>
osc
</Transmit>
<Verbosity>
0
</Verbosity>
</Communication>
<Osc>
<Base>
/Chordata
</Base>
</Osc>
<Fusion>
<Beta_start>
1.2
</Beta_start>
<Beta_final>
0.2
</Beta_final>
<Time>
10000
</Time>
</Fusion>
</Configuration>
<Armature>
<Mux Name="main" id="0">
0x77
<!--OFF -->
<Branch Name="off" id="0">
OFF
<Mux Name="sec" id="0">
0x71
<Branch Name="off-branch" id="0">
OFF
</Branch>
<Branch Name="l-leg-branch" id="0">
CH_2
<K_Ceptor Name="l-thigh" id="0">
0
<K_Ceptor Name="l-leg" id="0">
2
</K_Ceptor>
</K_Ceptor>
</Branch>
<Branch Name="r-leg-branch" id="0">
CH_3
<K_Ceptor Name="r-thigh" id="0">
0
<K_Ceptor Name="r-leg" id="0">
2
</K_Ceptor>
</K_Ceptor>
</Branch>
</Mux>
</Branch>
<!-- BASE BRANCH -->
<Branch Name="base-branch" id="0">
CH_4
<K_Ceptor Name="base" id="0">
0
<K_Ceptor Name="dorsal" id="0">
1
<K_Ceptor Name="head" id="0">
2
</K_Ceptor>
</K_Ceptor>
</K_Ceptor>
</Branch>
<!-- LEFT BRANCH -->
<Branch Name="left" id="0">
CH_1
<K_Ceptor Name="l-arm" id="0">
0
<K_Ceptor Name="l-forarm" id="0">
1
<K_Ceptor Name="l-hand" id="0">
2
</K_Ceptor>
</K_Ceptor>
</K_Ceptor>
</Branch>
<!-- RIGHT BRANCH -->
<Branch Name="right" id="0">
CH_2
<K_Ceptor Name="r-arm" id="0">
0
<K_Ceptor Name="r-forarm" id="0">
1
<K_Ceptor Name="r-hand" id="0">
2
</K_Ceptor>
</K_Ceptor>
</K_Ceptor>
</Branch>
</Mux>
</Armature>
</Chordata>
//
// Copyright(c) 2016-2018 Gabi Melman.
// Distributed under the MIT License (http://opensource.org/licenses/MIT)
//
#pragma once
//
// Include a bundled header-only copy of fmtlib or an external one.
// By default spdlog include its own copy.
//
#if !defined(SPDLOG_FMT_EXTERNAL)
#ifndef FMT_HEADER_ONLY
#define FMT_HEADER_ONLY
#endif
#ifndef FMT_USE_WINDOWS_H
#define FMT_USE_WINDOWS_H 0
#endif
#include "bundled/core.h"
#include "bundled/format.h"
#else // external fmtlib
#include <fmt/core.h>
#include <fmt/format.h>
#endif
\ No newline at end of file
......@@ -61,7 +61,21 @@
/////////////////////////////////
///The version of the program
/////////////////////////////////
#define _CHORDATA_VERSION 0.1f
#define _CHORDATA_VERSION_MAJOR 0
#define _CHORDATA_VERSION_MINOR 1
#define _CHORDATA_VERSION_PATCH 0
#define _CHORDATA_VERSION (( _CHORDATA_VERSION_MAJOR * 10000 ) +\
( _CHORDATA_VERSION_MINOR * 100 ) +\
( _CHORDATA_VERSION_PATCH ) )
#define _CHORDATA_VERSION_STR fmt::format("{}.{}.{}",\
_CHORDATA_VERSION_MAJOR,\
_CHORDATA_VERSION_MINOR,\
_CHORDATA_VERSION_PATCH)
//this is the string "chordata", used for checking presense of data in EEPROM
#define _CHORDATA_CRC32 0xCBF43926
//////////////////////////////////
/// The name of the client (where the render is performed and the animation recorded)
......@@ -205,6 +219,11 @@
#define _CHORDATA_ODR 50
//////////////////////////////////
///The default version of K-Ceptor used
//////////////////////////////////
#define _KC_REV 2
//////////////////////////////////
/// The defaule Beta parameter of the Madgwick sensor fusion algorithm
//////////////////////////////////
......@@ -222,17 +241,18 @@
//////////////////////////////////
/// The maximun number of ports allowed by default on a Multiplexer
//////////////////////////////////
#define _CHORDATA_MUX_MAX 4
#define _CHORDATA_MUX_MAX 6
//////////////////////////////////
/// The starting chunk of the gyro calib data stored on the EEPROM
/// The starting addrs of the chunks of calib data stored on the EEPROM
//////////////////////////////////
#define _CHORDATA_EEPROM_GYRO_OFFSET 8
#define _CHORDATA_EEPROM_VAL_CHKSUM 0
#define _CHORDATA_EEPROM_VERSION 8
#define _CHORDATA_EEPROM_TIMESTAMP 12
#define _CHORDATA_EEPROM_GYRO_SPACE 16
#define _CHORDATA_EEPROM_ACEL_SPACE 24
#define _CHORDATA_EEPROM_MAG_SPACE 32
//////////////////////////////////
/// The starting chunk of the acel calib data stored on the EEPROM
//////////////////////////////////
#define _CHORDATA_EEPROM_ACEL_OFFSET 16
///@}
/*:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::*/
......@@ -278,7 +298,16 @@ namespace Chordata {
};
/**
* A Struct that contains all the configuration data.
* Utility template for gettinh size of array on compile time.
* From Efficient Modern c++ by S. Meyers
*/
template<typename T, std::size_t N>
constexpr std::size_t arraySize(T (&)[N]) noexcept {
return N;
}
/**
* Possible outputs fot data and logging.
*/
enum Output_Redirect{
NONE = 0,
......@@ -298,11 +327,6 @@ namespace Chordata {
return a= static_cast<Output_Redirect>(static_cast<char>(a) | static_cast<char>(b));
}
template<typename T, std::size_t N>
constexpr std::size_t arraySize(T (&)[N]) noexcept //From E. Modern c++ by S. Meyers
{
return N;
}
static const char* _Output_Redirect_keywords[] =
{"none","stdout", "stderr", "file", "osc"};
......@@ -336,12 +360,19 @@ namespace Chordata {
static const char invalid_i2c_addr_n = arraySize(invalid_i2c_addr);
/**
* A Struct that contains all the configuration data.
*/
struct Configuration_Data
{
Configuration_Data(); //defined in Chordata_utils.cpp
std::string exe_path;
int hardware_concurrency;
bool wait;
bool raw;
int kc_ver;
int empirical_add;
struct Program_State{
mutable char beta_timer_init;
......@@ -354,14 +385,13 @@ namespace Chordata {
uint16_t time;
} fusion;
Configuration_Data(); //defined in Chordata_utils.cpp
void append_path();
struct Conf_info{
std::string description;
std::string notes;
float version;
uint32_t version;
} info;
struct Conf_XML{
......@@ -415,6 +445,8 @@ namespace Chordata {
static const char *wait = "Disable confirmation to start the program. By default Notochord will wait for a confirmation from the user.";
static const char *kc_ver = "The revision of the K-Ceptor hardware used.";
static const char *xml_g = "\nCONFIGURATION FILES:";
static const char *xml_filename = "The path to the configuration file. Default [./Chordata.xml]";
......
......@@ -149,29 +149,31 @@ void KC::bang(){
return;
}
try{
auto eeprom = read_calib(i2c, this->getAddress());
static const std::array<int16_t,3> niente = {0,0,0};
static const std::array<int16_t,3> tutti = {0,0,0};
uint8_t eeprom_addr = (Chordata::getConf().kc_ver == 1)? R1_EEPROM_ADDR : R2_EEPROM_ADDR;
eeprom_addr = this->getAddress() ^ eeprom_addr;
if (eeprom == niente || eeprom == tutti){
auto validation = read_info(i2c, eeprom_addr, _CHORDATA_EEPROM_VAL_CHKSUM);
if (validation != _CHORDATA_CRC32){
comm::try_debug("K_Ceptor {}: No EEPROM calib data found.", getLabel());
return;
}
magOffset[0] = eeprom[0];
auto eeprom = read_calib(i2c, eeprom_addr, _CHORDATA_EEPROM_MAG_SPACE);
magOffset[0] = eeprom[0];//TODO; use std::copy here
magOffset[1] = eeprom[1];
magOffset[2] = eeprom[2];
comm::try_debug("K_Ceptor {}: EEPROM calib data found :\n\t{}\n\t{}\n\t{}",
comm::try_debug("K_Ceptor {}: EEPROM calib data found. Mag correction vector =\n\t{}\t{}\t{}",
getLabel(),
eeprom[0],eeprom[1],eeprom[2]);
//get gyro offsets
eeprom = read_calib(i2c, this->getAddress(), _CHORDATA_EEPROM_GYRO_OFFSET);
eeprom = read_calib(i2c, eeprom_addr, _CHORDATA_EEPROM_GYRO_SPACE);
std::copy(std::begin(eeprom), std::end(eeprom), std::begin(imu->gBiasRaw));
//get acel offsets
eeprom = read_calib(i2c, this->getAddress(), _CHORDATA_EEPROM_ACEL_OFFSET);
eeprom = read_calib(i2c, eeprom_addr, _CHORDATA_EEPROM_ACEL_SPACE);
std::copy(std::begin(eeprom), std::end(eeprom), std::begin(imu->aBiasRaw));
......@@ -179,7 +181,7 @@ void KC::bang(){
comm::try_warn("EEPROM not found on {}", getLabel());
} //end try to read EEPROM
return; //setup and EEPROM read done. h
return; //setup and EEPROM read done
} else {
comm::try_err("K_Ceptor {}: setup ERROR!.", getLabel());
......@@ -214,49 +216,54 @@ void KC::bang(){
//TODO: atention, this may throw!!
//Check for errors!!
try{
imu->readGyro();
imu->readAccel();
imu->readMag();
//TODO: put this in another function, and choose btw them on startup
if (Chordata::getConf().raw){
Chordata::mag_lectures.push_back({imu->mx, imu->my, imu->mz});
comm::transmit(fmt::format("{},{:d},{:d},{:d},{:d},{:d},{:d},{:d},{:d},{:d}",
osc_addr, imu->gx, imu->gy, imu->gz, imu->ax, imu->ay, imu->az, imu->mx, imu->my, imu->mz));
return;
}
imu->readGyro();
imu->readAccel();
imu->readMag();
//TODO: put this in another function, and choose btw them on startup
if (Chordata::getConf().raw){
Chordata::mag_lectures.push_back({imu->mx, imu->my, imu->mz});
comm::transmit(fmt::format("{},{:d},{:d},{:d},{:d},{:d},{:d},{:d},{:d},{:d}",
osc_addr, imu->gx, imu->gy, imu->gz, imu->ax, imu->ay, imu->az, imu->mx, imu->my, imu->mz));
return;
//Correct magnetometer reads from callibration data
imu->mx += magOffset[0];
imu->my += magOffset[1];
imu->mz += magOffset[2];
//Converting Mixed axis convention on LSM9DS1 to Madgwick Right handed convention
//see: https://www.lythaniel.fr/index.php/2016/08/20/lsm9ds1-madgwicks-ahr-filter-and-robot-orientation/
//
//TODO: Right now the LSM9DS1 adaptation matrix is implemented. Also use the LSM9DS0 when neccesary
//
// LSM9DS0:
//
// gx gy gz
// ax ay az
// mx my -mz
//
// LSM9DS1:
//
// gy gx gz
// ay ax az
// my -mx mz
//
Quaternion q = MadgwickAHRSupdate(
degToRads(imu->calcGyro(imu->gy)), degToRads(imu->calcGyro(imu->gx)), degToRads(imu->calcGyro(imu->gz)),
imu->calcAccel(imu->ay), imu->calcAccel(imu->ax), imu->calcAccel(imu->az),
imu->calcMag(imu->my), -imu->calcMag(imu->mx), imu->calcMag(imu->mz)
);
comm::trace("Transmit {}", getLabel());
comm::transmit(this, move(q));
} catch (const Chordata::IO_Error& e) {
comm::err("Error reading {}", getLabel());
}
//Correct magnetometer reads from callibration data
imu->mx += magOffset[0];
imu->my += magOffset[1];
imu->mz += magOffset[2];
//Converting Mixed axis convention on LSM9DS1 to Madgwick Right handed convention
//see: https://www.lythaniel.fr/index.php/2016/08/20/lsm9ds1-madgwicks-ahr-filter-and-robot-orientation/
//
//TODO: Right now the LSM9DS1 adaptation matrix is implemented. Also use the LSM9DS0 when neccesary
//
// LSM9DS0:
//
// gx gy gz
// ax ay az
// mx my -mz
//
// LSM9DS1:
//
// gy gx gz
// ay ax az
// my -mx mz
//
Quaternion q = MadgwickAHRSupdate(
degToRads(imu->calcGyro(imu->gy)), degToRads(imu->calcGyro(imu->gx)), degToRads(imu->calcGyro(imu->gz)),
imu->calcAccel(imu->ay), imu->calcAccel(imu->ax), imu->calcAccel(imu->az),
imu->calcMag(imu->my), -imu->calcMag(imu->mx), imu->calcMag(imu->mz)
);
comm::trace("Transmit {}", getLabel());
comm::transmit(this, move(q));
}
void Branch::bang(){
......
......@@ -54,7 +54,9 @@
#include "MadgwickAHRS.h"
#include "../src/EEPROM.h" //This can't be included anywhere else :P
#include "EEPROM.h" //This can't be included anywhere else :P
// belongs to some parts of this programs that were developed on the rush.
// It shoudl be completely refactored one day
#include <cstdint>
......
......@@ -127,7 +127,8 @@ Configuration_Data Chordata::parse_cmd_line(int argc, const char **argv){
args::Flag version(parser, "version", msgs::version, {'V', "version"});
args::Flag wait(parser, "no_wait", msgs::wait, {'y', "yes"});
args::Flag raw(parser, "raw_lectures", msgs::wait, {'r', "raw"});
args::ValueFlag<int> kc_ver(parser, "K_Ceptor revision", msgs::kc_ver, {'k', "kceptor"});
args::ValueFlag<int> empirical_add(parser, "Empirical ADD", "temporary parameter", {'E', "empirical_add"});
args::Group xml(parser, msgs::xml_g, args::Group::Validators::DontCare);
......@@ -171,15 +172,25 @@ Configuration_Data Chordata::parse_cmd_line(int argc, const char **argv){
}
using args::get;
if (version){
args::Error e("");
throw CMDLine_Parse_Error(format(msgs::version_dump, _CHORDATA_VERSION), e);
throw CMDLine_Parse_Error(format(msgs::version_dump, _CHORDATA_VERSION_STR), e);
}
conf.wait = (wait)? false : true;
conf.raw = (raw)? true : false;
using args::get;
conf.empirical_add = (empirical_add)? get(empirical_add) : 0;
if (kc_ver){
args::Error e("");
throw CMDLine_Parse_Error("At the moment the K-Ceptor version can only be set on the XML", e);
}
// conf.kc_ver = (kc_ver)? get(kc_ver) : _KC_REV;
// if ( conf.kc_ver != 1 && conf.kc_ver != 2 ) {
// args::Error e("");
// throw CMDLine_Parse_Error("Invalid K-Ceptor revision argument passed, only revisions 1 and 2 are valid.", e);
// }
if (xml_filename) conf.xml.filename = get(xml_filename);
if (xml_schema) conf.xml.schema = get(xml_schema);
......@@ -261,16 +272,15 @@ XML_Parser::XML_Parser(Configuration_Data & conf):
init_procedure();
const XMLElement *theNode;
const XMLElement *conf_node = getBaseNode()
->FirstChildElement("Configuration");
const XMLElement *comm_node = getBaseNode()
->FirstChildElement("Configuration")
const XMLElement *comm_node = conf_node
->FirstChildElement("Communication");
const XMLElement *osc_node = getBaseNode()
->FirstChildElement("Configuration")
const XMLElement *osc_node = conf_node
->FirstChildElement("Osc");
const XMLElement *fusion_node = getBaseNode()
->FirstChildElement("Configuration")
const XMLElement *fusion_node = conf_node
->FirstChildElement("Fusion");
auto get = [](const XMLElement* e, const char* t){
......@@ -306,6 +316,12 @@ XML_Parser::XML_Parser(Configuration_Data & conf):
}
};
//parse <Configuration> node
theNode = getBaseNode()->FirstChildElement("Configuration");
string temp;
putInConf(temp, "KC_revision");
conf.kc_ver = stoi(temp);
//parse <Communication> node
theNode = comm_node;
......@@ -318,7 +334,7 @@ XML_Parser::XML_Parser(Configuration_Data & conf):
if (are_the_same(conf.comm.filename , default_conf.comm.filename))
putInConf(conf.comm.filename, "Filename"); //** conf.comm.filename **
string temp;
temp;
if (are_the_same(conf.comm.port , default_conf.comm.port)){
putInConf(temp, "Port"); //** conf.comm.port **
conf.comm.port = stoi(temp);
......
......@@ -55,10 +55,11 @@ namespace comm = Chordata::Communicator;
using namespace std;
using namespace Chordata;
using scheduler_citer = std::vector<I2C_Node*>::const_iterator;
#define LAP_MODIF_RATE 10
#define LAP_TOLERANCE 100
#define LAP_CORRECT_TIME_MARGIN 20*1000
#define LAP_CORRECT_TIME_MARGIN 10*1000
void Node_Scheluder::addNode(I2C_Node *n){
nodelist.push_back(n);
......@@ -69,6 +70,12 @@ void Node_Scheluder::addNode(I2C_Node *n){
};
const scheduler_citer seek_first_sensor(const std::vector<I2C_Node*>& list){
for (scheduler_citer i = list.begin(); i != list.end(); ++i){
if ((*i)->isSensor()) return i;
}
}
long int Node_Scheluder::bang(long int wait){
//not threaded at the moment
......@@ -77,13 +84,19 @@ long int Node_Scheluder::bang(long int wait){
static long int lap_teoric_duration = 1000000 / _CHORDATA_ODR;
static long int teoric_wait = lap_teoric_duration / getODRModifiers();
if (current == nodelist.begin()+1){
static auto first_sensor = seek_first_sensor(nodelist);
if (current == first_sensor){
//Check if lap duration is correct and adjust;
auto lap = comm::timekeeper.ellapsedLapMicros();
comm::trace(" ===== Ellapsed Lap Micros: {} =====", lap);
comm::timekeeper.resetLap();
if (comm::timekeeper.ellapsedMillis() > LAP_CORRECT_TIME_MARGIN ){
//Leave the first second of execution untouched
//after that start cheking if the lap has to be increased or decreased
if (lap > lap_teoric_duration + LAP_TOLERANCE){
wait -= LAP_MODIF_RATE;
comm::debug(" (- ) Read wait reduced, now is {} (teoric = {}, [Lap measured: {}, Lap teoric: {}])",
......@@ -94,12 +107,11 @@ long int Node_Scheluder::bang(long int wait){
comm::debug(" ( +) Read wait increased, now is {} (teoric = {}, [Lap measured: {}, Lap teoric: {}])",
wait, teoric_wait,
lap, lap_teoric_duration);
}
}
}
} // End lap duration cheking
doSearch();
return wait;
}
......@@ -112,7 +124,7 @@ void Node_Scheluder::doSearch(){
// mp->unlock();
step();
if (!(*current)->isSensor()){
while (!(*current)->isSensor()){
main_buffer.put(*current);
step();
}
......
......@@ -79,6 +79,7 @@ Chordata::Configuration_Data::Configuration_Data(): //from Chordata_def.h
hardware_concurrency(std::thread::hardware_concurrency()),
wait(true),
raw(false),
kc_ver(_KC_REV),
state({.beta_timer_init = 0, .imu_setup_complete = false})
{
......
......@@ -165,6 +165,10 @@ namespace Chordata{
time_point getStart() const {return start;};
inline const std::time_t initialTimestamp() const{
return std::chrono::system_clock::to_time_t(start);
}
inline const char *initialTime() const{
const std::time_t time = std::chrono::system_clock::to_time_t(start);
return std::ctime(&time);
......
......@@ -3,6 +3,9 @@
* Declarations of functions to perform read/write operations on an EEPROM memory.
* Definitions in Notochord.cpp
*
* // This file belongs to some parts of this programs that were developed on the rush.
* // It should be completely refactored one day
*
* @author Bruno Laurencich
* @version 0.1.0
* @date 2018/08/20
......@@ -51,9 +54,18 @@
#include "i2c_manager.h"
#include "Chordata_utils.h"
#define EEPROM_ADDR 0x51
#define R1_EEPROM_ADDR 0x50
#define R2_EEPROM_ADDR 0x51
void write_calib(I2C_io* io, uint16_t address, std::array<int16_t, 3> vals, const int offset = 0);
std::array<int16_t, 3> read_calib(I2C_io* io, uint16_t address, const int offset = 0);
void write_info(I2C_io* io, uint16_t address, uint32_t val, const int offset = 0);
uint32_t read_info(I2C_io* io, uint16_t address, const int offset = 0);
//TODO: set date on write!
bool write_calib(I2C_io* io, uint16_t xor_val, std::array<int16_t, 3> vals, const int offset = 0);
std::array<int16_t, 3> read_calib(I2C_io* io, uint16_t xor_val, const int offset = 0);
......@@ -88,34 +88,32 @@ config_fn Chordata::getConf = []()-> const Chordata::Configuration_Data& {
//////////////////////////////////
#define EEPROM_RW_SLEEP 1
bool write_calib(I2C_io* io, uint16_t xor_val, std::array<int16_t, 3> vals, int offset){
void write_calib(I2C_io* io, uint16_t address, std::array<int16_t, 3> vals, int offset){
// cout << "WRITING EEPROM OFFSET: " << offset << endl;
for (int i = 0; i < 6; i+=2)
{
int16_t val = vals[i/2];
int8_t msb = (val & 0xff00) >> 8;
io->writeByte( EEPROM_ADDR ^ xor_val, offset+i+0 , msb );
io->writeByte( address, offset+i+0 , msb );
thread_sleep(Chordata::millis(EEPROM_RW_SLEEP));
int8_t lsb = (val & 0x00ff);
io->writeByte( EEPROM_ADDR ^ xor_val, offset+i+1 , lsb );
io->writeByte( address, offset+i+1 , lsb );
thread_sleep(Chordata::millis(EEPROM_RW_SLEEP));
// printf("SLOT %d [ MSB:%2x LSB:%2x - VAL: %4x / %d]\n", i , msb, lsb, val, val);
}
return true;
}
std::array<int16_t, 3> read_calib(I2C_io* io, uint16_t xor_val, int offset){
std::array<int16_t, 3> read_calib(I2C_io* io, uint16_t address, int offset){
std::array<int16_t, 3> results;
for (int i = 0; i < 6; i+=2)
{
int8_t msb = io->readByte( EEPROM_ADDR ^ xor_val, offset+i+0 ) ;
int8_t msb = io->readByte( address, offset+i+0 ) ;
thread_sleep(Chordata::millis(EEPROM_RW_SLEEP));
int8_t lsb = io->readByte( EEPROM_ADDR ^ xor_val, offset+i+1 ) ;
int8_t lsb = io->readByte( address, offset+i+1 ) ;
thread_sleep(Chordata::millis(EEPROM_RW_SLEEP));
results[i/2] = (msb << 8) | (lsb & 0x00ff);
......@@ -126,6 +124,40 @@ std::array<int16_t, 3> read_calib(I2C_io* io, uint16_t xor_val, int offset){
return results;
}
void write_info(I2C_io* io, uint16_t address, uint32_t val, const int offset){
static const std::size_t N = 4;
uint8_t chunks[N] = {
(val & 0xff000000UL) >> 24,
(val & 0x00ff0000UL) >> 16,
(val & 0x0000ff00UL) >> 8,
(val & 0x000000ffUL)
};
for (std::size_t i = 0; i < N; ++i){
io->writeByte( address, offset+i , chunks[i] );
thread_sleep(Chordata::millis(EEPROM_RW_SLEEP));
}
}
uint32_t read_info(I2C_io* io, uint16_t address, const int offset){
static const std::size_t N = 4;
uint8_t chunks[N];
for (std::size_t i = 0; i < N; ++i){
chunks[i] = io->readByte( address, offset+i );
thread_sleep(Chordata::millis(EEPROM_RW_SLEEP));
}
uint32_t result = 0x00000000UL;
result |= ( chunks[0] << 24 ) & 0xff000000UL;
result |= ( chunks[1] << 16 ) & 0x00ff0000UL;
result |= ( chunks[2] << 8 ) & 0x0000ff00UL;
result |= chunks[3] & 0x000000ffUL;
return result;
}
//////////////////////////////////
......@@ -236,14 +268,14 @@ bool Notochord::run(){
int user_xor_val;
string user_label;
// The +2 is here because it was observed on the oscilloscope that there were 2 extra delay periods
// Presumably when the gates go to K_Ceptor_offset_calib
// This is not used when in raw mode, which at the moment is used only for collecting data for callibration
//TOFIX:change the way this is implemented!
int empirical_add = (config.raw)? 0 : 2;
//It was observed on the oscilloscope that there were some extra delays
//so the "config.empirical_add" value was introduced to virually change the ODRModifiers
//on runtime.
//Perhaps this is due to the nested multiplexer hack used on the R1 prototype.
//TODO:why is this happening?!
long sleepMicros = (1000*1000) /
((scheluder.getODRModifiers() + empirical_add) * _CHORDATA_ODR );
((scheluder.getODRModifiers() + config.empirical_add) * _CHORDATA_ODR );
const int notODRModifiers = scheluder.length() - scheluder.getODRModifiers();
if (notODRModifiers < 1){
......@@ -265,9 +297,9 @@ bool Notochord::run(){
std::thread t;
if (config.raw){ //if the raw flag is set, wait for an user imput to stop the timer
std::cout << "**Calibration mode**\n Enter the xor value of the node\n" << std::endl;
std::cout << "**Calibration mode**\n Enter the xor value of the node\n"_CHORDATA_DEF_PROMPT << std::endl;
std::cin >> user_xor_val;
std::cout << "Enter the label of the node" << std::endl;
std::cout << "Enter the label of the node\n"_CHORDATA_DEF_PROMPT << std::endl;
std::cin >> user_label;
std::cout << "Initilizing callibration procedure,\n1. Put the KCEPTOR facing up in a flat surface." << std::endl;
......@@ -290,10 +322,10 @@ bool Notochord::run(){
bool started = main_timer.startTimer(comm::timekeeper.getStart(), sleepMicros);
std::array<int16_t, 3> result;
std::array<int16_t, 3> mag_offsets;
try{ //If the timer was started, run the octave interpreter
result = runOctave(mag_lectures);
mag_offsets = runOctave(mag_lectures);
// comm::info("Results from the calibration: \nMAG OFFSETS:\t{}\t{}\t{}"
// "\nACEL OFFSETS:\t{}\t{}\t{}\nGYRO OFFSETS:\t{}\t{}\t{}",
......@@ -302,29 +334,37 @@ bool Notochord::run(){
// acel_offsets[0],acel_offsets[1],acel_offsets[2]);
comm::info("Results from the calibration for [{}]({}):\n{}\n{}\n{}",
user_label, user_xor_val,
OFFSET_TAG("mag", result),
OFFSET_TAG("mag", mag_offsets),
OFFSET_TAG("acel", acel_offsets),
OFFSET_TAG("gyro", gyro_offsets));
comm::info("Writting to EEPROM, xor: {}", Chordata::K_Ceptor_addr);
write_calib(i2c, Chordata::K_Ceptor_addr, result, 0);
write_calib(i2c, Chordata::K_Ceptor_addr, gyro_offsets, _CHORDATA_EEPROM_GYRO_OFFSET);
write_calib(i2c, Chordata::K_Ceptor_addr, acel_offsets, _CHORDATA_EEPROM_ACEL_OFFSET);
uint8_t eeprom_addr = (Chordata::getConf().kc_ver == 1)? R1_EEPROM_ADDR : R2_EEPROM_ADDR;
eeprom_addr = eeprom_addr ^ Chordata::K_Ceptor_addr;
write_info(i2c, eeprom_addr, _CHORDATA_VERSION, _CHORDATA_EEPROM_VERSION);
write_info(i2c, eeprom_addr, comm::timekeeper.initialTimestamp(), _CHORDATA_EEPROM_TIMESTAMP);
write_calib(i2c, eeprom_addr, mag_offsets, _CHORDATA_EEPROM_MAG_SPACE);
write_calib(i2c, eeprom_addr, gyro_offsets, _CHORDATA_EEPROM_GYRO_SPACE);
write_calib(i2c, eeprom_addr, acel_offsets, _CHORDATA_EEPROM_ACEL_SPACE);
write_info(i2c, eeprom_addr, _CHORDATA_CRC32, _CHORDATA_EEPROM_VAL_CHKSUM);
auto eeprom = read_calib(i2c, Chordata::K_Ceptor_addr);
auto eeprom = read_calib(i2c, eeprom_addr, _CHORDATA_EEPROM_MAG_SPACE);
if (eeprom != result)
if (eeprom != mag_offsets)
comm::err("The readed values (MAG) from the eeprom are different than those written:\n\t{}\n\t{}\n\t{}",
eeprom[0],eeprom[1],eeprom[2]);
eeprom = read_calib(i2c, Chordata::K_Ceptor_addr, _CHORDATA_EEPROM_GYRO_OFFSET);
eeprom = read_calib(i2c, eeprom_addr, _CHORDATA_EEPROM_GYRO_SPACE);
if (eeprom != gyro_offsets)
comm::err("The readed values (GYRO) from the eeprom are different than those written:\n\t{}\n\t{}\n\t{}",
eeprom[0],eeprom[1],eeprom[2]);
eeprom = read_calib(i2c, Chordata::K_Ceptor_addr, _CHORDATA_EEPROM_ACEL_OFFSET);
eeprom = read_calib(i2c, eeprom_addr, _CHORDATA_EEPROM_ACEL_SPACE);
if (eeprom != acel_offsets)
comm::err("The readed values (ACEL) from the eeprom are different than those written:\n\t{}\n\t{}\n\t{}",
eeprom[0],eeprom[1],eeprom[2]);
......@@ -339,7 +379,7 @@ bool Notochord::run(){
myfile.open (filename, std::ios_base::app);
myfile << "<!-- Value written on " << comm::timekeeper.now() << " -->\n";
myfile << "<K_Ceptor Name=" << user_label <<" id=\"0\">\n\t" << user_xor_val << "\n";
myfile << OFFSET_TAG("mag", result) << '\n';
myfile << OFFSET_TAG("mag", mag_offsets) << '\n';
myfile << OFFSET_TAG("acel", acel_offsets) << '\n';
myfile << OFFSET_TAG("gyro", gyro_offsets) << '\n';
myfile << "</K_Ceptor>";
......
......@@ -208,7 +208,7 @@ SCENARIO("The Command_Line_Parser parses the arguments "
THEN("It should fill the configuration struct with the default, and the user data "){
CHECK_THAT(config.info.description, Equals(_NOTOCHORD_DESCRIPTION));
CHECK_THAT(config.info.notes, Equals(_NOTOCHORD_NOTES));
CHECK(config.info.version == _CHORDATA_VERSION );
CHECK(config.info.version == _CHORDATA_VERSION_STR );
CHECK_THAT(config.xml.filename, Contains(config.exe_path) && Contains(_CHORDATA_CONF));
CHECK_THAT(config.xml.schema, Contains(config.exe_path) && Contains(_CHORDATA_CONF_SCHEMA));
......@@ -229,7 +229,7 @@ SCENARIO("The Command_Line_Parser parses the arguments "
THEN("It should fill the configuration struct with the default data"){
REQUIRE_THAT(config.info.description, Equals(_NOTOCHORD_DESCRIPTION));
REQUIRE_THAT(config.info.notes, Equals(_NOTOCHORD_NOTES));
REQUIRE(config.info.version == _CHORDATA_VERSION );
REQUIRE(config.info.version == _CHORDATA_VERSION_STR );
REQUIRE_THAT(config.comm.ip, Equals(_CHORDATA_TRANSMIT_ADDR));
REQUIRE(config.comm.port == _CHORDATA_TRANSMIT_PORT );
......