int
check_timestamp_is_fresh(const TimeInternal * timeA)
{
TimeInternal timeB;
getSystemClock()->getTime(getSystemClock(), &timeB);
return check_timestamp_is_fresh2(timeA, &timeB);
}
double
secondsToMidnight(void)
{
TimeInternal now;
double stm, ret;
getSystemClock()->getTime(getSystemClock(), &now);
stm = 86400.0 - (now.seconds % 86400);
ret = (stm - now.nanoseconds / 1E9);
return ret;
}
static double
feed (PIservo* self, Integer32 input, double tau) {
TimeInternal now, delta;
Boolean runningMaxOutput;
self->input = input;
switch(self->tauMethod) {
case DT_MEASURED:
getSystemClock()->getTimeMonotonic(getSystemClock(), &now);
if(isTimeZero(&self->lastUpdate)) {
self->tau = 1.0;
} else {
subTime(&delta, &now, &self->lastUpdate);
self->tau = delta.seconds + delta.nanoseconds / 1E9;
}
if(self->tau > (self->maxTau * tau)) {
self->tau = (self->maxTau + 0.0) * tau;
}
break;
case DT_CONSTANT:
self->tau = tau;
break;
default:
self->tau = 1.0;
}
if(self->tau <= ZEROF) {
self->tau = 1.0;
}
self->integral = clampDouble(self->integral, self->maxOutput);
self->output = clampDouble(self->output, self->maxOutput);
if (self->kP < 0.000001)
self->kP = 0.000001;
if (self->kI < 0.000001)
self->kI = 0.000001;
self->integral += (self->tau / self->delayFactor) * ((input + 0.0 ) * self->kI);
self->output = (self->kP * (input + 0.0) ) + self->integral;
self->integral = clampDouble(self->integral, self->maxOutput);
self->output = clampDouble(self->output, self->maxOutput);
runningMaxOutput = (fabs(self->output) >= self->maxOutput);
if(self->controller) {
DBG("%s tau %.09f input %d fabs %f out %f, mo %f\n", self->controller->name, self->tau, input, fabs(self->output), self->output, self->maxOutput);
}
if(runningMaxOutput && !self->runningMaxOutput) {
WARNING(THIS_COMPONENT"Clock %s servo now running at maximum output\n", self->controller->name);
}
self->runningMaxOutput = runningMaxOutput;
if(self->tauMethod == DT_MEASURED) {
self->lastUpdate = now;
}
self->_updated = TRUE;
self->_lastInput = self->input;
self->_lastOutput = self->output;
return self->output;
}
int main(int argc, const char** argv)
{
if (argc < 2)
{
std::cerr << "Usage: " << argv[0] << " <node-id>" << std::endl;
return 1;
}
const int self_node_id = std::stoi(argv[1]);
/*
* Initializing the node.
* Hardware and software version information is paramount for firmware update process.
*/
uavcan::Node<16384> node(getCanDriver(), getSystemClock());
node.setNodeID(self_node_id);
node.setName("org.uavcan.tutorial.updatee");
uavcan::protocol::HardwareVersion hwver; // TODO initialize correct values
hwver.major = 1;
node.setHardwareVersion(hwver);
uavcan::protocol::SoftwareVersion swver; // TODO initialize correct values
swver.major = 1;
node.setSoftwareVersion(swver);
const int node_start_res = node.start();
if (node_start_res < 0)
{
throw std::runtime_error("Failed to start the node; error: " + std::to_string(node_start_res));
}
/*
* Storage for the firmware downloader object.
* Can be replaced with a smart pointer instead.
*/
uavcan::LazyConstructor<FirmwareLoader> fw_loader;
/*
* Initializing the BeginFirmwareUpdate server.
*/
uavcan::ServiceServer<uavcan::protocol::file::BeginFirmwareUpdate> bfu_server(node);
const int bfu_res = bfu_server.start(
[&fw_loader, &node]
(const uavcan::ReceivedDataStructure<uavcan::protocol::file::BeginFirmwareUpdate::Request>& req,
uavcan::protocol::file::BeginFirmwareUpdate::Response resp)
{
std::cout << "Firmware update request:\n" << req << std::endl;
if (fw_loader.isConstructed())
{
resp.error = resp.ERROR_IN_PROGRESS;
}
else
{
const auto source_node_id = (req.source_node_id == 0) ? req.getSrcNodeID() : req.source_node_id;
fw_loader.construct<uavcan::INode&, uavcan::NodeID, decltype(req.image_file_remote_path.path)>
(node, source_node_id, req.image_file_remote_path.path);
}
std::cout << "Response:\n" << resp << std::endl;
});
if (bfu_res < 0)
{
throw std::runtime_error("Failed to start the BeginFirmwareUpdate server: " + std::to_string(bfu_res));
}
/*
* Running the node normally.
* All of the work will be done in background.
*/
while (true)
{
if (fw_loader.isConstructed())
{
node.setModeSoftwareUpdate();
if (fw_loader->getStatus() != FirmwareLoader::Status::InProgress)
{
if (fw_loader->getStatus() == FirmwareLoader::Status::Success)
{
auto image = fw_loader->getImage();
std::cout << "Firmware download succeeded [" << image.size() << " bytes]" << std::endl;
printHexDump(std::begin(image), std::end(image));
// TODO: save the firmware image somewhere.
}
else
{
std::cout << "Firmware download failed" << std::endl;
// TODO: handle the error, e.g. retry download, send a log message, etc.
}
fw_loader.destroy();
}
}
else
{
node.setModeOperational();
}
const int res = node.spin(uavcan::MonotonicDuration::fromMSec(500));
if (res < 0)
{
std::cerr << "Transient failure: " << res << std::endl;
}
}
}
int main(int argc, const char** argv)
{
if (argc < 2)
{
std::cerr << "Usage: " << argv[0] << " <node-id>" << std::endl;
return 1;
}
const int self_node_id = std::stoi(argv[1]);
uavcan::Node<NodeMemoryPoolSize> node(getCanDriver(), getSystemClock());
node.setNodeID(self_node_id);
node.setName("org.uavcan.tutorial.configuree");
const int node_start_res = node.start();
if (node_start_res < 0)
{
throw std::runtime_error("Failed to start the node; error: " + std::to_string(node_start_res));
}
/*
* This would be our configuration storage.
*/
static struct Params
{
unsigned foo = 42;
float bar = 0.123456F;
double baz = 1e-5;
std::string booz = "Hello world!";
} configuration;
/*
* Now, we need to define some glue logic between the server (below) and our configuration storage (above).
* This is done via the interface uavcan::IParamManager.
*/
class : public uavcan::IParamManager
{
void getParamNameByIndex(Index index, Name& out_name) const override
{
if (index == 0) { out_name = "foo"; }
if (index == 1) { out_name = "bar"; }
if (index == 2) { out_name = "baz"; }
if (index == 3) { out_name = "booz"; }
}
void assignParamValue(const Name& name, const Value& value) override
{
if (name == "foo")
{
/*
* Parameter "foo" is an integer, so we accept only integer values here.
*/
if (value.is(uavcan::protocol::param::Value::Tag::integer_value))
{
configuration.foo = *value.as<uavcan::protocol::param::Value::Tag::integer_value>();
}
}
else if (name == "bar")
{
/*
* Parameter "bar" is a floating point, so we accept only float values here.
*/
if (value.is(uavcan::protocol::param::Value::Tag::real_value))
{
configuration.bar = *value.as<uavcan::protocol::param::Value::Tag::real_value>();
}
}
else if (name == "baz")
{
/*
* Ditto
*/
if (value.is(uavcan::protocol::param::Value::Tag::real_value))
{
configuration.baz = *value.as<uavcan::protocol::param::Value::Tag::real_value>();
}
}
else if (name == "booz")
{
/*
* Parameter "booz" is a string, so we take only strings.
*/
if (value.is(uavcan::protocol::param::Value::Tag::string_value))
{
configuration.booz = value.as<uavcan::protocol::param::Value::Tag::string_value>()->c_str();
}
}
else
{
std::cout << "Can't assign parameter: " << name.c_str() << std::endl;
}
}
void readParamValue(const Name& name, Value& out_value) const override
{
if (name == "foo")
{
out_value.to<uavcan::protocol::param::Value::Tag::integer_value>() = configuration.foo;
}
else if (name == "bar")
{
out_value.to<uavcan::protocol::param::Value::Tag::real_value>() = configuration.bar;
}
else if (name == "baz")
{
out_value.to<uavcan::protocol::param::Value::Tag::real_value>() = configuration.baz;
}
else if (name == "booz")
{
out_value.to<uavcan::protocol::param::Value::Tag::string_value>() = configuration.booz.c_str();
}
else
{
std::cout << "Can't read parameter: " << name.c_str() << std::endl;
}
}
int saveAllParams() override
{
std::cout << "Save - this implementation does not require any action" << std::endl;
return 0; // Zero means that everything is fine.
}
int eraseAllParams() override
{
std::cout << "Erase - all params reset to default values" << std::endl;
configuration = Params();
return 0;
}
/**
* Note that this method is optional. It can be left unimplemented.
*/
void readParamDefaultMaxMin(const Name& name, Value& out_def,
NumericValue& out_max, NumericValue& out_min) const override
{
if (name == "foo")
{
out_def.to<uavcan::protocol::param::Value::Tag::integer_value>() = Params().foo;
out_max.to<uavcan::protocol::param::NumericValue::Tag::integer_value>() = 9000;
out_min.to<uavcan::protocol::param::NumericValue::Tag::integer_value>() = 0;
}
else if (name == "bar")
{
out_def.to<uavcan::protocol::param::Value::Tag::real_value>() = Params().bar;
out_max.to<uavcan::protocol::param::NumericValue::Tag::real_value>() = 1;
out_min.to<uavcan::protocol::param::NumericValue::Tag::real_value>() = 0;
}
else if (name == "baz")
{
out_def.to<uavcan::protocol::param::Value::Tag::real_value>() = Params().baz;
out_max.to<uavcan::protocol::param::NumericValue::Tag::real_value>() = 1;
out_min.to<uavcan::protocol::param::NumericValue::Tag::real_value>() = 0;
}
else if (name == "booz")
{
out_def.to<uavcan::protocol::param::Value::Tag::string_value>() = Params().booz.c_str();
std::cout << "Limits for 'booz' are not defined" << std::endl;
}
else
{
std::cout << "Can't read the limits for parameter: " << name.c_str() << std::endl;
}
}
} param_manager;
/*
* Initializing the configuration server.
* A pointer to the glue logic object is passed to the method start().
*/
uavcan::ParamServer server(node);
const int server_start_res = server.start(¶m_manager);
if (server_start_res < 0)
{
throw std::runtime_error("Failed to start ParamServer: " + std::to_string(server_start_res));
}
/*
* Now, this node can be reconfigured via UAVCAN. Awesome.
* Many embedded applications require a restart before the new configuration settings can
* be applied, so it is highly recommended to also support the remote restart service.
*/
class : public uavcan::IRestartRequestHandler
{
bool handleRestartRequest(uavcan::NodeID request_source) override
{
std::cout << "Got a remote restart request from " << int(request_source.get()) << std::endl;
/*
* We won't really restart, so return 'false'.
* Returning 'true' would mean that we're going to restart for real.
* Note that it is recognized that some nodes may not be able to respond to the
* restart request (e.g. if they restart immediately from the service callback).
*/
return false;
}
} restart_request_handler;
/*
* Installing the restart request handler.
*/
node.setRestartRequestHandler(&restart_request_handler); // Done.
/*
* Running the node.
*/
node.setModeOperational();
while (true)
{
const int res = node.spin(uavcan::MonotonicDuration::getInfinite());
if (res < 0)
{
std::cerr << "Transient failure: " << res << std::endl;
}
}
}
