Commit 6d6a62b4 authored by Bruno Laurencich's avatar Bruno Laurencich

add --odr as experimental feature

parent 09fdef05
......@@ -72,7 +72,7 @@ public:
// If IMU_MODE_SPI, this is the chip select pin of the gyro (CS_AG)
// - mAddr = If IMU_MODE_I2C, this is the I2C address of the magnetometer.
// If IMU_MODE_SPI, this is the cs pin of the magnetometer (CS_M)
LSM9DS1(IO*, uint8_t _xor ,
LSM9DS1(IO*, uint8_t _xor , uint16_t odr,
uint8_t xgAddr = LSM9DS1_AG_ADDR(1),
uint8_t mAddr = LSM9DS1_M_ADDR(1));
......@@ -350,7 +350,7 @@ protected:
bool _autoCalc;
// init() -- Sets up gyro, accel, and mag settings to default.
void init(/*interface_mode interface, uint8_t xgAddr, uint8_t mAddr*/);
void init(uint16_t);
// initGyro() -- Sets up the gyroscope to begin reading.
// This function steps through all five gyroscope control registers.
......
......@@ -46,17 +46,17 @@ LSM9DS1<IO>* make_LSM9DS1(IO *io, uint8_t _xor){
template <typename IO>
LSM9DS1<IO>::LSM9DS1(IO * _io, uint8_t _xor, uint8_t xgAddr, uint8_t mAddr):
LSM9DS1<IO>::LSM9DS1(IO * _io, uint8_t _xor, uint16_t _odr, uint8_t xgAddr, uint8_t mAddr):
Chordata::I_Imu<IO>(_io),
xor_addr(_xor),
xg_addr( xgAddr ^ _xor ),
m_addr( mAddr ^ _xor )
{
init(/*interface, xgAddr, mAddr*/);
init(_odr);
}
template <typename IO>
void LSM9DS1<IO>::init(/*interface_mode interface, uint8_t xgAddr, uint8_t mAddr*/)
void LSM9DS1<IO>::init(uint16_t odr)
{
settings.device.commInterface = IMU_MODE_I2C;
settings.device.agAddress = xg_addr;
......@@ -72,7 +72,7 @@ void LSM9DS1<IO>::init(/*interface_mode interface, uint8_t xgAddr, uint8_t mAddr
// 1 = 14.9 4 = 238
// 2 = 59.5 5 = 476
// 3 = 119 6 = 952
settings.gyro.sampleRate = 3;
settings.gyro.sampleRate = (odr >= 60)? 3 : 2;
// gyro cutoff frequency: value between 0-3
// Actual value of cutoff frequency depends
// on sample rate.
......@@ -99,7 +99,7 @@ void LSM9DS1<IO>::init(/*interface_mode interface, uint8_t xgAddr, uint8_t mAddr
// 1 = 10 Hz 4 = 238 Hz
// 2 = 50 Hz 5 = 476 Hz
// 3 = 119 Hz 6 = 952 Hz
settings.accel.sampleRate = 3;
settings.accel.sampleRate = (odr > 50)? 3 : 2;
// Accel cutoff freqeuncy can be any value between -1 - 3.
// -1 = bandwidth determined by sample rate
// 0 = 408 Hz 2 = 105 Hz
......
......@@ -213,7 +213,7 @@
//////////////////////////////////
///The default output data rate (for each sensor)
//////////////////////////////////
#define _CHORDATA_ODR 100
#define _CHORDATA_ODR 50
//////////////////////////////////
......@@ -396,6 +396,7 @@ namespace Chordata {
bool scan;
int kc_ver;
int empirical_add;
int odr;
struct Program_State{
mutable char beta_timer_init;
......
......@@ -139,9 +139,9 @@ K_Ceptor::K_Ceptor( Link *_parent,
uint8_t _address, std::string _label,
I2C_io *_i2c):
I2C_Node(_i2c, _address, _label, _parent),
imu( new _CHORDATA_IMU_TYPE_(_i2c, _address )),
imu( new _CHORDATA_IMU_TYPE_(_i2c, _address, Chordata::getConf().odr )),
osc_addr(fmt::format("{}/{}", base_osc_addr, _label)),
sensorFusion(),
sensorFusion(Chordata::getConf().odr),
magOffset(),
mag_matrix_set(false), mag_offset_set(false),
acel_offset_set(false), gyro_offset_set(false)
......
......@@ -126,14 +126,17 @@ Configuration_Data Chordata::parse_cmd_line(int argc, const char **argv){
args::HelpFlag help(parser, "help", msgs::help, {'h', "help"});
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::Flag raw(parser, "raw_lectures", msgs::raw, {'r', "raw"});
args::Flag scan(parser, "scan Armature", msgs::scan, {"scan"});
args::Flag calib(parser, "Calibrate", msgs::calib, {'x', "calibrate", "calib"});
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 experimental(parser, ">> EXPERIMENTAL FEATURES <<", args::Group::Validators::DontCare);
args::ValueFlag<int> odr(experimental, "Sensor Output Data Rate",
"Read sensors at this frequency (default=50Hz, max=100Hz)", {"odr"});
args::Group xml(parser, msgs::xml_g, args::Group::Validators::DontCare);
......@@ -162,7 +165,7 @@ Configuration_Data Chordata::parse_cmd_line(int argc, const char **argv){
textFlag osc_err(osc_g, "osc_addr", msgs::osc_err, {"error_osc"});
textFlag osc_comm(osc_g, "osc_addr", msgs::osc_comm, {"comm_osc"});
args::Group misc_g(parser, "miscelaneous", args::Group::Validators::DontCare);
// args::Group misc_g(parser, "miscelaneous", args::Group::Validators::DontCare);
// args::ValueFlag<float>
// beta(misc_g, "Beta", "Madgwick's sensor fusion Beta parameter", {'B', "Beta"});
......@@ -191,16 +194,19 @@ Configuration_Data Chordata::parse_cmd_line(int argc, const char **argv){
conf.scan = (scan)? true : false;
conf.empirical_add = (empirical_add)? get(empirical_add) : 0;
if (odr){
if (get(odr) > 100){
throw args::ParseError(
format("The --odr argument should be less or equal to 100, was {} ", get(odr)));
}
conf.odr = get(odr);
}
if (kc_ver){
conf.kc_ver = get(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);
......
......@@ -82,7 +82,7 @@ long int Node_Scheluder::bang(long int wait){
// thread nodeSearch(&Node_Scheluder::doSearch, this);
// nodeSearch.detach();
static long int lap_teoric_duration = 1000000 / _CHORDATA_ODR;
static long int lap_teoric_duration = 1000000 / Chordata::getConf().odr;
static long int teoric_wait = lap_teoric_duration / getODRModifiers();
static auto first_sensor = seek_first_sensor(nodelist);
......
......@@ -82,6 +82,7 @@ Chordata::Configuration_Data::Configuration_Data(): //from Chordata_def.h
calib(false),
scan(false),
kc_ver(-1),
odr(_CHORDATA_ODR),
state({.beta_timer_init = 0, .imu_setup_complete = false})
{
......
......@@ -347,7 +347,7 @@ bool Notochord::run(){
//TODO:why is this happening?!
long sleepMicros = (1000*1000) /
((scheluder.getODRModifiers() + config.empirical_add) * _CHORDATA_ODR );
((scheluder.getODRModifiers() + config.empirical_add) * Chordata::getConf().odr );
const int notODRModifiers = scheluder.length() - scheluder.getODRModifiers();
if (notODRModifiers < 1){
......@@ -362,7 +362,7 @@ bool Notochord::run(){
comm::try_info("Starting timer. [NODES < {} , {} > = {} | ODR = {} | Sleep = {}us]",
notODRModifiers, scheluder.getODRModifiers(), scheluder.length(),
_CHORDATA_ODR, sleepMicros);
Chordata::getConf().odr, sleepMicros);
comm::try_info("Sending rate = {}Hz",
Chordata::getConf().comm.send_rate);
......@@ -573,6 +573,11 @@ int main(int argc, char const *argv[])
if (!status)
return 1;
} catch (const Chordata::CMDLine_Parse_Error& e){
std::cout<< e.what() << std::endl;
return 1;
} catch (const Chordata::Error& e){
auto d = parse_cmd_line(argc, argv);
......@@ -592,7 +597,7 @@ int main(int argc, char const *argv[])
if (d.comm.error & (Chordata::STDOUT | Chordata::STDERR) ){
spdlog::set_level(spdlog::level::err);
comm::init_communicator(d);
comm::err("Program aborted because of a memory alloction error:\n{}", e.what());
comm::err("Program aborted because of a memory allocation error:\n{}", e.what());
}else{
std::cerr<< e.what() << std::endl;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment