Skip to content

fix(simple_flight): protect motor/PID clamps from Windows min/max macros #9820

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Bartok9 wants to merge 1 commit into
base: main
Choose a base branch
from
Open

fix(simple_flight): protect motor/PID clamps from Windows min/max macros #9820

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Mixer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,8 +43,8 @@ class Mixer
}

for (int motor_index = 0; motor_index < kMotorCount; ++motor_index)
motor_outputs[motor_index] = std::max(params_->motor.min_motor_output,
std::min(motor_outputs[motor_index], params_->motor.max_motor_output));
motor_outputs[motor_index] = (std::max)(params_->motor.min_motor_output,
(std::min)(motor_outputs[motor_index], params_->motor.max_motor_output));
}

private:
Expand Down
266 changes: 133 additions & 133 deletions AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PidController.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,133 +1,133 @@
#pragma once
#include <cstdlib>
#include <algorithm>
#include "interfaces/CommonStructs.hpp"
#include "interfaces/IPidIntegrator.hpp"
#include "StdPidIntegrator.hpp"
#include "RungKuttaPidIntegrator.hpp"
namespace simple_flight
{
template <class T>
class PidController : public IUpdatable
{
public:
PidController(const IBoardClock* clock = nullptr, const PidConfig<T>& config = PidConfig<T>())
: clock_(clock), config_(config)
{
switch (config.integrator_type) {
case PidConfig<T>::IntegratorType::Standard:
integrator = std::unique_ptr<StdPidIntegrator<T>>(new StdPidIntegrator<T>(config_));
break;
case PidConfig<T>::IntegratorType::RungKutta:
integrator = std::unique_ptr<RungKuttaPidIntegrator<T>>(new RungKuttaPidIntegrator<T>(config_));
break;
default:
throw std::invalid_argument("PID integrator type is not recognized");
}
}
void setGoal(const T& goal)
{
goal_ = goal;
}
const T& getGoal() const
{
return goal_;
}
void setMeasured(const T& measured)
{
measured_ = measured;
}
const T& getMeasured() const
{
return measured_;
}
const PidConfig<T>& getConfig() const
{
return config_;
}
//allow changing config at runtime
void setConfig(const PidConfig<T>& config)
{
bool renabled = !config_.enabled && config.enabled;
config_ = config;
if (renabled) {
last_error_ = goal_ - measured_;
integrator->set(output_);
}
}
T getOutput()
{
return output_;
}
virtual void reset() override
{
IUpdatable::reset();
goal_ = T();
measured_ = T();
last_time_ = clock_ == nullptr ? 0 : clock_->millis();
integrator->reset();
last_error_ = goal_ - measured_;
min_dt_ = config_.time_scale * config_.time_scale;
}
virtual void update() override
{
IUpdatable::update();
if (!config_.enabled)
return;
const T error = goal_ - measured_;
float dt = clock_ == nullptr ? 1 : (clock_->millis() - last_time_) * config_.time_scale;
float pterm = error * config_.kp;
float dterm = 0;
if (dt > min_dt_) {
integrator->update(dt, error, last_time_);
float error_der = dt > 0 ? (error - last_error_) / dt : 0;
dterm = error_der * config_.kd;
last_error_ = error;
}
output_ = config_.output_bias + pterm + integrator->getOutput() + dterm;
//limit final output
output_ = clip(output_, config_.min_output, config_.max_output);
last_time_ = clock_->millis();
}
private:
//TODO: replace with std::clamp after moving to C++17
static T clip(T val, T min_value, T max_value)
{
return std::max(min_value, std::min(val, max_value));
}
private:
T goal_, measured_;
T output_;
uint64_t last_time_;
const IBoardClock* clock_;
float last_error_;
float min_dt_;
const PidConfig<T> config_;
std::unique_ptr<IPidIntegrator<T>> integrator;
};
} //namespace
#pragma once

#include <cstdlib>
#include <algorithm>
#include "interfaces/CommonStructs.hpp"
#include "interfaces/IPidIntegrator.hpp"
#include "StdPidIntegrator.hpp"
#include "RungKuttaPidIntegrator.hpp"

