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

Merge pull request #244 from PX4/hil 

Small HIL improvements
This commit is contained in:
Lorenz Meier 2013-04-07 10:15:26 -07:00
parent 9dbd2695d3 a1c8d19c34
commit f562a27976
2 changed files with 96 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ROMFS/scripts/rc.hil
View File

@ -35,6 +35,17 @@ param set MAV_TYPE 1
#
commander start
#
# Check if we got an IO
#
if [ px4io start ]
then
echo "IO started"
else
fmu mode_serial
echo "FMU started"
end
#
# Start the sensors (depends on orb, px4io)
#

92
apps/mavlink/mavlink_receiver.c
View File

@ -308,6 +308,14 @@ 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;
@ -376,6 +384,83 @@ handle_message(mavlink_message_t *msg)
}
}
if (msg->msgid == MAVLINK_MSG_ID_HIGHRES_IMU) {
mavlink_highres_imu_t imu;
mavlink_msg_highres_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 / mrad2rad;
hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad;
hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad;
hil_sensors.gyro_rad_s[0] = imu.xgyro;
hil_sensors.gyro_rad_s[1] = imu.ygyro;
hil_sensors.gyro_rad_s[2] = imu.zgyro;
/* accelerometer */
hil_sensors.accelerometer_counter = hil_counter;
static const float mg2ms2 = 9.8f / 1000.0f;
hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2;
hil_sensors.accelerometer_m_s2[0] = imu.xacc;
hil_sensors.accelerometer_m_s2[1] = imu.yacc;
hil_sensors.accelerometer_m_s2[2] = 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 / mga2ga;
hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga;
hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga;
hil_sensors.magnetometer_ga[0] = imu.xmag;
hil_sensors.magnetometer_ga[1] = imu.ymag;
hil_sensors.magnetometer_ga[2] = imu.zmag;
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;
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_temp_celcius = imu.temperature;
/* 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_GPS_RAW_INT) {
mavlink_gps_raw_int_t gps;
@ -435,13 +520,6 @@ handle_message(mavlink_message_t *msg)
hil_sensors.timestamp = press.time_usec;
/* baro */
/* 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;
float tempC = press.temperature / 100.0f;
float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f;