fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #265 from jgoppert/hil-press-fix

Browse Source 
Hil press fix
This commit is contained in:
Lorenz Meier 2013-05-09 06:42:39 -07:00
commit ccd3de2f90
4 changed files with 17 additions and 135 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
apps/controllib/fixedwing.cpp
View File

@ -156,9 +156,9 @@ 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, ""),
@ -170,7 +170,7 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par
_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)
@ -301,8 +301,8 @@ void BlockMultiModeBacksideAutopilot::update()
// 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());

6
apps/controllib/fixedwing.hpp
View File

@ -311,9 +311,9 @@ private:
BlockLimit _theLimit;
BlockLimit _vLimit;
// altitude/ roc hold
// altitude/ climb rate hold
BlockPID _h2Thr;
BlockPID _roc2Thr;
BlockPID _cr2Thr;
// guidance
BlockWaypointGuidance _guide;
@ -325,7 +325,7 @@ private:
BlockParam<float> _trimThr;
BlockParam<float> _trimV;
BlockParam<float> _vCmd;
BlockParam<float> _rocMax;
BlockParam<float> _crMax;
struct pollfd _attPoll;
vehicle_global_position_setpoint_s _lastPosCmd;

14
apps/examples/control_demo/params.c
View File

@ -59,14 +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_V, 12.0f); // trim velocity, m/s

122
apps/mavlink/mavlink_receiver.c
View File

@ -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.15;
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;