namespace simple_flight
{

template <class T>
class PidController : public IUpdatable
{
public:
PidController(const IBoardClock* clock = nullptr, const PidConfig<T>& config = PidConfig<T>())
: clock_(clock), config_(config)
{
switch (config.integrator_type) {
case PidConfig<T>::IntegratorType::Standard:
integrator = std::unique_ptr<StdPidIntegrator<T>>(new StdPidIntegrator<T>(config_));
break;
case PidConfig<T>::IntegratorType::RungKutta:
integrator = std::unique_ptr<RungKuttaPidIntegrator<T>>(new RungKuttaPidIntegrator<T>(config_));
break;
default:
throw std::invalid_argument("PID integrator type is not recognized");
}
}

void setGoal(const T& goal)
{
goal_ = goal;
}
const T& getGoal() const
{
return goal_;
}

void setMeasured(const T& measured)
{
measured_ = measured;
}
const T& getMeasured() const
{
return measured_;
}

const PidConfig<T>& getConfig() const
{
return config_;
}

//allow changing config at runtime
void setConfig(const PidConfig<T>& config)
{
bool renabled = !config_.enabled && config.enabled;
config_ = config;

if (renabled) {
last_error_ = goal_ - measured_;
integrator->set(output_);
}
}

T getOutput()
{
return output_;
}

virtual void reset() override
{
IUpdatable::reset();

goal_ = T();
measured_ = T();
last_time_ = clock_ == nullptr ? 0 : clock_->millis();
integrator->reset();
last_error_ = goal_ - measured_;
min_dt_ = config_.time_scale * config_.time_scale;
}

virtual void update() override
{
IUpdatable::update();

if (!config_.enabled)
return;

const T error = goal_ - measured_;

float dt = clock_ == nullptr ? 1 : (clock_->millis() - last_time_) * config_.time_scale;

float pterm = error * config_.kp;
float dterm = 0;
if (dt > min_dt_) {
integrator->update(dt, error, last_time_);

float error_der = dt > 0 ? (error - last_error_) / dt : 0;
dterm = error_der * config_.kd;
last_error_ = error;
}

output_ = config_.output_bias + pterm + integrator->getOutput() + dterm;

//limit final output
output_ = clip(output_, config_.min_output, config_.max_output);

last_time_ = clock_->millis();
}

private:
//TODO: replace with std::clamp after moving to C++17
static T clip(T val, T min_value, T max_value)
{
return (std::max)(min_value, (std::min)(val, max_value));
}

private:
T goal_, measured_;
T output_;
uint64_t last_time_;
const IBoardClock* clock_;

float last_error_;
float min_dt_;
const PidConfig<T> config_;

std::unique_ptr<IPidIntegrator<T>> integrator;
};

} //namespace
2 changes: 1 addition & 1 deletion ...b/include/vehicles/multirotor/firmwares/simple_flight/firmware/RungKuttaPidIntegrator.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ class RungKuttaPidIntegrator : public IPidIntegrator<T>
//TODO: replace with std::clamp after moving to C++17
static T clip(T val, T min_value, T max_value)
{
return std::max(min_value, std::min(val, max_value));
return (std::max)(min_value, (std::min)(val, max_value));
}

void model(float* y_val, float last_time_, float* y_out)
Expand Down
2 changes: 1 addition & 1 deletion AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/StdPidIntegrator.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ class StdPidIntegrator : public IPidIntegrator<T>
//TODO: replace with std::clamp after moving to C++17
static T clip(T val, T min_value, T max_value)
{
return std::max(min_value, std::min(val, max_value));
return (std::max)(min_value, (std::min)(val, max_value));
}

private:
Expand Down
2 changes: 1 addition & 1 deletion AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/VelocityController.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -114,7 +114,7 @@ class VelocityController : public IAxisController
break;
case 3: //+vz is -ve throttle (NED coordinates)
output_ = (-pid_->getOutput() + 1) / 2; //-1 to 1 --> 0 to 1
output_ = std::max(output_, params_->velocity_pid.min_throttle);
output_ = (std::max)(output_, params_->velocity_pid.min_throttle);
break;
default:
throw std::invalid_argument("axis must be 0, 1 or 3 for VelocityController");
Expand Down