Skip to content

Commit

Permalink
Merge branch 'fix54' into RC1.0
Browse files Browse the repository at this point in the history
  • Loading branch information
@dallinbriggs
dallinbriggs committed Aug 29, 2017
2 parents 91ad95a + 4ee7877 commit 3cbfbe2
Show file tree
Hide file tree
Showing 24 changed files with 2,101 additions and 1,951 deletions.
9 changes: 9 additions & 0 deletions .astylerc
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
style=break
indent=spaces=2
pad-header
unpad-paren
align-pointer=name
align-reference=name
max-code-length=120
suffix=none
pad-comma
8 changes: 8 additions & 0 deletions fix_code_style.sh
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
#!/bin/bash

CMD="astyle --options=.astylerc"

$CMD rosplane/src/*
$CMD rosplane/include/*
$CMD rosplane_sim/src/*
$CMD rosplane_sim/include/rosplane_sim/*
193 changes: 99 additions & 94 deletions rosplane/include/controller_base.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,121 +18,126 @@
#include <dynamic_reconfigure/server.h>
#include <rosplane/ControllerConfig.h>

namespace rosplane {
namespace rosplane
{

enum class alt_zones {
TakeOff,
Climb,
Descend,
AltitudeHold
enum class alt_zones
{
TAKE_OFF,
CLIMB,
DESCEND,
ALTITUDE_HOLD
};

class controller_base
{
public:
controller_base();
float spin();
controller_base();
float spin();

protected:

struct input_s{
float Ts; /** time step */
float h; /** altitude */
float va; /** airspeed */
float phi; /** roll angle */
float theta; /** pitch angle */
float chi; /** course angle */
float p; /** body frame roll rate */
float q; /** body frame pitch rate */
float r; /** body frame yaw rate */
float Va_c; /** commanded airspeed (m/s) */
float h_c; /** commanded altitude (m) */
float chi_c; /** commanded course (rad) */
float phi_ff; /** feed forward term for orbits (rad) */
};

struct output_s{
float theta_c;
float delta_e;
float phi_c;
float delta_a;
float delta_r;
float delta_t;
alt_zones current_zone;
};

struct params_s {
double alt_hz; /**< altitude hold zone */
double alt_toz; /**< altitude takeoff zone */
double tau;
double c_kp;
double c_kd;
double c_ki;
double r_kp;
double r_kd;
double r_ki;
double p_kp;
double p_kd;
double p_ki;
double p_ff;
double a_p_kp;
double a_p_kd;
double a_p_ki;
double a_t_kp;
double a_t_kd;
double a_t_ki;
double a_kp;
double a_kd;
double a_ki;
double b_kp;
double b_kd;
double b_ki;
double trim_e;
double trim_a;
double trim_r;
double trim_t;
double max_e;
double max_a;
double max_r;
double max_t;
double pwm_rad_e;
double pwm_rad_a;
double pwm_rad_r;
};

virtual void control(const struct params_s &params, const struct input_s &input, struct output_s &output) = 0;
struct input_s
{
float Ts; /** time step */
float h; /** altitude */
float va; /** airspeed */
float phi; /** roll angle */
float theta; /** pitch angle */
float chi; /** course angle */
float p; /** body frame roll rate */
float q; /** body frame pitch rate */
float r; /** body frame yaw rate */
float Va_c; /** commanded airspeed (m/s) */
float h_c; /** commanded altitude (m) */
float chi_c; /** commanded course (rad) */
float phi_ff; /** feed forward term for orbits (rad) */
};

struct output_s
{
float theta_c;
float delta_e;
float phi_c;
float delta_a;
float delta_r;
float delta_t;
alt_zones current_zone;
};

struct params_s
{
double alt_hz; /**< altitude hold zone */
double alt_toz; /**< altitude takeoff zone */
double tau;
double c_kp;
double c_kd;
double c_ki;
double r_kp;
double r_kd;
double r_ki;
double p_kp;
double p_kd;
double p_ki;
double p_ff;
double a_p_kp;
double a_p_kd;
double a_p_ki;
double a_t_kp;
double a_t_kd;
double a_t_ki;
double a_kp;
double a_kd;
double a_ki;
double b_kp;
double b_kd;
double b_ki;
double trim_e;
double trim_a;
double trim_r;
double trim_t;
double max_e;
double max_a;
double max_r;
double max_t;
double pwm_rad_e;
double pwm_rad_a;
double pwm_rad_r;
};

virtual void control(const struct params_s &params, const struct input_s &input, struct output_s &output) = 0;

