diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-05-09 15:52:36 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-05-09 15:52:36 +0200 |
commit | 3152dae3dca9f6104e685fc27da0aba7c09ac3ca (patch) | |
tree | 962726341b6b6bc5983dffc35a0a14bb9771f9d3 /src/modules | |
parent | b944962a7352c94dce6d41c5b6c6cb2b2cfec373 (diff) | |
parent | 83ce24072fb881802339e40511e0c36006656840 (diff) | |
download | px4-firmware-3152dae3dca9f6104e685fc27da0aba7c09ac3ca.tar.gz px4-firmware-3152dae3dca9f6104e685fc27da0aba7c09ac3ca.tar.bz2 px4-firmware-3152dae3dca9f6104e685fc27da0aba7c09ac3ca.zip |
Merged with master
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/controllib/fixedwing.cpp | 64 | ||||
-rw-r--r-- | src/modules/controllib/fixedwing.hpp | 12 | ||||
-rw-r--r-- | src/modules/fixedwing_backside/params.c | 17 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.c | 122 |
4 files changed, 59 insertions, 156 deletions
diff --git a/src/modules/controllib/fixedwing.cpp b/src/modules/controllib/fixedwing.cpp index 1cc28b9dd..7be38015c 100644 --- a/src/modules/controllib/fixedwing.cpp +++ b/src/modules/controllib/fixedwing.cpp @@ -56,9 +56,9 @@ BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) : BlockYawDamper::~BlockYawDamper() {}; -void BlockYawDamper::update(float rCmd, float r) +void BlockYawDamper::update(float rCmd, float r, float outputScale) { - _rudder = _r2Rdr.update(rCmd - + _rudder = outputScale*_r2Rdr.update(rCmd - _rWashout.update(_rLowPass.update(r))); } @@ -77,13 +77,13 @@ BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) : BlockStabilization::~BlockStabilization() {}; void BlockStabilization::update(float pCmd, float qCmd, float rCmd, - float p, float q, float r) + float p, float q, float r, float outputScale) { - _aileron = _p2Ail.update( + _aileron = outputScale*_p2Ail.update( pCmd - _pLowPass.update(p)); - _elevator = _q2Elv.update( + _elevator = outputScale*_q2Elv.update( qCmd - _qLowPass.update(q)); - _yawDamper.update(rCmd, r); + _yawDamper.update(rCmd, r, outputScale); } BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) : @@ -156,21 +156,21 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par _theLimit(this, "THE"), _vLimit(this, "V"), - // altitude/roc hold + // altitude/climb rate hold _h2Thr(this, "H2THR"), - _roc2Thr(this, "ROC2THR"), + _cr2Thr(this, "CR2THR"), // guidance block _guide(this, ""), - // block params - _trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */ - _trimElv(this, "TRIM_PITCH", false), /* general pitch trim */ - _trimRdr(this, "TRIM_YAW", false), /* general yaw trim */ - _trimThr(this, "TRIM_THR", true), /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */ + _trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */ + _trimElv(this, "TRIM_PITCH", false), /* general pitch trim */ + _trimRdr(this, "TRIM_YAW", false), /* general yaw trim */ + _trimThr(this, "TRIM_THR"), /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */ + _trimV(this, "TRIM_V"), /* FWB_ specific trim velocity (full name : FWB_TRIM_V) */ _vCmd(this, "V_CMD"), - _rocMax(this, "ROC_MAX"), + _crMax(this, "CR_MAX"), _attPoll(), _lastPosCmd(), _timeStamp(0) @@ -228,7 +228,15 @@ void BlockMultiModeBacksideAutopilot::update() _guide.update(_pos, _att, _posCmd, _lastPosCmd); // calculate velocity, XXX should be airspeed, but using ground speed for now - float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz); + // for the purpose of control we will limit the velocity feedback between + // the min/max velocity + float v = _vLimit.update(sqrtf( + _pos.vx * _pos.vx + + _pos.vy * _pos.vy + + _pos.vz * _pos.vz)); + + // limit velocity command between min/max velocity + float vCmd = _vLimit.update(_vCmd.get()); // altitude hold float dThrottle = _h2Thr.update(_posCmd.altitude - _pos.alt); @@ -240,16 +248,19 @@ void BlockMultiModeBacksideAutopilot::update() // velocity hold // negative sign because nose over to increase speed - float thetaCmd = _theLimit.update(-_v2Theta.update( - _vLimit.update(_vCmd.get()) - v)); + float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); float qCmd = _theta2Q.update(thetaCmd - _att.pitch); // yaw rate cmd float rCmd = 0; // stabilization + float velocityRatio = _trimV.get()/v; + float outputScale = velocityRatio*velocityRatio; + // this term scales the output based on the dynamic pressure change from trim _stabilization.update(pCmd, qCmd, rCmd, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); + _att.rollspeed, _att.pitchspeed, _att.yawspeed, + outputScale); // output _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); @@ -280,13 +291,18 @@ void BlockMultiModeBacksideAutopilot::update() } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { // calculate velocity, XXX should be airspeed, but using ground speed for now - float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz); + // for the purpose of control we will limit the velocity feedback between + // the min/max velocity + float v = _vLimit.update(sqrtf( + _pos.vx * _pos.vx + + _pos.vy * _pos.vy + + _pos.vz * _pos.vz)); // pitch channel -> rate of climb // TODO, might want to put a gain on this, otherwise commanding // from +1 -> -1 m/s for rate of climb - //float dThrottle = _roc2Thr.update( - //_rocMax.get()*_manual.pitch - _pos.vz); + //float dThrottle = _cr2Thr.update( + //_crMax.get()*_manual.pitch - _pos.vz); // roll channel -> bank angle float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax()); @@ -294,8 +310,10 @@ void BlockMultiModeBacksideAutopilot::update() // throttle channel -> velocity // negative sign because nose over to increase speed - float vCmd = _manual.throttle * (_vLimit.getMax() - _vLimit.getMin()) + _vLimit.getMin(); - float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v)); + float vCmd = _vLimit.update(_manual.throttle * + (_vLimit.getMax() - _vLimit.getMin()) + + _vLimit.getMin()); + float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); float qCmd = _theta2Q.update(thetaCmd - _att.pitch); // yaw rate cmd diff --git a/src/modules/controllib/fixedwing.hpp b/src/modules/controllib/fixedwing.hpp index 281cbb4cb..53d0cf893 100644 --- a/src/modules/controllib/fixedwing.hpp +++ b/src/modules/controllib/fixedwing.hpp @@ -193,7 +193,7 @@ public: * good idea to declare a member to store the temporary * variable. */ - void update(float rCmd, float r); + void update(float rCmd, float r, float outputScale = 1.0); /** * Rudder output value accessor @@ -226,7 +226,8 @@ public: BlockStabilization(SuperBlock *parent, const char *name); virtual ~BlockStabilization(); void update(float pCmd, float qCmd, float rCmd, - float p, float q, float r); + float p, float q, float r, + float outputScale = 1.0); float getAileron() { return _aileron; } float getElevator() { return _elevator; } float getRudder() { return _yawDamper.getRudder(); } @@ -310,9 +311,9 @@ private: BlockLimit _theLimit; BlockLimit _vLimit; - // altitude/ roc hold + // altitude/ climb rate hold BlockPID _h2Thr; - BlockPID _roc2Thr; + BlockPID _cr2Thr; // guidance BlockWaypointGuidance _guide; @@ -322,8 +323,9 @@ private: BlockParam<float> _trimElv; BlockParam<float> _trimRdr; BlockParam<float> _trimThr; + BlockParam<float> _trimV; BlockParam<float> _vCmd; - BlockParam<float> _rocMax; + BlockParam<float> _crMax; struct pollfd _attPoll; vehicle_global_position_setpoint_s _lastPosCmd; diff --git a/src/modules/fixedwing_backside/params.c b/src/modules/fixedwing_backside/params.c index 428b779b1..db30af416 100644 --- a/src/modules/fixedwing_backside/params.c +++ b/src/modules/fixedwing_backside/params.c @@ -59,13 +59,14 @@ 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_ROC_MAX, 1.0f); +PARAM_DEFINE_FLOAT(FWB_CR_MAX, 1.0f); -// rate of climb -> thr -PARAM_DEFINE_FLOAT(FWB_ROC2THR_P, 0.01f); // rate of climb to throttle PID -PARAM_DEFINE_FLOAT(FWB_ROC2THR_I, 0.0f); -PARAM_DEFINE_FLOAT(FWB_ROC2THR_D, 0.0f); -PARAM_DEFINE_FLOAT(FWB_ROC2THR_D_LP, 0.0f); -PARAM_DEFINE_FLOAT(FWB_ROC2THR_I_MAX, 0.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_THR, 0.8f); // trim throttle (0,1) +PARAM_DEFINE_FLOAT(FWB_TRIM_V, 12.0f); // trim velocity, m/s diff --git a/src/modules/mavlink/mavlink_receiver.c b/src/modules/mavlink/mavlink_receiver.c index e62e4dcc4..a42612f9e 100644 --- a/src/modules/mavlink/mavlink_receiver.c +++ b/src/modules/mavlink/mavlink_receiver.c @@ -308,82 +308,6 @@ handle_message(mavlink_message_t *msg) uint64_t timestamp = hrt_absolute_time(); - /* TODO, set ground_press/ temp during calib */ - static const float ground_press = 1013.25f; // mbar - static const float ground_tempC = 21.0f; - static const float ground_alt = 0.0f; - static const float T0 = 273.15f; - static const float R = 287.05f; - static const float g = 9.806f; - - if (msg->msgid == MAVLINK_MSG_ID_RAW_IMU) { - - mavlink_raw_imu_t imu; - mavlink_msg_raw_imu_decode(msg, &imu); - - /* packet counter */ - static uint16_t hil_counter = 0; - static uint16_t hil_frames = 0; - static uint64_t old_timestamp = 0; - - /* sensors general */ - hil_sensors.timestamp = imu.time_usec; - - /* hil gyro */ - static const float mrad2rad = 1.0e-3f; - hil_sensors.gyro_counter = hil_counter; - hil_sensors.gyro_raw[0] = imu.xgyro; - hil_sensors.gyro_raw[1] = imu.ygyro; - hil_sensors.gyro_raw[2] = imu.zgyro; - hil_sensors.gyro_rad_s[0] = imu.xgyro * mrad2rad; - hil_sensors.gyro_rad_s[1] = imu.ygyro * mrad2rad; - hil_sensors.gyro_rad_s[2] = imu.zgyro * mrad2rad; - - /* accelerometer */ - hil_sensors.accelerometer_counter = hil_counter; - static const float mg2ms2 = 9.8f / 1000.0f; - hil_sensors.accelerometer_raw[0] = imu.xacc; - hil_sensors.accelerometer_raw[1] = imu.yacc; - hil_sensors.accelerometer_raw[2] = imu.zacc; - hil_sensors.accelerometer_m_s2[0] = mg2ms2 * imu.xacc; - hil_sensors.accelerometer_m_s2[1] = mg2ms2 * imu.yacc; - hil_sensors.accelerometer_m_s2[2] = mg2ms2 * imu.zacc; - hil_sensors.accelerometer_mode = 0; // TODO what is this? - hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 - - /* adc */ - hil_sensors.adc_voltage_v[0] = 0; - hil_sensors.adc_voltage_v[1] = 0; - hil_sensors.adc_voltage_v[2] = 0; - - /* magnetometer */ - float mga2ga = 1.0e-3f; - hil_sensors.magnetometer_counter = hil_counter; - hil_sensors.magnetometer_raw[0] = imu.xmag; - hil_sensors.magnetometer_raw[1] = imu.ymag; - hil_sensors.magnetometer_raw[2] = imu.zmag; - hil_sensors.magnetometer_ga[0] = imu.xmag * mga2ga; - hil_sensors.magnetometer_ga[1] = imu.ymag * mga2ga; - hil_sensors.magnetometer_ga[2] = imu.zmag * mga2ga; - hil_sensors.magnetometer_range_ga = 32.7f; // int16 - hil_sensors.magnetometer_mode = 0; // TODO what is this - hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; - - /* publish */ - orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); - - // increment counters - hil_counter += 1 ; - hil_frames += 1 ; - - // output - if ((timestamp - old_timestamp) > 10000000) { - printf("receiving hil imu at %d hz\n", hil_frames/10); - old_timestamp = timestamp; - hil_frames = 0; - } - } - if (msg->msgid == MAVLINK_MSG_ID_HIGHRES_IMU) { mavlink_highres_imu_t imu; @@ -437,13 +361,9 @@ handle_message(mavlink_message_t *msg) hil_sensors.magnetometer_mode = 0; // TODO what is this hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; + /* baro */ hil_sensors.baro_pres_mbar = imu.abs_pressure; - - float tempC = imu.temperature; - float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f; - float h = ground_alt + (R / g) * tempAvgK * logf(ground_press / imu.abs_pressure); - - hil_sensors.baro_alt_meter = h; + hil_sensors.baro_alt_meter = imu.pressure_alt; hil_sensors.baro_temp_celcius = imu.temperature; hil_sensors.gyro_counter = hil_counter; @@ -516,44 +436,6 @@ handle_message(mavlink_message_t *msg) } } - if (msg->msgid == MAVLINK_MSG_ID_RAW_PRESSURE) { - - mavlink_raw_pressure_t press; - mavlink_msg_raw_pressure_decode(msg, &press); - - /* packet counter */ - static uint16_t hil_counter = 0; - static uint16_t hil_frames = 0; - static uint64_t old_timestamp = 0; - - /* sensors general */ - hil_sensors.timestamp = press.time_usec; - - /* baro */ - - float tempC = press.temperature / 100.0f; - float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f; - float h = ground_alt + (R / g) * tempAvgK * logf(ground_press / press.press_abs); - hil_sensors.baro_counter = hil_counter; - hil_sensors.baro_pres_mbar = press.press_abs; - hil_sensors.baro_alt_meter = h; - hil_sensors.baro_temp_celcius = tempC; - - /* publish */ - orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); - - // increment counters - hil_counter += 1 ; - hil_frames += 1 ; - - // output - if ((timestamp - old_timestamp) > 10000000) { - printf("receiving hil pressure at %d hz\n", hil_frames/10); - old_timestamp = timestamp; - hil_frames = 0; - } - } - if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) { mavlink_hil_state_t hil_state; |