forked from Archive/PX4-Autopilot
Merge branch 'master' of github.com:PX4/Firmware into position_estimator_mc
This commit is contained in:
commit
3bba1ea71c
|
@ -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)
|
||||
#
|
||||
|
|
|
@ -684,9 +684,10 @@ L3GD20::measure()
|
|||
* 74 from all measurements centers them around zero.
|
||||
*/
|
||||
report->timestamp = hrt_absolute_time();
|
||||
/* XXX adjust for sensor alignment to board here */
|
||||
report->x_raw = raw_report.x;
|
||||
report->y_raw = raw_report.y;
|
||||
|
||||
/* swap x and y and negate y */
|
||||
report->x_raw = raw_report.y;
|
||||
report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
|
||||
report->z_raw = raw_report.z;
|
||||
|
||||
report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
|
||||
|
|
|
@ -1,7 +1,7 @@
|
|||
#include <systemlib/param/param.h>
|
||||
|
||||
/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
|
||||
PARAM_DEFINE_FLOAT(KF_V_GYRO, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f);
|
||||
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
|
||||
|
@ -14,4 +14,3 @@ PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
|
|||
PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f);
|
||||
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f);
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -88,7 +88,7 @@ PARAM_DEFINE_FLOAT(RC4_MIN, 1000);
|
|||
PARAM_DEFINE_FLOAT(RC4_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC4_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC4_REV, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(RC4_DZ, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(RC4_DZ, 30.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC5_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC5_TRIM, 1500);
|
||||
|
|
Loading…
Reference in New Issue