diff options
author | James Goppert <james.goppert@gmail.com> | 2013-07-28 22:27:05 -0400 |
---|---|---|
committer | James Goppert <james.goppert@gmail.com> | 2013-07-28 22:27:05 -0400 |
commit | dc542b2a4818bfc1c3c1cd24c5ee513d5b5ea0c4 (patch) | |
tree | a3a3715da3a9bac871241df71a79d7687b862e3f /src/modules | |
parent | 1980d9dd63e29390f7c3ba9b31be576c07706f73 (diff) | |
download | px4-firmware-dc542b2a4818bfc1c3c1cd24c5ee513d5b5ea0c4.tar.gz px4-firmware-dc542b2a4818bfc1c3c1cd24c5ee513d5b5ea0c4.tar.bz2 px4-firmware-dc542b2a4818bfc1c3c1cd24c5ee513d5b5ea0c4.zip |
Segway stabilized.
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/controllib/uorb/blocks.cpp | 37 | ||||
-rw-r--r-- | src/modules/controllib/uorb/blocks.hpp | 23 | ||||
-rw-r--r-- | src/modules/fixedwing_backside/fixedwing.cpp | 37 | ||||
-rw-r--r-- | src/modules/fixedwing_backside/fixedwing.hpp | 23 | ||||
-rw-r--r-- | src/modules/segway/BlockSegwayController.cpp | 19 | ||||
-rw-r--r-- | src/modules/segway/BlockSegwayController.hpp | 13 | ||||
-rw-r--r-- | src/modules/segway/params.c | 72 | ||||
-rw-r--r-- | src/modules/segway/segway_main.cpp | 22 |
8 files changed, 88 insertions, 158 deletions
diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index 6e5ade519..448a42a99 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -42,6 +42,43 @@ namespace control { +BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _xtYawLimit(this, "XT2YAW"), + _xt2Yaw(this, "XT2YAW"), + _psiCmd(0) +{ +} + +BlockWaypointGuidance::~BlockWaypointGuidance() {}; + +void BlockWaypointGuidance::update(vehicle_global_position_s &pos, + vehicle_attitude_s &att, + vehicle_global_position_setpoint_s &posCmd, + vehicle_global_position_setpoint_s &lastPosCmd) +{ + + // heading to waypoint + float psiTrack = get_bearing_to_next_waypoint( + (double)pos.lat / (double)1e7d, + (double)pos.lon / (double)1e7d, + (double)posCmd.lat / (double)1e7d, + (double)posCmd.lon / (double)1e7d); + + // cross track + struct crosstrack_error_s xtrackError; + get_distance_to_line(&xtrackError, + (double)pos.lat / (double)1e7d, + (double)pos.lon / (double)1e7d, + (double)lastPosCmd.lat / (double)1e7d, + (double)lastPosCmd.lon / (double)1e7d, + (double)posCmd.lat / (double)1e7d, + (double)posCmd.lon / (double)1e7d); + + _psiCmd = _wrap_2pi(psiTrack - + _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance))); +} + BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) : SuperBlock(parent, name), // subscriptions diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 54f31735c..9c0720aa5 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -57,6 +57,10 @@ #include <drivers/drv_hrt.h> #include <poll.h> +extern "C" { +#include <systemlib/geo/geo.h> +} + #include "../blocks.hpp" #include "UOrbSubscription.hpp" #include "UOrbPublication.hpp" @@ -65,6 +69,25 @@ namespace control { /** + * Waypoint Guidance block + */ +class __EXPORT BlockWaypointGuidance : public SuperBlock +{ +private: + BlockLimitSym _xtYawLimit; + BlockP _xt2Yaw; + float _psiCmd; +public: + BlockWaypointGuidance(SuperBlock *parent, const char *name); + virtual ~BlockWaypointGuidance(); + void update(vehicle_global_position_s &pos, + vehicle_attitude_s &att, + vehicle_global_position_setpoint_s &posCmd, + vehicle_global_position_setpoint_s &lastPosCmd); + float getPsiCmd() { return _psiCmd; } +}; + +/** * UorbEnabledAutopilot */ class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index d5dc746e0..f655a13bd 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -86,43 +86,6 @@ void BlockStabilization::update(float pCmd, float qCmd, float rCmd, _yawDamper.update(rCmd, r, outputScale); } -BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - _xtYawLimit(this, "XT2YAW"), - _xt2Yaw(this, "XT2YAW"), - _psiCmd(0) -{ -} - -BlockWaypointGuidance::~BlockWaypointGuidance() {}; - -void BlockWaypointGuidance::update(vehicle_global_position_s &pos, - vehicle_attitude_s &att, - vehicle_global_position_setpoint_s &posCmd, - vehicle_global_position_setpoint_s &lastPosCmd) -{ - - // heading to waypoint - float psiTrack = get_bearing_to_next_waypoint( - (double)pos.lat / (double)1e7d, - (double)pos.lon / (double)1e7d, - (double)posCmd.lat / (double)1e7d, - (double)posCmd.lon / (double)1e7d); - - // cross track - struct crosstrack_error_s xtrackError; - get_distance_to_line(&xtrackError, - (double)pos.lat / (double)1e7d, - (double)pos.lon / (double)1e7d, - (double)lastPosCmd.lat / (double)1e7d, - (double)lastPosCmd.lon / (double)1e7d, - (double)posCmd.lat / (double)1e7d, - (double)posCmd.lon / (double)1e7d); - - _psiCmd = _wrap_2pi(psiTrack - - _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance))); -} - BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) : BlockUorbEnabledAutopilot(parent, name), _stabilization(this, ""), // no name needed, already unique diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp index eb5d38381..3876e4630 100644 --- a/src/modules/fixedwing_backside/fixedwing.hpp +++ b/src/modules/fixedwing_backside/fixedwing.hpp @@ -42,10 +42,6 @@ #include <controllib/blocks.hpp> #include <controllib/uorb/blocks.hpp> -extern "C" { -#include <systemlib/geo/geo.h> -} - namespace control { @@ -232,25 +228,6 @@ public: */ /** - * Waypoint Guidance block - */ -class __EXPORT BlockWaypointGuidance : public SuperBlock -{ -private: - BlockLimitSym _xtYawLimit; - BlockP _xt2Yaw; - float _psiCmd; -public: - BlockWaypointGuidance(SuperBlock *parent, const char *name); - virtual ~BlockWaypointGuidance(); - void update(vehicle_global_position_s &pos, - vehicle_attitude_s &att, - vehicle_global_position_setpoint_s &posCmd, - vehicle_global_position_setpoint_s &lastPosCmd); - float getPsiCmd() { return _psiCmd; } -}; - -/** * Multi-mode Autopilot */ class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index b7a0bbbcc..682758a14 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -30,23 +30,24 @@ void BlockSegwayController::update() { // update guidance } - // XXX handle STABILIZED (loiter on spot) as well - // once the system switches from manual or auto to stabilized - // the setpoint should update to loitering around this position + // compute speed command + float spdCmd = -theta2spd.update(_att.pitch) - q2spd.update(_att.pitchspeed); // handle autopilot modes if (_status.state_machine == SYSTEM_STATE_AUTO || _status.state_machine == SYSTEM_STATE_STABILIZED) { - - float spdCmd = phi2spd.update(_att.phi); - - // output _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { - _actuators.control[0] = _manual.roll; - _actuators.control[1] = _manual.roll; + if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + _actuators.control[CH_LEFT] = _manual.throttle; + _actuators.control[CH_RIGHT] = _manual.pitch; + + } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + _actuators.control[0] = spdCmd; + _actuators.control[1] = spdCmd; + } } // update all publications diff --git a/src/modules/segway/BlockSegwayController.hpp b/src/modules/segway/BlockSegwayController.hpp index b16d38338..e2faa4916 100644 --- a/src/modules/segway/BlockSegwayController.hpp +++ b/src/modules/segway/BlockSegwayController.hpp @@ -8,11 +8,20 @@ class BlockSegwayController : public control::BlockUorbEnabledAutopilot { public: BlockSegwayController() : BlockUorbEnabledAutopilot(NULL,"SEG"), - phi2spd(this, "PHI2SPD") + theta2spd(this, "THETA2SPD"), + q2spd(this, "Q2SPD"), + _attPoll(), + _timeStamp(0) { + _attPoll.fd = _att.getHandle(); + _attPoll.events = POLLIN; } void update(); private: - BlockP phi2spd; + enum {CH_LEFT, CH_RIGHT}; + BlockPI theta2spd; + BlockP q2spd; + struct pollfd _attPoll; + uint64_t _timeStamp; }; diff --git a/src/modules/segway/params.c b/src/modules/segway/params.c index db30af416..1669785d3 100644 --- a/src/modules/segway/params.c +++ b/src/modules/segway/params.c @@ -1,72 +1,8 @@ #include <systemlib/param/param.h> -// currently tuned for easystar from arkhangar in HIL -//https://github.com/arktools/arkhangar - // 16 is max name length +PARAM_DEFINE_FLOAT(SEG_THETA2SPD_P, 10.0f); // pitch to speed +PARAM_DEFINE_FLOAT(SEG_THETA2SPD_I, 0.0f); // pitch integral to speed +PARAM_DEFINE_FLOAT(SEG_THETA2SPD_I_MAX, 0.0f); // integral limiter +PARAM_DEFINE_FLOAT(SEG_Q2SPD, 1.0f); // pitch rate to speed -// gyro low pass filter -PARAM_DEFINE_FLOAT(FWB_P_LP, 300.0f); // roll rate low pass cut freq -PARAM_DEFINE_FLOAT(FWB_Q_LP, 300.0f); // pitch rate low pass cut freq -PARAM_DEFINE_FLOAT(FWB_R_LP, 300.0f); // yaw rate low pass cut freq - -// yaw washout -PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass - -// stabilization mode -PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.3f); // roll rate 2 aileron -PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator -PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder - -// psi -> phi -> p -PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f); // heading 2 roll -PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f); // roll to roll rate -PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.3f); // roll limit, 28 deg - -// velocity -> theta -PARAM_DEFINE_FLOAT(FWB_V2THE_P, 1.0f); // velocity to pitch angle PID, prop gain -PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f); // integral gain -PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f); // derivative gain -PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f); // derivative low-pass -PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f); // integrator wind up guard -PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f); // the max commanded pitch angle -PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f); // the min commanded pitch angle - - -// theta -> q -PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f); // pitch angle to pitch-rate PID -PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f); -PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f); -PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f); -PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f); - -// h -> thr -PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f); // altitude to throttle PID -PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.0f); -PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.0f); -PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 0.0f); -PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f); - -// crosstrack -PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // cross-track to yaw angle limit 90 deg -PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.005f); // cross-track to yaw angle gain - -// speed command -PARAM_DEFINE_FLOAT(FWB_V_MIN, 10.0f); // minimum commanded velocity -PARAM_DEFINE_FLOAT(FWB_V_CMD, 12.0f); // commanded velocity -PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity - -// rate of climb -// this is what rate of climb is commanded (in m/s) -// when the pitch stick is fully defelcted in simple mode -PARAM_DEFINE_FLOAT(FWB_CR_MAX, 1.0f); - -// climb rate -> thr -PARAM_DEFINE_FLOAT(FWB_CR2THR_P, 0.01f); // rate of climb to throttle PID -PARAM_DEFINE_FLOAT(FWB_CR2THR_I, 0.0f); -PARAM_DEFINE_FLOAT(FWB_CR2THR_D, 0.0f); -PARAM_DEFINE_FLOAT(FWB_CR2THR_D_LP, 0.0f); -PARAM_DEFINE_FLOAT(FWB_CR2THR_I_MAX, 0.0f); - -PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1) -PARAM_DEFINE_FLOAT(FWB_TRIM_V, 12.0f); // trim velocity, m/s diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp index 8be1cc7aa..061fbf9b9 100644 --- a/src/modules/segway/segway_main.cpp +++ b/src/modules/segway/segway_main.cpp @@ -64,12 +64,7 @@ extern "C" __EXPORT int segway_main(int argc, char *argv[]); /** * Mainloop of deamon. */ -int control_demo_thread_main(int argc, char *argv[]); - -/** - * Test function - */ -void test(); +int segway_thread_main(int argc, char *argv[]); /** * Print the correct usage. @@ -114,16 +109,11 @@ int segway_main(int argc, char *argv[]) SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 5120, - control_demo_thread_main, + segway_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } - if (!strcmp(argv[1], "test")) { - test(); - exit(0); - } - if (!strcmp(argv[1], "stop")) { thread_should_exit = true; exit(0); @@ -144,7 +134,7 @@ int segway_main(int argc, char *argv[]) exit(1); } -int control_demo_thread_main(int argc, char *argv[]) +int segway_thread_main(int argc, char *argv[]) { warnx("starting"); @@ -165,9 +155,3 @@ int control_demo_thread_main(int argc, char *argv[]) return 0; } - -void test() -{ - warnx("beginning control lib test"); - control::basicBlocksTest(); -} |