/*/
 * Copyright (c) 2018-2019, 2021-2024, 2026 LAAS/CNRS
 * All rights reserved.
 *
 * Redistribution  and  use  in  source  and binary  forms,  with  or  without
 * modification, are permitted provided that the following conditions are met:
 *
 *   1. Redistributions of  source  code must retain the  above copyright
 *      notice and this list of conditions.
 *   2. Redistributions in binary form must reproduce the above copyright
 *      notice and  this list of  conditions in the  documentation and/or
 *      other materials provided with the distribution.
 *
 * THE SOFTWARE  IS PROVIDED "AS IS"  AND THE AUTHOR  DISCLAIMS ALL WARRANTIES
 * WITH  REGARD   TO  THIS  SOFTWARE  INCLUDING  ALL   IMPLIED  WARRANTIES  OF
 * MERCHANTABILITY AND  FITNESS.  IN NO EVENT  SHALL THE AUTHOR  BE LIABLE FOR
 * ANY  SPECIAL, DIRECT,  INDIRECT, OR  CONSEQUENTIAL DAMAGES  OR  ANY DAMAGES
 * WHATSOEVER  RESULTING FROM  LOSS OF  USE, DATA  OR PROFITS,  WHETHER  IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR  OTHER TORTIOUS ACTION, ARISING OUT OF OR
 * IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 *                                           Anthony Mallet on Wed May 30 2018
 */
#pragma require "openrobots2-idl >= 2.0"

#include "or/pose/pose_estimator.gen"
#include "or/robot/uav.gen"
#include "or/robot/rotorcraft.gen"

