Skip to content

Commit

Permalink
Mission handler (#331)
Browse files Browse the repository at this point in the history 
* Use X Y Z instead of 0, 1, 2

* Use default origin location in constants.hpp

* Style

* Use Z instead of 2

* Style

* Fix include

* Fix doc

* Use X Y Z instead of 0, 1, 2

* Rename variables

* Use origin from INS instead of config

* Set origin as protected

* Moved Waypoint::mavlink_stream as function input to Waypoint::send() to get rid of references in waypoint class

* Removed position estimation from waypoint class

* Removed unnecessary update function from waypoint class

* Removed functions from mission planner, gave control of hold position to mission planner handler

* Update constructor of new classes in lequad

* Fixed segmentation fault bugs

* removed waypoint_hold_coordinates

* Fixed compiler errors from ins merge

* Fixed debug printing formatting

* Added debug printint on internal state change

* Added takeoff notification

* Changed set heading function on goal

* Removed unnecessary heading

* Change waypoint frame in function to the local frame, not Waypoint::frame_

* Added hold waypoint set for navigation

* Fixed incorrect function parameters

* Added waiting at waypoint check to nav plan init

* Added waiting at waypoint sets during waypoint changes

* Moved dubin state machine to navigation

* Moved dubin state machine call to navigation update function

* Added debug logs for dubin state change

* Added set goal function in navigation

* Added check for when to autocontinue

* Fixed compiler errors

* Moved set waiting at waypoint until the drone was moving towards the next one

* Added goal waypoint set after successful takeoff

* Fixed issue where goal was not being updated when setting the new waypoint

* Removed unnecessary update current waypoint

* Removed nav plan init

* Fixed nav plan active update in mission planner update

* Added dubin init when switching from manual control to navigation

* Changed method that navigation::internal_state is set

* Removed duplicate print

* Added comments and removed dubin init

* Fixed output message

* Removed unnecessary else condition

* Fixed debug message output

* Moved all dubin changes to navigation so that the dubin structure will be reinitialized if there is a change with the goal waypoint

* Fixed comment

* Moved planning functions to mission planner, fixed print issues in navigating handler

* Cleaned up code

* Cleaned up landing code

* Cleaned up navigating handler a bit

* Moved mission planner classes to control folder

* Moved waypoint to control folder

* Critical handler uses navigation config instead of hardcoded values

* Fixed segfault with mission planner constructor

* Readded the reset hold waypoint on clear waypoint list

* Added function comment

* Removed commented out function declaration

* Modified waypoint structure to include just params

* Changed mission handler interface

* Changed hold position handler

* Renamed mission planner handler to mission handler

* Modified state machine and moved navigation enum to mission planner

* Fixed various mistakes. Added on ground handler

* Changed landing handler

* Fixed type

* Added takeoff handler

* Modified goal

* Fixed mission handler registry

* Removed declaration of unnecessary handlers

* Added navigating handler

* Started to get rid of nav_plan_active

* Started to remove old NAV_ enum

* Modified navigation comments

* Made mission handlers responsible for setting Navigation::waiting_at_waypoint

* Changed navigation code to check only if we are waiting at a waypoint

* Removed nav plan active from if statement and replaced with waiting at waypoint

* Removed setters

* Fixed constructor

* Added mission handler registry

* Mission planner state machine no longer checks for default handlers

* Added internal state control inside the mission handlers

* Fixed compiler errors, contains segfault

* Fixed segfault

* Made the critical handler and mission planner state machine make more sense

* Added checks for hold waypoint finished

* Made waypoints const

* Made waypoints const

* Changed goal waypoints to copies instead of references

* Added get waypoint with no waypoint list case

* Removed unnecessary comment

* Moved mission items to new folder and adds mission handler internal state function

* Renamed insert mission waypoint

* Added check for inputted waypoint

* Changed waypoint::heading() and waypoint::radius() to return success and input the waypoint

* Changed enum for on ground mav command

* Added critical landing and critical navigating handlers

* Changed the takeoff is_finished() function

* Added require takeoff

* Fixed throttle auto takeoff

* Rearranged variables of onboard waypoints

* Moved settings in navigation to appropriate classes

* Fixed compiler errors

* Removed dist2vel_gain

* Removed nav soft zone size

* removed Mission_handler::is_finished and replaced with Mission_handler::handle return status code

* Changed handlers to be templates

* Renamed handle to update

* Added commands to handlers

* Fixed critical handler template and lequad handler instantiation, waiting on controllers

* Cascade controller: added interfaces for controller levels

- Added following interfaces:
  + iattitude_controll
  + inavigation_controller
  + iposition_controller
  + irate_controller
  + itorque_controller
  + ixyposition_zvel_controller

- Added base_command_t to control/control_command.h

- Added local_velocity_t to util/coord_conventions.hpp

* Cascade controller: added ivelocity_controller

* Coord_conventions: fixed bug (missing ';')

* Cascade_controller: made argument of set_xxx_command in controller interfaces (inavigation, iposition, etc. ) const

* Cascade_controller: change return of set_xxx_command in interfaces to bool

This allows the controllers to accept or reject a command

* Cascade_controller: change return statement of set_xyposition_zvel_command from void to bool

* Cascade controller: Added implementation from torque to attitude level

* Cascade controller: added torque_controller for quadrotor cross configuration

* Fixed compiler issues for handler templates

* Moved waypoint to mission folder

* Cascade Controller: changed joystick.attitude_scale_Z to avoid instability in yaw

* Cascade_controller: added ivelocity_yaw_controller interface

* Cascade controller: adapted velocity controller for cascade; bug fix in torque_controllers

* Cascade controller: bug fix in velocity_controller_copter; added velocity_controller_copter to Cascade_controller

* Cascade Controller: moved remaining servos_mixes

* Cascade controller: pid_controller: added pid_controller_apply_config

* Cascade controller: Controllers: changed config to const as constructor arguments; Velocity_controller_copter: added attitude default config to default config

* Cascade controller: added position controller implementing iposition_ctrl, inavigation_ctrl and ixyposition_zvel_ctrl

* Cascade Controller & Mission handler: fused the two

* Added forgotten returns on functions

* Cascade_controller: implemented set_xxx_command(...) in controller interfaces to avoid compilation warnings in mission_handlers

* Revert "Cascade_controller: implemented set_xxx_command(...) in controller interfaces to avoid compilation warnings in mission_handlers"

This reverts commit 646725a.

* Added forgotten returns on functions

Conflicts:
	mission/mission_handler_landing.cpp

* Added ixyposition_zvel_controller interface

* Mission Handler working with stabilisation copter (yaw still strange)

* LEQuad: Moved init_* calls from constructor to dedicated init() function

This allows to override init_* functions from child classes

* LEQuad_dronedome: moved call of gps_mocap_.init() and ahrs_ekf_mocap_.init() to init()

* Waypoints: added heading for MAV_CMD_NAV_PATHPLANNING and ;MAV_CMD_NAV_SPLINE_WAYPOINT

* Waypoint_handler: changed callbacks to listen to messages for target_component 50 for compatibility with DroneKit

* Position_controller: moved kp_yaw to config

* Position_controller: bug fix for avr and stm32

* Position_controller: changed yaw to only be set when in cruise mode

* Fixed two bugs

* Takeoff handler: bug fix ported from dev/waypoint_handler_rewrite

Changed takeoff altitude

* Fixed hold position waypoint initialization

* Added yaw to hold position

* Added auto takeoff from ground when in hold position

* Fixed disarmed setting and autotakeoff from ground

* Added manual control handler

* Added necessary file to rules

* Added manual control to Mission_planner:state_machine()

* Removed heading command from standby mode

* fixed callback functions of mission planning

* Added mission enum to switch case

* Removed yaw control

* Removed Mission_planner::is_arrived()

* Removed empty cpp files

* Remove forward declaration of Mission_planner

* Rename boolean to is_XXX

* Remove unused state member in Waypoint_handler

* Add const

* navigation is const ref in waypoint_handler

* Remove camelCase

* Add const keywords

* Renamed callbacks to mavlink commands to match command name in waypoint_handler

* Added home waypoint

* Remove dubious piece of code and add comment

* Removed mission planner from handler inputs

* Changed Mission_handler::update() return to an enum

* Add const keywords in Waypoint handler

* Rename callback methods to match mavlink command

* Added time recording for waypoints list received

* On ground handler is ready to accept attitude controller

* Fixed order of landing handler update function to prevent accidental auto-takeoff after rearming

* Use Xxx_I for controller interfaces

* fix

* Removed navigation update task

* Added go home callback

* Removed reset hold todo on clear waypoint list

* Added check in critical handler to not run when not in auto or continuous

* Reduced checks in waypoint communication

* Renamed confusing function name

* Renamed Torque_controller* to Servos_mix
Removed Servos_mix from controller stack: Rate_controller contains a Servos_mix rather than inheriting

* Removed waypoint initialization

* Moved config variables form navigation to mission/*

* Fixed receiving 0 waypoint count case

* Rate_controller + Position_controller: bug fix

* added home waypoint sending (does not send on construction)

* Removed waiting at waypoint

* Removed navigation.start_wpt_time_

* Removed navigation from several modules

* Removed navigation

* Added Navigation_directto; Removed pid controller switching from position_controller

* Added rate control to on ground handler

* Fixed compiler errors for avr

* Do not track .d files

* Fix filenames in rules_common.mk

* Manual_control: changed get_attitude_command

* Navigation_directto: commented changing of pid out for avr32

* Joystick: changed default config for yaw scale to be adapted for get_attitude_command

* add missing init

* Fix merge conflicts

* Fixed incorrect altitude in land callback

* Moved descent to ground altitude to config

* Added check if drone is below desc2gnd altitude

* Fixed dronedome takeoff altitutude

* Position_controller: fixed bug: setting to xyz mode on update_cascade

* Made range for switch to desc2gnd a config param

* simplify if statement according to julien command

* clean useless extern C

* Re-enable yaw alignment during navigation
  • Loading branch information
@mdouglas90 @jlecoeur
mdouglas90 authored and jlecoeur committed Oct 14, 2016
1 parent f6f1d6e commit 22f1426
Show file tree
Hide file tree
Showing 96 changed files with 7,648 additions and 5,199 deletions.
2,084 changes: 334 additions & 1,750 deletions communication/mavlink_waypoint_handler.cpp
View file

Large diffs are not rendered by default.

324 changes: 91 additions & 233 deletions communication/mavlink_waypoint_handler.hpp
View file

Large diffs are not rendered by default.

22 changes: 12 additions & 10 deletions communication/remote.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -512,17 +512,19 @@ void remote_get_attitude_command_absolute_yaw(const remote_t* remote, attitude_c
}


void remote_get_attitude_command(const remote_t* remote, const float ki_yaw, attitude_command_t* command, float scale)
Attitude_controller_I::att_command_t remote_get_attitude_command(const remote_t* remote, quat_t current_attitude)
{
command->rpy[ROLL] = scale * remote_get_roll(remote);
command->rpy[PITCH] = scale * remote_get_pitch(remote);
command->rpy[YAW] += ki_yaw * scale * remote_get_yaw(remote);

aero_attitude_t attitude;
attitude.rpy[ROLL] = command->rpy[ROLL];
attitude.rpy[PITCH] = command->rpy[PITCH];
attitude.rpy[YAW] = command->rpy[YAW];
command->quat = coord_conventions_quaternion_from_aero(attitude);
// TODO: work directly on quaternion
// TODO: add scale as config member
float rpy[3];
rpy[YAW] = coord_conventions_get_yaw(current_attitude) + 0.5f * remote_get_yaw(remote);
rpy[ROLL] = remote_get_roll(remote);
rpy[PITCH] = remote_get_pitch(remote);

Attitude_controller_I::att_command_t command;
command.att = coord_conventions_quaternion_from_rpy(rpy);
command.thrust = remote_get_throttle(remote);
return command;
}


Expand Down
14 changes: 8 additions & 6 deletions communication/remote.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,7 @@
#include "communication/mav_modes.hpp"
#include "drivers/satellite.hpp"
#include "control/stabilisation.hpp"
#include "control/attitude_controller_i.hpp"

extern "C"
{
Expand Down Expand Up @@ -380,14 +381,15 @@ void remote_get_attitude_command_absolute_yaw(const remote_t* remote, attitude_c


/**
* \brief Compute attitude command from the remote (absolute roll and pitch, integrated yaw)
* \brief Compute attitude command from the remote (absolute roll and pitch, relative yaw)
* \details Yaw is relative to current yaw (command.yaw = current.yaw + 0.5 * input.yaw)
*
* \param remote Remote structure (input)
* \param k_yaw Integration factor for yaw (0.02 is ok) (input)
* \param command Attitude command (output)
* \param scale Scale (maximum output / max remote input)
* \param remote Remote structure
* \param current_attitude Current attitude of the vehicle
*
* \return command
*/
void remote_get_attitude_command(const remote_t* remote, const float k_yaw, attitude_command_t* command, float scale);
Attitude_controller_I::att_command_t remote_get_attitude_command(const remote_t* remote, quat_t current_attitude);


/**
Expand Down
3 changes: 0 additions & 3 deletions communication/state.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,8 +75,6 @@ State::State(Mavlink_stream& mavlink_stream, Battery& battery, State::conf_t con
out_of_fence_1 = false;
out_of_fence_2 = false;

nav_plan_active = false;

reset_position = false;

last_heartbeat_msg = time_keeper_get_s();
Expand All @@ -93,7 +91,6 @@ void State::switch_to_active_mode(mav_state_t* mav_state_)

// Tell other modules to reset position and re-compute waypoints
reset_position = true;
nav_plan_active = false;

print_util_dbg_print("Switching to active mode.\r\n");
}
Expand Down
3 changes: 0 additions & 3 deletions communication/state.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,6 @@ class State
bool out_of_fence_1; ///< Flag to tell whether we are out the first fence or not
bool out_of_fence_2; ///< Flag to tell whether we are out the second fence or not

bool nav_plan_active; ///< Flag to tell that a flight plan (min 1 waypoint) is active
bool reset_position; ///< Flag to enable the reset of the position estimation

double last_heartbeat_msg; ///< Time of reception of the last heartbeat message from the ground station
Expand Down Expand Up @@ -249,8 +248,6 @@ class State
bool out_of_fence_1; ///< Flag to tell whether we are out the first fence or not
bool out_of_fence_2; ///< Flag to tell whether we are out the second fence or not

bool nav_plan_active; ///< Flag to tell that a flight plan (min 1 waypoint) is active
bool in_the_air; ///< Flag to tell whether the vehicle is airborne or not
bool reset_position; ///< Flag to enable the reset of the position estimation

double last_heartbeat_msg; ///< Time of reception of the last heartbeat message from the ground station
Expand Down
208 changes: 83 additions & 125 deletions control/attitude_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,8 +35,9 @@
* \author MAV'RIC Team
* \author Julien Lecoeur
*
* \brief A cascaded controller for attitude & rate control.
* \brief A controller for attitude control, to be use within a cascade controller structure
*
* TODO: update this text
* \details It takes a command in attitude (roll/pitch/yaw or quaternion) as
* input, and computes a torque command on roll pitch and yaw.
* The inner PID loop controls the angular speed around the 3 axis. This inner
Expand All @@ -56,174 +57,131 @@
#include "sensing/ahrs.hpp"
#include "util/constants.hpp"
#include "control/control_command.h"
#include "control/attitude_controller_i.hpp"



class Attitude_controller
template<class TRate_controller>
class Attitude_controller : public Attitude_controller_I, public TRate_controller
{
public:

/**
* \brief Control mode (attitude or rate)
* \brief Attitude controller configuration
*/
enum class mode_t
struct conf_t
{
ATTITUDE_RATE = 0, ///< Angles + rate control
RATE = 1, ///< Rate control only
pid_controller_conf_t pid_config[3]; ///< Attitude PID controller for roll, pitch and yaw
typename TRate_controller::conf_t rate_controller_config; ///< Config for rate control which this controller inherits from
};

/**
* \brief Attitude controller configuration
* \brief Attitude controller constructor arguments
*/
struct conf_t
struct args_t
{
mode_t mode; ///< Default mode of controller (rate/attitude)
pid_controller_conf_t rate_pid_config[3]; ///< Angular rate PID controller for roll, pitch and yaw
pid_controller_conf_t angle_pid_config[3]; ///< Attitude PID controller for roll, pitch and yaw
};

const ahrs_t& ahrs;
typename TRate_controller::args_t rate_controller_args;
};

