dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Segway stabilized.

This commit is contained in:
James Goppert 2013-07-28 22:27:05 -04:00
parent 1980d9dd63
commit dc542b2a48
11 changed files with 138 additions and 165 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/drivers/md25/md25.cpp
View File

@ -116,7 +116,8 @@ MD25::MD25(const char *deviceName, int bus,
setMotor2Speed(0);
resetEncoders();
_setMode(MD25::MODE_UNSIGNED_SPEED);
setSpeedRegulation(true);
setSpeedRegulation(false);
setMotorAccel(10);
setTimeout(true);
}
@ -308,6 +309,12 @@ int MD25::setDeviceAddress(uint8_t address)
return OK;
}
int MD25::setMotorAccel(uint8_t accel)
{
return _writeUint8(REG_ACCEL_RATE_RW,
accel);
}
int MD25::setMotor1Speed(float value)
{
return _writeUint8(REG_SPEED1_RW,
@ -461,12 +468,12 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address)
MD25 md25("/dev/md25", bus, address);
// print status
char buf[200];
char buf[400];
md25.status(buf, sizeof(buf));
printf("%s\n", buf);
// setup for test
md25.setSpeedRegulation(true);
md25.setSpeedRegulation(false);
md25.setTimeout(true);
float dt = 0.1;
float speed = 0.2;
@ -568,12 +575,12 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu
MD25 md25("/dev/md25", bus, address);
// print status
char buf[200];
char buf[400];
md25.status(buf, sizeof(buf));
printf("%s\n", buf);
// setup for test
md25.setSpeedRegulation(true);
md25.setSpeedRegulation(false);
md25.setTimeout(true);
float dt = 0.01;
float t_final = 60.0;

13
src/drivers/md25/md25.hpp
View File

@ -212,6 +212,19 @@ public:
*/
int setDeviceAddress(uint8_t address);
/**
* set motor acceleration
* @param accel
* controls motor speed change (1-10)
* accel rate | time for full fwd. to full rev.
* 1 | 6.375 s
* 2 | 1.6 s
* 3 | 0.675 s
* 5(default) | 1.275 s
* 10 | 0.65 s
*/
int setMotorAccel(uint8_t accel);
/**
* set motor 1 speed
* @param normSpeed normalize speed between -1 and 1

27
src/drivers/md25/md25_main.cpp
View File

@ -82,7 +82,7 @@ usage(const char *reason)
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: md25 {start|stop|status|search|test|change_address}\n\n");
fprintf(stderr, "usage: md25 {start|stop|read|status|search|test|change_address}\n\n");
exit(1);
}
@ -184,6 +184,29 @@ int md25_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(argv[1], "read")) {
if (argc < 4) {
printf("usage: md25 read bus address\n");
exit(0);
}
const char *deviceName = "/dev/md25";
uint8_t bus = strtoul(argv[2], nullptr, 0);
uint8_t address = strtoul(argv[3], nullptr, 0);
MD25 md25(deviceName, bus, address);
// print status
char buf[400];
md25.status(buf, sizeof(buf));
printf("%s\n", buf);
exit(0);
}
if (!strcmp(argv[1], "search")) {
if (argc < 3) {
printf("usage: md25 search bus\n");
@ -268,7 +291,7 @@ int md25_thread_main(int argc, char *argv[])
uint8_t address = strtoul(argv[4], nullptr, 0);
// start
MD25 md25("/dev/md25", bus, address);
MD25 md25(deviceName, bus, address);
thread_running = true;

37
src/modules/controllib/uorb/blocks.cpp
View File

@ -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

23
src/modules/controllib/uorb/blocks.hpp
View File

@ -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"
@ -64,6 +68,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
*/

37
src/modules/fixedwing_backside/fixedwing.cpp
View File

@ -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

23
src/modules/fixedwing_backside/fixedwing.hpp
View File

@ -42,10 +42,6 @@
#include <controllib/blocks.hpp>
#include <controllib/uorb/blocks.hpp>
extern "C" {
#include <systemlib/geo/geo.h>
}
namespace control
{
@ -231,25 +227,6 @@ public:
* than frontside at high speeds.
*/
/**
* 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
*/

19
src/modules/segway/BlockSegwayController.cpp
View File

@ -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

13
src/modules/segway/BlockSegwayController.hpp
View File

@ -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;
};

72
src/modules/segway/params.c
View File

@ -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

22
src/modules/segway/segway_main.cpp
View File

@ -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();
}