component uavatt {
  version		"1.3";
  email			"openrobots@laas.fr";
  lang			"c";
  require		"genom3 >= 2.99.41";
  codels-require	"eigen3";
  codels-require	"proxsuite >= 0.6.4";

  provides	or_uav;
  uses		or_rotorcraft;
  uses		or_pose_estimator;

  port in	or_pose_estimator::state state;

  exception e_input, e_geom;
  exception e_sys { short code; string<128> what; };
  exception e_inval { string<128> what; };

  native log_s;

  enum attitude_control_mode {
    full_attitude,
    tilt_prioritized
  };

  ids {
    /* geometry */
    struct body_s {
      double G[6 * or_rotorcraft::max_rotors]; /* velocities² to wrench */
      double iG[or_rotorcraft::max_rotors * 6]; /* wrench to velocities² */

      double mass;

      unsigned short rotors;
      double wmin, wmax;
      double wrench_min[6], wrench_max[6];

      boolean init; /* true when data was set */
    } body;

    /* controller */
    struct servo_s {
      struct satweight_s {
        boolean enable;
        double thrust, lateral, tilt, head;
      } satweight;

      struct gain_s {
        double Kqxy, Kqz, Kwxy, Kwz;
      } gain;

      attitude_control_mode att_mode;

      double ramp;
      double scale;

      struct emerg_s {
        double dq, dw;
      } emerg;
    } servo;

    or_uav::input reference;

    /* logging */
    log_s log;
  };

  attribute set_mass(in body.mass);

  attribute set_geom(
    in body.mass =: "Mass (kg)",
    in body.G =: "Mapping from propellers velocity² to wrench") {
    doc "Set mass and allocation matrix";
    doc "";
    doc "This is a generic function for setting the geometric parameters of";
    doc "the controlled robot. See <<set_gtmrp_geom>> for a specialized";
    doc "function for tilted multi rotors robots.";

    codel uavatt_set_geom(inout body);
    codel uavatt_reset_controller(in body, in servo);
  };

  function set_gtmrp_geom(
    in unsigned short rotors = 4 : "Number of rotors",
    in double cx = 0. : "X position of center of rotors wrt center of mass (m)",
    in double cy = 0. : "Y position of center of rotors wrt center of mass (m)",
    in double cz = 0. : "Z position of center of rotors wrt center of mass (m)",
    in double armlen = 0.23 : "Distance of rotors to the center (m)",
    in double mass = 1. : "Total mass (kg)",
    in double rx = 0 : "X rotation of the first rotor thrust vector (deg)",
    in double ry = 0 : "Y rotation of the first rotor thrust vector (deg)",
    in short rz = -1 : "Z spin direction of the first rotor (1: ccw, -1: cw)",
    in double cf = 6.5e-4 : "Propeller thrust coefficient (N.s²)",
    in double ct = 1e-5 : "Propeller torque coefficient (N.m.s²)") {
    doc "Compute allocation matrix and inertia tensor for tilt rotors robots.";
    doc "";
    doc "Generically Tilted Multi-Rotor Platforms (GTMRP) are made of a set";
    doc "of rotors evenly distributed in a horizontal plane. The rotors are";
    doc "tilted around an axis lying in the plane, all by the same angle but";
    doc "with alternating signs on the X axis. The spinning direction is also";
    doc "alternating.";
    doc "";
    doc "This function is a specialization of the more generic <<set_geom>>.";

    codel uavatt_set_gtmrp_geom(in rotors, in cx, in cy, in cz, in armlen,
                                in mass, in rx, in ry, in rz, in cf, in ct,
                                out body);
    codel uavatt_reset_controller(in body, in servo);

    throw e_inval;
  };

  attribute set_saturation_weights(in servo.satweight = {
      .enable = TRUE : "Enable wrench saturation",
      .thrust = 10. : "Relative weight for thrust saturation",
      .lateral = 1. : "Relative weight for lateral forces saturation",
      .tilt = 100. : "Relative weight for roll and pitch saturation",
      .head = 1. : "Relative weight for heading saturation"
    }) {
    doc "Select relative priority for thrust, titlting and heading.";
    doc "";
    doc "When the controller reaches the physical wrench limits of the";
    doc "platform (see <<set_wlimit>>), an optimisation algorithm finds";
    doc "the closest feasible wrench that keeps the same thurst direction";
    doc "as well as the same roll and pitch torque direction.";
    doc "";
    doc "The weights specified here allow to give higher priority to certain";
    doc "components of the wrench and let them be less affected by the";
    doc "optimization algorithm. Higher weight means that the corresponding";
    doc "component is less likely to change in intensity.";

    validate uavatt_set_satweights(in satweight);
    codel uavatt_reset_controller(in body, in servo);

    throw e_inval;
  };

  attribute set_control_mode(
    in servo.att_mode = tilt_prioritized: "Attitude tracking control law") {
    doc "Select attitude tracking control law.";
    doc "";
    doc "With `tilt_prioritized`, attitude error is split into a";
    doc "reduced attitude error, which describes the misalignment of";
    doc "the thrust direction, and a yaw error, which describes the";
    doc "heading orientation error. This is the default.";
    doc "";
    doc "With `full_attitude`, a conventional controller, based on the full";
    doc "attitude error, is used.";
  };
  attribute set_wlimit(
    in body.wmin =: "Minimum propeller velocity",
    in body.wmax =: "Maximum propeller velocity") {
    codel uavatt_set_wlimit(inout body);
    codel uavatt_reset_controller(in body, in servo);
  };

  attribute set_servo_gain(in servo.gain);

  attribute set_emerg(in servo.emerg = {
    5 : "Attitude uncertainty threshold (deg)",
    20 : "Angular velocity uncertainty threshold (deg/s)" }) {
    doc "Set thresholds for emergency hovering.";
    doc "";
    doc "`dq` and `dw` represent the maximum uncertainty tolerated in the";
    doc "input <<state>> for attitude (`dq`) and angular velocity (`dw`),";
    doc "measured as 3 times the standard deviation.";
    doc "By default, `dq` is 5⁰ and `dw` 20⁰/s. Beyond that threshold, an";
    doc "emergency hovering is started.";
    doc "";
    doc "The emergency hovering uses no feedback, as the state is considered";
    doc "invalid. Thus, stability is not guaranteed ...";

    validate uavatt_set_emerg(local inout emerg);
  };

  attribute get_servo_parameters(out servo);
  attribute get_geom_parameters(out body);
  attribute get_reference(out reference);


  /* --- control ----------------------------------------------------------- */

  const unsigned short control_period_ms = 1;

  task main {
    period control_period_ms ms;

    codel<start> uavatt_main_start(out ::ids)
      yield control;

    codel<control> uavatt_main_control(in body, inout servo,
                                       in state, inout reference,
                                       inout log, out rotor_input)
      yield measure;
    codel<measure> uavatt_main_measure(in body,
                                       in state, in rotor_measure,
                                       out wrench_measure)
      yield pause::control;

    codel<stop> uavatt_main_stop(out rotor_input)
      yield ether;
  };

  activity servo() {
    doc		"Track a desired attitude";
    task	main;

    validate uavatt_check_geom(in body.init);

    codel<start> uavatt_servo_loop(in uav_input, out reference)
      yield pause::start, ether;
    codel<stop> uavatt_servo_stop(ids out reference)
      yield ether;

    throw e_input, e_geom;
  };

  function set_state(
    in or::rb3d::force thrust =: "Thrust",
    in or::t3d::att att =: "Orientation",
    in or::t3d::avel avel =: "Angular velocity",
    in or::t3d::aacc aacc =: "Angular acceleration") {
    doc		"Set the desired state";

    validate uavatt_check_geom(in body.init);

    codel uavatt_set_state(in thrust, in att, in avel, in aacc,
                           ids out reference);

    interrupt servo;
    throw e_geom;
  };

  function coast() {
    doc "Stop tracking a desired attitude";
    doc "";
    doc "This sets the reference thrust to a purely vertical";
    doc "gravity-compensation vector with 0 angular velocity";
    doc "and acceleration, resulting in a coasting state.";
    doc "Any dynamic reference tracking is cancelled (see <<servo>>).";

    codel uavatt_coast(ids out reference);

    interrupt servo;
  };


  /* --- logging ----------------------------------------------------------- */

  function log(in string<64> path = "/tmp/uavatt.log": "Log file name",
               in unsigned long decimation = 1: "Reduced logging frequency") {
    doc		"Log controller data";

    codel uavatt_log(in path, in decimation, inout log);

    throw e_sys;
  };

  function log_stop() {
    doc		"Stop logging";

    codel uavatt_log_stop(out log);
  };

  function log_info(out unsigned long miss = :"Missed log entries",
                    out unsigned long total = :"Total log entries") {
    doc		"Show missed log entries";

    codel uavatt_log_info(in log, out miss, out total);
  };
};