/**
* \brief Constructor
*
* \param ahrs Pointer to the estimated attitude
* \param args Constructor arguments
* \param config Configuration
*/
Attitude_controller(const ahrs_t& ahrs, const attitude_command_t& attitude_command, rate_command_t& rate_command, torque_command_t& torque_command, conf_t config = default_config());
Attitude_controller(args_t args, const conf_t& config = default_config());

void update();
virtual void update();

bool set_attitude_command(const att_command_t& att_command);

inline att_command_t attitude_command(){return att_command_;};

static inline conf_t default_config();

protected:
/*
* \brief calc rate commands based on given attitude command and update underlaying cascade level (TRate)
* \details Sets the internal attitude_command_ to the provided one, without modifiying cascade_command_
* Calls calc_rate_command followed by TRate::update_cascade
* This function should be called from higher level controllers if they provide a command
* \param attitude_command attitude command to be set
*/
void update_cascade(const att_command_t& att_command);

/*
* \brief calc rate commands based on given attitude command
* \details Sets the internal attitude_command_ to the provided one, without modifiying cascade_command_
* This function should be called from higher level controllers if they provide a command
* \param attitude_command attitude command
*/
typename TRate_controller::rate_command_t calc_rate_command(const att_command_t& att_command);

private:
attitude_error_estimator_t attitude_error_estimator_; ///< Attitude error estimator
pid_controller_t rate_pid_[3]; ///< Angular rate PID controller for roll, pitch and yaw
pid_controller_t angle_pid_[3]; ///< Attitude PID controller for roll, pitch and yaw
mode_t mode_; ///< Control mode: Angle (default), or rate
pid_controller_t pid_[3]; ///< Attitude PID controller for roll, pitch and yaw
float dt_s_; ///< The time interval between two updates
float last_update_s_; ///< The time of the last update in s
const ahrs_t& ahrs_; ///< Ref to attitude estimation (input)
const attitude_command_t& attitude_command_; ///< Ref to attitude command (input)
rate_command_t& rate_command_; ///< Ref to rate command (input/output)
torque_command_t& torque_command_; ///< Ref to torque command (output)

