quadratic_throttle_model.h

#ifndef QUADRATIC_THRUST_MODEL_H
#define QUADRATIC_THRUST_MODEL_H
 
#include <cmath>
 
namespace mrs_lib
{
 
namespace quadratic_throttle_model
{
 
typedef struct
{
  double A;
  double B;
  int    n_motors;
} MotorParams_t;
 
double inline throttleToForce(const MotorParams_t motor_params, const double throttle) {
 
  return motor_params.n_motors * pow((throttle - motor_params.B) / motor_params.A, 2);
}
 
double inline forceToThrottle(const MotorParams_t motor_params, const double force) {
 
  return sqrt(force / motor_params.n_motors) * motor_params.A + motor_params.B;
}
 
}  // namespace quadratic_throttle_model
 
}  // namespace mrs_lib
 
#endif  // QUADRATIC_THRUST_MODEL_H