Commit 6ccdfaf7 authored by Christian Sponfeldner's avatar Christian Sponfeldner

2WD works again

parent 51ad0134
......@@ -42,7 +42,8 @@ USER_LIB_PATHS =
PATH_EM5 =/media/em5
OPTIMISATAON =-O0 -fno-strict-overflow
USER_DEFINES =DEF_4WD
#USER_DEFINES =DEF_2WD
USER_DEFINES =
######################################################
###############Don't touch############################
......
......@@ -453,7 +453,7 @@ EXTRACT_PACKAGE = NO
# included in the documentation.
# The default value is: NO.
EXTRACT_STATIC = NO
EXTRACT_STATIC = YES
# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined
# locally in source files will be included in the documentation. If set to NO,
......@@ -790,7 +790,7 @@ WARN_LOGFILE =
# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING
# Note: If this tag is empty the current directory is searched.
INPUT = can_gateway
INPUT = can_gateway emros
# This tag can be used to specify the character encoding of the source files
# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
......
......@@ -8,6 +8,7 @@
#ifndef EMROS_EMROS_INC_BASECONTROLLER_H_
#define EMROS_EMROS_INC_BASECONTROLLER_H_
#include "../../emrosCommon/inc/drive_kinematics.h"
#include "../../emrosCommon/inc/emros_commands.h"
#include "../../../nanotec/inc/nanoj.h"
#include <cmath>
......@@ -19,9 +20,10 @@
#include "ramp_generator.h"
#include "../../system/inc/factories.h"
#include "basecontroller_types.h"
#include "../../emrosCommon/inc/drive_kinematics.h"
#ifdef DEF_4WD
#include "../../motors/inc/can_mapping_4WD.h"
#elif DEF_2WD
#elif defined DEF_2WD
#include "../../motors/inc/can_mapping_2WD.h"
#endif
#include <vector>
......
/*
* powermanagemant.h
*
* Created on: Mar 14, 2018
* Author: speckle_s
*/
#ifndef EMROS_DEVICES_INC_POWERMANAGEMANT_H_
#define EMROS_DEVICES_INC_POWERMANAGEMANT_H_
#include "../../NanoOFC/core/inc/nano_ofc.h"
class PowerManagemant
{
public:
struct RAW_MSG_struct
{
uint16_t cmd;
int16_t data;
uint32_t reserved;
};
struct POWER_DATA_struct
{
int16_t supply_current;
int16_t supply_voltage;
int16_t battery_current;
int16_t battery_voltage;
};
typedef Container<POWER_DATA_struct> power_data_t;
typedef Container<RAW_MSG_struct> RAW_MSG_t;
PowerManagemant() :
mJob(WAIT),mBootup(false)
{
}
~PowerManagemant()
{
}
Error init_power_management();
Error new_raw_can_msg(uint8_t *buffer,uint8_t len);
private:
Error msgBoxUpdate();
Error ethMsgUpdate();
Error CANSyncUpdate();
enum job_enum {WAIT,START,GET_BATTERY_CURRENT,GET_BATTERY_VOLTAGE,GET_SUPPLY_CURRENT,GET_SUPPLY_VOLTAGE,SEND_TO_GUI} mJob;
power_data_t mPower_data;
bool mBootup;
};
#endif /* EMROS_DEVICES_INC_POWERMANAGEMANT_H_ */
/*
* powermanagement.h
*
* Created on: May 05, 2018
* Author: sponfeldner_c
*/
#ifndef EMROS_DEVICES_INC_POWERMANAGEMENT_H_
#define EMROS_DEVICES_INC_POWERMANAGEMENT_H_
#include "../../NanoOFC/core/inc/nano_ofc.h"
#include "sdo_bridge_intern.h"
#include "../../system/inc/msg.h"
class PowerManagement {
public:
struct POWER_DATA_struct {
int16_t supply_current;
int16_t supply_voltage;
int16_t battery_current;
int16_t battery_voltage;
};
typedef Container<POWER_DATA_struct> power_data_t;
PowerManagement();
~PowerManagement();
Error setUp(bool& set_up_finished);
private:
enum job_enum {
WAIT, START, GET_BATTERY_CURRENT, GET_BATTERY_VOLTAGE, GET_SUPPLY_CURRENT, GET_SUPPLY_VOLTAGE, SEND_TO_GUI
} mJob;
power_data_t mPower_data;
bool mBootup;
CANSyncHandler::ObserverSyncHandler mObserver_cyclic_can_sync;
SdoBridgeIntern mSdo_bridge_intern;
OFCLoggerRTClient& myLogger { *OFCLoggerRTClient::instance() };
// Error msgBoxUpdate();
// Error ethMsgUpdate();
// Error CANSyncUpdate();
Error CyclicCANSyncCallback();
};
#endif /* EMROS_DEVICES_INC_POWERMANAGEMENT_H_ */
/*
* sdo_bridge.h
*
* Created on: Feb 7, 2018
* Author: speckle_s
*/
#ifndef CAN_GATEWAY_DEVICES_INC_SDO_BRIDGE_H_
#define CAN_GATEWAY_DEVICES_INC_SDO_BRIDGE_H_
#include "../../NanoOFC/core/inc/nano_ofc.h"
#include <array>
#include "../../utilityOFC/inc/timeout.h"
class SdoBridge
{
public:
SdoBridge();
~SdoBridge();
void check_timeout();
private:
Error ethCallback(const EthHandler::Dataframe& data);
Error sdoCallback(const SdoHandler::SdoRxData& data);
enum class Specifier:uint8_t
{
SUCCSESSFUL_OPERATION = 0, READ = 1, WRITE = 2, ERROR = 0xff
};
enum class Status:uint8_t
{
READY = 0, MASTER_BUSY = 1, SLAVE_BUSY = 2
};
enum :uint32_t{MAX_DATA_LENGTH = (SdoHandler::SDO_MAX_DATA_LENGTH - sizeof(Specifier))};
struct ProtocolFrame
{
SdoHandler::NodeId node_id;
union {
struct
{
SdoHandler::Index index;
SdoHandler::Subindex subindex;
Status status;
}__attribute__((packed));
Ethprotocol::Cmd cmd;
};
union
{
struct
{
Specifier specifier;
std::array<uint8_t, MAX_DATA_LENGTH > data;
}__attribute__((packed));
std::array<uint8_t, SdoHandler::SDO_MAX_DATA_LENGTH> raw_payload;
};
uint8_t length_data;
};
std::array<EthHandler::ObserverEthHandler*, 127> mEth_observers;
std::array<SdoHandler::SdoObserver*, 127> mSdo_observers;
ProtocolFrame mSdo_request;
Timeout<uint32_t, uint32_t, uint32_t (*)()> mSdo_request_timeout;
bool mSdo_request_pending;
SdoHandler& mSdo_handler;
EthHandler& mEth_handler;
OFCLoggerRTClient& mLogger_client;
Error SendErrorMessage(const ProtocolFrame& sdo_request, const char* error_message, Status status=Status::READY);
};
#endif /* CAN_GATEWAY_DEVICES_INC_SDO_BRIDGE_H_ */
/*
* sdo_bridge_intern.h
*
* Created on: Mar 27, 2018
* Author: sponfeldner_c
*/
#ifndef EMROS_DEVICES_INC_SDO_BRIDGE_INTERN_H_
#define EMROS_DEVICES_INC_SDO_BRIDGE_INTERN_H_
#include <array>
#include "../../NanoOFC/core/inc/nano_ofc.h"
#include "../../NanoOFC/core/inc/sdo_handler.h"
#include "../../utilityOFC/inc/timeout.h"
#include "../../utilityOFC/inc/datacontainer.h"
class SdoBridgeIntern {
public:
SdoBridgeIntern(SdoHandler::NodeId node_id);
~SdoBridgeIntern();
using NodeId = SdoHandler::NodeId;
using Index = SdoHandler::Index;
using Subindex = SdoHandler::Subindex;
const NodeId mNode_id;
Error sdoRead(const Index index, const Subindex subindex, bool &finished, uint16_t &readout);
Error sdoRead(const Index index, const Subindex subindex, bool &finished, uint32_t &readout);
Error sdoRead(const Index index, const Subindex subindex, bool &finished, float &readout);
Error sdoWrite(const Index index, const Subindex subindex, bool &finished, const uint16_t write_value);
enum SdoCommand {
READ, WRITE
};
struct SdoRequest {
Index index;
Subindex subindex;
SdoHandler::DataLen data_size_bytes;
SdoCommand command;
SdoRequest(SdoHandler::Index _index, SdoHandler::Subindex _subindex, SdoHandler::DataLen _sdo_data_len, SdoCommand _command) :
index(_index), subindex(_subindex), data_size_bytes(_sdo_data_len), command(_command) {
}
void reset(void) {
index = 0;
subindex = 0;
data_size_bytes = 0;
command = READ;
}
friend bool operator==(const SdoRequest& lhs, const SdoRequest& rhs) {
return lhs.index == rhs.index && lhs.subindex == rhs.subindex && lhs.data_size_bytes == rhs.data_size_bytes
&& lhs.command == rhs.command;
}
friend bool operator!=(const SdoRequest& lhs, const SdoRequest& rhs) {
return !(lhs == rhs);
}
} mSdo_request;
private:
enum SdoRequestStep {
IDLE, SEND_SDO_REQUEST, WAIT_FOR_SDO_ANSWER, FOREWARD_ANSWER,
};
SdoHandler::SdoObserver mSdoObserver;
SdoRequestStep mSdo_request_step = IDLE;
struct sdo_transmission {
uint32_t number_bytes = 0;
std::array<uint8_t, SdoHandler::SDO_MAX_DATA_LENGTH> data;
Error status = Error::NO_ERROR;
bool answer_received = false;
void reset(void) {
number_bytes = 0;
data.fill(0);
status = Error::NO_ERROR;
answer_received = false;
}
} mSdo_transmission;
Timeout<uint32_t, uint32_t, uint32_t (*)()> mSdo_request_timeout;
OFCLoggerRTClient& myLogger { *OFCLoggerRTClient::instance() };
Error sdoCallback(const SdoHandler::SdoRxData& data);
Error sdoRequestHandler(const SdoRequest sdo_target, bool &finished);
};
#endif /* EMROS_DEVICES_INC_SDO_BRIDGE_INTERN_H_ */
This diff is collapsed.
/*
* sdo_bridge.cpp
*
* Created on: Feb 7, 2018
* Author: speckle_s
*/
#include "../inc/sdo_bridge.h"
//#define T
#ifdef T
using namespace CanOpenErrors;
SdoBridge::SdoBridge() :
mEth_observer(DEVICE_ID,&SdoBridge::sdoCallback, this),mSdo_request(),mLast_cmd(), mSdo_request_timeout(static_cast<uint32_t>(10000), System::GetReferenceTimer), mSdo_request_pending(
false), mSdo_handler(*SdoHandler::instance()), mEth_handler(*EthHandler::instance()),mLogger_client(*OFCLoggerRTClient::instance())
{
for (int i = 0; i < 127; i++)
{
mSdo_observers.at(i) = new SdoHandler::SdoObserver((static_cast<uint8_t>(i + 1)), &SdoBridge::sdoCallback, this);
}
}
SdoBridge::~SdoBridge()
{
}
void SdoBridge::check_timeout()
{
if (mSdo_request_pending && mSdo_request_timeout.timeoutReached())
{
SendErrorMessage(mSdo_request,"SdoBridge : check_timeout : timeout");
mSdo_request_pending = false;
Can::SdoAbort(mSdo_request.getData()..node_id,0);
}
}
Error SdoBridge::ethCallback(const EthHandler::Dataframe& data)
{
if (mSdo_request_pending == false)
{
//std::copy(data.getData().payload.begin(),data.getData().payload.begin()+data.getData().payload_length,mSdo_request.raw.begin());
mSdo_request.raw = data.getData().payload;
//TODO: check for len=0 ?
if (data.getData().cmd == READ_SDO)
{
SdoHandler::SdoUploadData upload_data {};
upload_data.node_id = mSdo_request.data.node_id;
upload_data.index = mSdo_request.data.index;
upload_data.subindex = mSdo_request.data.subindex;
Error status {mSdo_handler.SdoUpload(upload_data)};
if (status != Error::NO_ERROR)
{
return (Error::ERROR);
}
mSdo_request_pending = true;
mSdo_request_timeout.resetTime();
}
else if (data.getData().cmd == WRITE_SDO)
{
SdoHandler::SdoDownloadData download_data {};
download_data.node_id = mSdo_request.data.node_id;
download_data.index = mSdo_request.data.index;
download_data.subindex = mSdo_request.data.subindex;
download_data.data_length = mSdo_request.data.sdo_data_size;
std::copy_n(mSdo_request.data.sdo_data.begin(), download_data.data_length, download_data.data.begin());
Error status {mSdo_handler.SdoDownload(download_data)};
if (status != Error::NO_ERROR)
{
return (Error::ERROR);
}
mSdo_request_pending = true;
mSdo_request_timeout.resetTime();
}
else
{
SendErrorMessage(mSdo_request,"SdoBridge : ethCallback : wrong specifier");
return (Error::ERROR);
}
}
else
{
SdoFrame protocol_frame {};
EthHandler::Dataframe eth_tx_frame {};
protocol_frame.data.node_id = static_cast<SdoHandler::NodeId>(data.getData().id);
protocol_frame.data.cmd = data.getData().cmd;
SendErrorMessage(protocol_frame,"SdoBridge : ethCallback : Bridge busy",Status::MASTER_BUSY);
return (Error::ERROR);
}
return (Error::NO_ERROR);
}
Error SdoBridge::sdoCallback(const SdoHandler::SdoRxData& data)
{
if (data.node_id != mSdo_request.node_id)
{
SendErrorMessage(mSdo_request,"SdoBridge : sdoCallback : received NodeId does not fit to requested NodeId");
return (Error::ERROR);
}
auto cm_error = CmSDOErrors::_from_integral_nothrow(data.extended_status);
if (cm_error == false)
{
SendErrorMessage(mSdo_request,"SdoBridge : sdoCallback : unknown CM error");
return (Error::ERROR);
}
else
{
if (*cm_error != (+CmSDOErrors::CM_SUCCSESSFUL))
{
if (*cm_error == +CmSDOErrors::CM_SDO_C_BUSY)
{
SendErrorMessage(mSdo_request,"SdoBridge : sdoCallback : CM error: CM_SDO_C_BUSY",Status::MASTER_BUSY);
return (Error::ERROR);
}
else if (*cm_error == +CmSDOErrors::CM_SDO_S_BUSY)
{
SendErrorMessage(mSdo_request,"SdoBridge : sdoCallback : CM error: CM_SDO_S_BUSY",Status::SLAVE_BUSY);
return (Error::ERROR);
}
else
{
char str[150];
std::sprintf(str, "SdoBridge : sdoCallback : CM error: %s", cm_error->_to_string());
SendErrorMessage(mSdo_request,str);
return (Error::ERROR);
}
}
}
auto abort_code = AbortCodes::_from_integral_nothrow(data.abort_code);
if (abort_code == false)
{
SendErrorMessage(mSdo_request,"SdoBridge : sdoCallback : unknown abort code");
return (Error::ERROR);
}
else
{
if (abort_code->_to_integral() != (+AbortCodes::SUCCSESS)._to_integral())
{
char str[150];
std::sprintf(str, "SdoBridge : sdoCallback : abort code: %s", abort_code->_to_string());
SendErrorMessage(mSdo_request,str);
return (Error::ERROR);
}
}
EthHandler::Dataframe dataframe;
ProtocolFrame sdo_answer;
sdo_answer.node_id = mSdo_request.node_id;
sdo_answer.cmd = mSdo_request.cmd;
sdo_answer.status = Status::READY;
sdo_answer.specifier = Specifier::SUCCSESSFUL_OPERATION;
if(mSdo_request.specifier == Specifier::READ)
{
if (data.transmitted_bytes <= MAX_DATA_LENGTH)
{
sdo_answer.length_data = static_cast<uint8_t>(data.transmitted_bytes);
std::copy_n(data.data.begin(),data.transmitted_bytes,sdo_answer.data.begin());
}
else
{
SendErrorMessage(mSdo_request,"SdoBridge : sdoCallback : rx sdo data exceeds tx eth data");
return (Error::ERROR);
}
}
else if (mSdo_request.specifier == Specifier::WRITE)
{
sdo_answer.length_data = 0;
}
else
{
SendErrorMessage(mSdo_request,"SdoBridge : sdoCallback : WTF");
return (Error::ERROR);
}
dataframe.setData().id = sdo_answer.node_id;
dataframe.setData().cmd = sdo_answer.cmd;
dataframe.setData().payload_length = static_cast<Ethprotocol::PayLen>(sdo_answer.length_data + sizeof(Specifier));
std::copy_n(sdo_answer.raw_payload.begin(), dataframe.setData().payload_length, dataframe.setData().payload.begin());
mSdo_request_pending = false;
Error error {mEth_handler.SendEthMsg(dataframe)};
if (error != Error::NO_ERROR)
{
return error;
}
return (Error::NO_ERROR);
}
Error SdoBridge::SendErrorMessage(const ProtocolFrame& sdo_request, const char* error_message, Status status)
{
mLogger_client.InsertError(__PRETTY_FUNCTION__ ,error_message);
EthHandler::Dataframe eth_tx_frame {};
ProtocolFrame protocol_frame;
protocol_frame.node_id = sdo_request.node_id;
protocol_frame.cmd = sdo_request.cmd;
protocol_frame.status = status;
protocol_frame.specifier = Specifier::ERROR;
std::string str(error_message);
if(str.size() <= (MAX_DATA_LENGTH))
{
protocol_frame.length_data = static_cast <uint8_t>(str.size());
std::copy(str.begin(),str.end(), protocol_frame.data.begin());
eth_tx_frame.setData().id = protocol_frame.node_id;
eth_tx_frame.setData().cmd = protocol_frame.cmd;
eth_tx_frame.setData().payload_length = static_cast<Ethprotocol::PayLen>(protocol_frame.length_data + sizeof(protocol_frame.specifier));
std::copy_n(protocol_frame.raw_payload.begin(), eth_tx_frame.setData().payload_length,
eth_tx_frame.setData().payload.begin());
mEth_handler.SendEthMsg(eth_tx_frame);
mSdo_request_pending = false;
return (Error::NO_ERROR);
}
else
{
mLogger_client.InsertError(__PRETTY_FUNCTION__ ," msg overflow");
mSdo_request_pending = false;
return (Error::ERROR);
}
}
#endif
/*
* sdo_bridge_intern.cpp
*
* Created on: Mar 27, 2018
* Author: sponfeldner_c
*/
//#ifdef T
#include "../inc/sdo_bridge_intern.h"
namespace {
const uint32_t SDO_TIMEOUT_US = 1E6;
const uint32_t SDO_STANDARD_ABORDCODE = 0L;
}
SdoBridgeIntern::SdoBridgeIntern(SdoHandler::NodeId node_id) :
mNode_id(node_id), mSdoObserver(node_id, &SdoBridgeIntern::sdoCallback, this), mSdo_request(0, 0, 0, SdoCommand::READ), mSdo_transmission(
sdo_transmission()), mSdo_request_timeout(SDO_TIMEOUT_US, System::GetReferenceTimer) {
myLogger.InsertInfo(__PRETTY_FUNCTION__, " open intern Sdo-bridge with node-id ", node_id, ".");
}
//This method needs to be called multiple times until the finished-flag is true
Error SdoBridgeIntern::sdoRead(const Index index, const Subindex subindex, bool &finished, uint16_t &readout) {
Error retStatus = sdoRequestHandler(SdoBridgeIntern::SdoRequest(index, subindex, sizeof(uint16_t), SdoBridgeIntern::SdoCommand::READ),
finished);
union_t<uint16_t> temp { uint16_t(0) };
std::copy_n(mSdo_transmission.data.begin(), sizeof(uint16_t), temp.raw.begin());
readout = temp.data;
return retStatus;
}
//This method needs to be called multiple times until the finished-flag is true
Error SdoBridgeIntern::sdoRead(const Index index, const Subindex subindex, bool &finished, uint32_t &readout) {
Error retStatus = sdoRequestHandler(SdoBridgeIntern::SdoRequest(index, subindex, sizeof(uint32_t), SdoBridgeIntern::SdoCommand::READ),
finished);
union_t<uint32_t> temp { uint32_t(0) };
std::copy_n(mSdo_transmission.data.begin(), sizeof(uint32_t), temp.raw.begin());
readout = temp.data;
return retStatus;
}
//This method needs to be called multiple times until the finished-flag is true
Error SdoBridgeIntern::sdoRead(const Index index, const Subindex subindex, bool &finished, float &readout) {
Error retStatus = sdoRequestHandler(SdoBridgeIntern::SdoRequest(index, subindex, sizeof(uint32_t), SdoBridgeIntern::SdoCommand::READ),
finished);
union_t<float> temp { float(0) };
std::copy_n(mSdo_transmission.data.begin(), sizeof(uint32_t), temp.raw.begin());
readout = temp.data;
return retStatus;
}
//This method needs to be called multiple times until the finished-flag is true
Error SdoBridgeIntern::sdoWrite(const Index index, const Subindex subindex, bool &finished, const uint16_t write_value) {
mSdo_transmission.number_bytes = sizeof(uint16_t);
union_t<uint16_t> temp { uint16_t(write_value) };
std::copy_n(temp.raw.begin(), sizeof(uint16_t), mSdo_transmission.data.begin());
Error retStatus = sdoRequestHandler(SdoBridgeIntern::SdoRequest(index, subindex, sizeof(uint16_t), SdoBridgeIntern::SdoCommand::WRITE),
finished);
return retStatus;
}
Error SdoBridgeIntern::sdoRequestHandler(SdoRequest sdo_request, bool &sdo_finished) {
Error return_value = Error::ERROR;
sdo_finished = false;
if (mSdo_request_step != SdoRequestStep::IDLE && sdo_request != mSdo_request) {
myLogger.InsertError(__PRETTY_FUNCTION__, " can not start new sdo-request because another sdo-request is still ongoing.");
return_value = Error::ERROR;
} else {
switch (mSdo_request_step) {
case IDLE: {
mSdo_request.reset();
mSdo_request.index = sdo_request.index;
mSdo_request.subindex = sdo_request.subindex;
mSdo_request.data_size_bytes = sdo_request.data_size_bytes;
mSdo_request.command = sdo_request.command;
switch (mSdo_request.command) {
case READ: {
mSdo_transmission.reset();
break;
}
case WRITE: {
mSdo_transmission.answer_received = false;
break;
}
}
}
/* no break */
case SEND_SDO_REQUEST: {
Error status = Error::ERROR;
switch (mSdo_request.command) {
case READ: {
SdoHandler::SdoUploadData upload_data { };
upload_data.node_id = mNode_id;
upload_data.index = sdo_request.index;
upload_data.subindex = sdo_request.subindex;
status = SdoHandler::instance()->SdoUpload(upload_data);
break;
}
case WRITE: {
SdoHandler::SdoDownloadData download_data { };
download_data.node_id = mNode_id;
download_data.index = mSdo_request.index;
download_data.subindex = mSdo_request.subindex;