private:
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Subscriber _vehicle_state_sub;
ros::Subscriber _controller_commands_sub;
ros::Publisher _actuators_pub;
ros::Publisher _internals_pub;
ros::Timer _act_pub_timer;
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Subscriber vehicle_state_sub_;
ros::Subscriber controller_commands_sub_;
ros::Publisher actuators_pub_;
ros::Publisher internals_pub_;
ros::Timer act_pub_timer_;

struct params_s _params; /**< params */
rosplane_msgs::Controller_Commands _controller_commands;
rosplane_msgs::State _vehicle_state;
struct params_s params_; /**< params */
rosplane_msgs::Controller_Commands controller_commands_;
rosplane_msgs::State vehicle_state_;

void vehicle_state_callback(const rosplane_msgs::StateConstPtr& msg);
void controller_commands_callback(const rosplane_msgs::Controller_CommandsConstPtr& msg);
bool _command_recieved;
void vehicle_state_callback(const rosplane_msgs::StateConstPtr &msg);
void controller_commands_callback(const rosplane_msgs::Controller_CommandsConstPtr &msg);
bool command_recieved_;

dynamic_reconfigure::Server<rosplane::ControllerConfig> _server;
dynamic_reconfigure::Server<rosplane::ControllerConfig>::CallbackType _func;
dynamic_reconfigure::Server<rosplane::ControllerConfig> server_;
dynamic_reconfigure::Server<rosplane::ControllerConfig>::CallbackType func_;

void reconfigure_callback(rosplane::ControllerConfig &config, uint32_t level);
void reconfigure_callback(rosplane::ControllerConfig &config, uint32_t level);

/**
/**
* Convert from deflection angle to pwm
*/
void convert_to_pwm(struct output_s &output);
void convert_to_pwm(struct output_s &output);

/**
/**
* Publish the outputs
*/
void actuator_controls_publish(const ros::TimerEvent &);
void actuator_controls_publish(const ros::TimerEvent &);
};
} //end namespace

Expand Down
59 changes: 30 additions & 29 deletions rosplane/include/controller_example.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,49 +3,50 @@

#include "controller_base.h"

namespace rosplane {
namespace rosplane
{

class controller_example : public controller_base
{
public:
controller_example();
controller_example();
private:
virtual void control(const struct params_s &params, const struct input_s &input, struct output_s &output);
alt_zones current_zone;
virtual void control(const struct params_s &params, const struct input_s &input, struct output_s &output);
alt_zones current_zone;

float course_hold(float chi_c, float chi, float phi_ff, float r, const struct params_s &params, float Ts);
float c_error;
float c_integrator;
float course_hold(float chi_c, float chi, float phi_ff, float r, const struct params_s &params, float Ts);
float c_error_;
float c_integrator_;

float roll_hold(float phi_c, float phi, float p, const struct params_s &params, float Ts);
float r_error;
float r_integrator;
float roll_hold(float phi_c, float phi, float p, const struct params_s &params, float Ts);
float r_error_;
float r_integrator;

float pitch_hold(float theta_c, float theta, float q, const struct params_s &params, float Ts);
float p_error;
float p_integrator;
float pitch_hold(float theta_c, float theta, float q, const struct params_s &params, float Ts);
float p_error_;
float p_integrator_;

float airspeed_with_pitch_hold(float Va_c, float Va, const struct params_s &params, float Ts);
float ap_error;
float ap_integrator;
float ap_differentiator;
float airspeed_with_pitch_hold(float Va_c, float Va, const struct params_s &params, float Ts);
float ap_error_;
float ap_integrator_;
float ap_differentiator_;

float airspeed_with_throttle_hold(float Va_c, float Va, const struct params_s &params, float Ts);
float at_error;
float at_integrator;
float at_differentiator;
float airspeed_with_throttle_hold(float Va_c, float Va, const struct params_s &params, float Ts);
float at_error_;
float at_integrator_;
float at_differentiator_;

float altitiude_hold(float h_c, float h, const struct params_s &params, float Ts);
float a_error;
float a_integrator;
float a_differentiator;
float altitiude_hold(float h_c, float h, const struct params_s &params, float Ts);
float a_error_;
float a_integrator_;
float a_differentiator_;

// float cooridinated_turn_hold(float v, const struct params_s &params, float Ts);
// float ct_error;
// float ct_integrator;
// float ct_differentiator;
// float ct_error_;
// float ct_integrator_;
// float ct_differentiator_;

float sat(float value, float up_limit, float low_limit);
float sat(float value, float up_limit, float low_limit);
};
} //end namespace

Expand Down
Loading

0 comments on commit 3cbfbe2

Please sign in to comment.