/**
* \brief Performs outer stabilisation loop on attitude angles
*/
void update_angles();

/**
* \brief Performs inner stabilisation loop on angular rate
*/
void update_rates();
att_command_t att_command_; ///< Ref to attitude command (input)
};


Attitude_controller::conf_t Attitude_controller::default_config()
template<class TRate_controller>
typename Attitude_controller<TRate_controller>::conf_t Attitude_controller<TRate_controller>::default_config()
{
conf_t conf = {};

conf.mode = mode_t::ATTITUDE_RATE;

// #########################################################################
// ###### RATE CONTROL ###################################################
// #########################################################################
// -----------------------------------------------------------------
// ------ ROLL RATE PID --------------------------------------------
// -----------------------------------------------------------------
conf.rate_pid_config[ROLL] = {};
conf.rate_pid_config[ROLL].p_gain = 0.07f;
conf.rate_pid_config[ROLL].clip_min = -0.9f;
conf.rate_pid_config[ROLL].clip_max = 0.9f;
conf.rate_pid_config[ROLL].integrator = {};
conf.rate_pid_config[ROLL].integrator.gain = 0.125f;
conf.rate_pid_config[ROLL].integrator.clip_pre = 6.0f;
conf.rate_pid_config[ROLL].integrator.clip = 0.3f;
conf.rate_pid_config[ROLL].differentiator = {};
conf.rate_pid_config[ROLL].differentiator.gain = 0.008f;
conf.rate_pid_config[ROLL].differentiator.clip = 0.14f;
conf.rate_pid_config[ROLL].soft_zone_width = 0.0f;
// -----------------------------------------------------------------
// ------ PITCH RATE PID -------------------------------------------
// -----------------------------------------------------------------
conf.rate_pid_config[PITCH] = {};
conf.rate_pid_config[PITCH].p_gain = 0.07f;
conf.rate_pid_config[PITCH].clip_min = -0.9f;
conf.rate_pid_config[PITCH].clip_max = 0.9f;
conf.rate_pid_config[PITCH].integrator = {};
conf.rate_pid_config[PITCH].integrator.gain = 0.125f,
conf.rate_pid_config[PITCH].integrator.clip_pre = 6.0f;
conf.rate_pid_config[PITCH].integrator.clip = 0.3f;
conf.rate_pid_config[PITCH].differentiator = {};
conf.rate_pid_config[PITCH].differentiator.gain = 0.008f;
conf.rate_pid_config[PITCH].differentiator.clip = 0.14f;
conf.rate_pid_config[PITCH].soft_zone_width = 0.0f;
// -----------------------------------------------------------------
// ------ YAW RATE PID ---------------------------------------------
// -----------------------------------------------------------------
conf.rate_pid_config[YAW] = {};
conf.rate_pid_config[YAW].p_gain = 0.3f;
conf.rate_pid_config[YAW].clip_min = -0.3f;
conf.rate_pid_config[YAW].clip_max = 0.3f;
conf.rate_pid_config[YAW].integrator = {};
conf.rate_pid_config[YAW].integrator.gain = 0.075f;
conf.rate_pid_config[YAW].integrator.clip_pre = 1.0f;
conf.rate_pid_config[YAW].integrator.clip = 0.045f;
conf.rate_pid_config[YAW].differentiator = {};
conf.rate_pid_config[YAW].differentiator.gain = 0.0f;
conf.rate_pid_config[YAW].differentiator.clip = 0.0f;
conf.rate_pid_config[YAW].soft_zone_width = 0.0;

// #########################################################################
// ###### ANGLE CONTROL ##################################################
// #########################################################################
// -----------------------------------------------------------------
// ------ ROLL ANGLE PID -------------------------------------------
// -----------------------------------------------------------------
conf.angle_pid_config[ROLL] = {};
conf.angle_pid_config[ROLL].p_gain = 4.0f;
conf.angle_pid_config[ROLL].clip_min = -12.0f;
conf.angle_pid_config[ROLL].clip_max = 12.0f;
conf.angle_pid_config[ROLL].integrator = {};
conf.angle_pid_config[ROLL].integrator.gain = 0.0f;
conf.angle_pid_config[ROLL].integrator.clip_pre = 0.0f;
conf.angle_pid_config[ROLL].integrator.clip = 0.0f;
conf.angle_pid_config[ROLL].differentiator = {};
conf.angle_pid_config[ROLL].differentiator.gain = 0.0f;
conf.angle_pid_config[ROLL].differentiator.clip = 0.0f;
conf.angle_pid_config[ROLL].soft_zone_width = 0.0f;
conf.pid_config[ROLL] = {};
conf.pid_config[ROLL].p_gain = 4.0f;
conf.pid_config[ROLL].clip_min = -12.0f;
conf.pid_config[ROLL].clip_max = 12.0f;
conf.pid_config[ROLL].integrator = {};
conf.pid_config[ROLL].integrator.gain = 0.0f;
conf.pid_config[ROLL].integrator.clip_pre = 0.0f;
conf.pid_config[ROLL].integrator.clip = 0.0f;
conf.pid_config[ROLL].differentiator = {};
conf.pid_config[ROLL].differentiator.gain = 0.0f;
conf.pid_config[ROLL].differentiator.clip = 0.0f;
conf.pid_config[ROLL].soft_zone_width = 0.0f;
// -----------------------------------------------------------------
// ------ PITCH ANGLE PID ------------------------------------------
// -----------------------------------------------------------------
conf.angle_pid_config[PITCH] = {};
conf.angle_pid_config[PITCH].p_gain = 4.0f;
conf.angle_pid_config[PITCH].clip_min = -12.0f;
conf.angle_pid_config[PITCH].clip_max = 12.0f;
conf.angle_pid_config[PITCH].integrator = {};
conf.angle_pid_config[PITCH].integrator.gain = 0.0f;
conf.angle_pid_config[PITCH].integrator.clip_pre = 0.0f;
conf.angle_pid_config[PITCH].integrator.clip = 0.0f;
conf.angle_pid_config[PITCH].differentiator = {};
conf.angle_pid_config[PITCH].differentiator.gain = 0.0f;
conf.angle_pid_config[PITCH].differentiator.clip = 0.0f;
conf.angle_pid_config[PITCH].soft_zone_width = 0.0f;
conf.pid_config[PITCH] = {};
conf.pid_config[PITCH].p_gain = 4.0f;
conf.pid_config[PITCH].clip_min = -12.0f;
conf.pid_config[PITCH].clip_max = 12.0f;
conf.pid_config[PITCH].integrator = {};
conf.pid_config[PITCH].integrator.gain = 0.0f;
conf.pid_config[PITCH].integrator.clip_pre = 0.0f;
conf.pid_config[PITCH].integrator.clip = 0.0f;
conf.pid_config[PITCH].differentiator = {};
conf.pid_config[PITCH].differentiator.gain = 0.0f;
conf.pid_config[PITCH].differentiator.clip = 0.0f;
conf.pid_config[PITCH].soft_zone_width = 0.0f;
// -----------------------------------------------------------------
// ------ YAW ANGLE PID --------------------------------------------
// -----------------------------------------------------------------
conf.angle_pid_config[YAW] = {};
conf.angle_pid_config[YAW].p_gain = 3.0f;
conf.angle_pid_config[YAW].clip_min = -1.5f;
conf.angle_pid_config[YAW].clip_max = 1.5f;
conf.angle_pid_config[YAW].integrator = {};
conf.angle_pid_config[YAW].integrator.gain = 0.0f;
conf.angle_pid_config[YAW].integrator.clip_pre = 0.0f;
conf.angle_pid_config[YAW].integrator.clip = 0.0f;
conf.angle_pid_config[YAW].differentiator = {};
conf.angle_pid_config[YAW].differentiator.gain = 0.0f;
conf.angle_pid_config[YAW].differentiator.clip = 0.0f;
conf.angle_pid_config[YAW].soft_zone_width = 0.0f;
conf.pid_config[YAW] = {};
conf.pid_config[YAW].p_gain = 3.0f;
conf.pid_config[YAW].clip_min = -1.5f;
conf.pid_config[YAW].clip_max = 1.5f;
conf.pid_config[YAW].integrator = {};
conf.pid_config[YAW].integrator.gain = 0.0f;
conf.pid_config[YAW].integrator.clip_pre = 0.0f;
conf.pid_config[YAW].integrator.clip = 0.0f;
conf.pid_config[YAW].differentiator = {};
conf.pid_config[YAW].differentiator.gain = 0.0f;
conf.pid_config[YAW].differentiator.clip = 0.0f;
conf.pid_config[YAW].soft_zone_width = 0.0f;

conf.rate_controller_config = TRate_controller::default_config();

return conf;
};

#include "control/attitude_controller.hxx"

#endif /* ATTITUDE_CONTROLLER_HPP_ */
Loading

0 comments on commit 22f1426

Please sign in to comment.