diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 34da2dfef2..00baed6468 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -23,7 +23,7 @@ if param compare SYS_AUTOSTART 1000 then - #sh /etc/init.d/1000_rc_fw_easystar.hil + sh /etc/init.d/1000_rc_fw_easystar.hil fi if param compare SYS_AUTOSTART 1001 diff --git a/ROMFS/px4fmu_common/mixers/FMU_Q.mix b/ROMFS/px4fmu_common/mixers/FMU_Q.mix index 17ff711513..b8ecbc879f 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_Q.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_Q.mix @@ -25,13 +25,13 @@ for the elevons. M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 -5000 -8000 0 -10000 10000 -S: 0 1 8000 8000 0 -10000 10000 +S: 0 0 -8000 -8000 0 -10000 10000 +S: 0 1 6000 6000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 -8000 -5000 0 -10000 10000 -S: 0 1 -8000 -8000 0 -10000 10000 +S: 0 0 -8000 -8000 0 -10000 10000 +S: 0 1 -6000 -6000 0 -10000 10000 Output 2 -------- diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index f2db4aa1ed..95408b729b 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1,5 +1,8 @@ #include "estimator.h" +// For debugging only +#include + // Global variables float KH[n_states][n_states]; // intermediate result used for covariance updates float KHP[n_states][n_states]; // intermediate result used for covariance updates @@ -32,14 +35,15 @@ float posNED[3]; // North, East Down position (m) float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement +float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time +float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time + float innovMag[3]; // innovation output float varInnovMag[3]; // innovation variance output Vector3f magData; // magnetometer flux radings in X,Y,Z body axes -float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time float innovVtas; // innovation output float varInnovVtas; // innovation variance output float VtasMeas; // true airspeed measurement (m/s) -float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time float latRef; // WGS-84 latitude of reference point (rad) float lonRef; // WGS-84 longitude of reference point (rad) float hgtRef; // WGS-84 height of reference point (m) @@ -1125,6 +1129,8 @@ void FuseVelposNED() } } } + + //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL")); } void FuseMagnetometer() @@ -1586,13 +1592,15 @@ float sq(float valIn) } // Store states in a history array along with time stamp -void StoreStates() +void StoreStates(uint64_t timestamp_ms) { static uint8_t storeIndex = 0; - if (storeIndex == data_buffer_size) storeIndex = 0; - for (uint8_t i=0; i<=n_states; i++) storedStates[i][storeIndex] = states[i]; - statetimeStamp[storeIndex] = millis(); - storeIndex = storeIndex + 1; + for (unsigned i=0; i 1.0f) + if (deltaT > 1.0f || deltaT < 0.000001f) deltaT = 0.01f; // Always store data, independent of init status @@ -547,41 +556,13 @@ FixedwingEstimator::task_main() #endif - if (_initialized) { + bool airspeed_updated; + orb_check(_airspeed_sub, &airspeed_updated); + if (airspeed_updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + perf_count(_perf_airspeed); - /* predict states and covariances */ - - /* run the strapdown INS every sensor update */ - UpdateStrapdownEquationsNED(); - - /* store the predictions */ - StoreStates(); - - /* evaluate if on ground */ - OnGroundCheck(); - - /* prepare the delta angles and time used by the covariance prediction */ - summedDelAng = summedDelAng + correctedDelAng; - summedDelVel = summedDelVel + correctedDelVel; - dt += dtIMU; - - /* predict the covairance if the total delta angle has exceeded the threshold - * or the time limit will be exceeded on the next measurement update - */ - if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { - CovariancePrediction(); - summedDelAng = summedDelAng.zero(); - summedDelVel = summedDelVel.zero(); - dt = 0.0f; - } - } - - bool baro_updated; - orb_check(_baro_sub, &baro_updated); - if (baro_updated) { - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - - baroHgt = _baro.altitude; + VtasMeas = _airspeed.true_airspeed_m_s; } bool gps_updated; @@ -590,65 +571,36 @@ FixedwingEstimator::task_main() orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); - if (_gps.fix_type > 2) { + if (_gps.fix_type < 3) { + gps_updated = false; + } else { /* fuse GPS updates */ //_gps.timestamp / 1e3; GPSstatus = _gps.fix_type; - gpsCourse = _gps.cog_rad; - gpsGndSpd = sqrtf(_gps.vel_n_m_s * _gps.vel_n_m_s + _gps.vel_e_m_s * _gps.vel_e_m_s); - gpsVelD = _gps.vel_d_m_s; + velNED[0] = _gps.vel_n_m_s; + velNED[1] = _gps.vel_e_m_s; + velNED[2] = _gps.vel_d_m_s; + + // warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]); + gpsLat = math::radians(_gps.lat / (double)1e7); gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; gpsHgt = _gps.alt / 1e3f; - if (hrt_elapsed_time(&start_time) > 500000 && !_initialized) { - InitialiseFilter(); - _initialized = true; - - warnx("init done."); - continue; - } - - if (_initialized) { - - /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ - velNED[0] = _gps.vel_n_m_s; - velNED[1] = _gps.vel_e_m_s; - velNED[2] = _gps.vel_d_m_s; - calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); - - posNE[0] = posNED[0]; - posNE[1] = posNED[1]; - hgtMea = -posNED[2]; - - // set flags for further processing - fuseVelData = true; - fusePosData = true; - fuseHgtData = true; - - /* recall states after adjusting for delays */ - RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); - - /* run the actual fusion */ - FuseVelposNED(); - } - - } else { - - /* do not fuse anything, we got no position / vel update */ - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; } - } else { - /* do not fuse anything, we got no position / vel update */ - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; + } + + bool baro_updated; + orb_check(_baro_sub, &baro_updated); + if (baro_updated) { + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + + baroHgt = _baro.altitude; + + // Could use a blend of GPS and baro alt data if desired + hgtMea = 1.0f*baroHgt + 0.0f*gpsHgt; } #ifndef SENSOR_COMBINED_SUB @@ -687,41 +639,107 @@ FixedwingEstimator::task_main() #endif - if (_initialized) { + } - fuseMagData = true; - RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); - FuseMagnetometer(); + + /** + * PART TWO: EXECUTE THE FILTER + **/ + + if (hrt_elapsed_time(&start_time) > 500000 && !_initialized && (GPSstatus == 3)) { + InitialiseFilter(velNED); + _initialized = true; + + warnx("init done."); + } + + if (_initialized) { + + /* predict states and covariances */ + + /* run the strapdown INS every sensor update */ + UpdateStrapdownEquationsNED(); + + /* store the predictions */ + StoreStates(IMUmsec); + + /* evaluate if on ground */ + OnGroundCheck(); + + /* prepare the delta angles and time used by the covariance prediction */ + summedDelAng = summedDelAng + correctedDelAng; + summedDelVel = summedDelVel + correctedDelVel; + dt += dtIMU; + + /* predict the covairance if the total delta angle has exceeded the threshold + * or the time limit will be exceeded on the next measurement update + */ + if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { + CovariancePrediction(); + summedDelAng = summedDelAng.zero(); + summedDelVel = summedDelVel.zero(); + dt = 0.0f; } + } + + + if (gps_updated && _initialized) { + + /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ + calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); + + posNE[0] = posNED[0]; + posNE[1] = posNED[1]; + + // set flags for further processing + fuseVelData = true; + fusePosData = true; + + /* recall states after adjusting for delays */ + RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + + /* run the actual fusion */ + FuseVelposNED(); + } else { + fuseVelData = false; + fusePosData = false; + } + + if (baro_updated && _initialized) { + + fuseHgtData = true; + // recall states stored at time of measurement after adjusting for delays + RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + // run the fusion step + FuseVelposNED(); + } else { + fuseHgtData = false; + } + + if (mag_updated && _initialized) { + fuseMagData = true; + RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); } else { fuseMagData = false; } - bool airspeed_updated; - orb_check(_airspeed_sub, &airspeed_updated); - if (airspeed_updated && _initialized) { - orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - perf_count(_perf_airspeed); + if (_initialized) { + FuseMagnetometer(); + } - if (_airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { - - VtasMeas = _airspeed.true_airspeed_m_s; - - if (_initialized) { - - fuseVtasData = true; - RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data - FuseAirspeed(); - } - } else { - fuseVtasData = false; - } + if (airspeed_updated && _initialized + && _airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { + fuseVtasData = true; + RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + FuseAirspeed(); } else { fuseVtasData = false; } + // Publish results if (_initialized) { // State vector: @@ -740,7 +758,7 @@ FixedwingEstimator::task_main() for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) _att.R[i][j] = R(i, j); - _att.timestamp = _gyro.timestamp; + _att.timestamp = last_sensor_timestamp; _att.q[0] = states[0]; _att.q[1] = states[1]; _att.q[2] = states[2]; @@ -748,7 +766,7 @@ FixedwingEstimator::task_main() _att.q_valid = true; _att.R_valid = true; - _att.timestamp = _gyro.timestamp; + _att.timestamp = last_sensor_timestamp; _att.roll = euler.getPhi(); _att.pitch = euler.getTheta(); _att.yaw = euler.getPsi(); @@ -771,7 +789,7 @@ FixedwingEstimator::task_main() _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); } - _local_pos.timestamp = _gyro.timestamp; + _local_pos.timestamp = last_sensor_timestamp; _local_pos.x = states[7]; _local_pos.y = states[8]; _local_pos.z = states[9]; @@ -892,6 +910,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) + printf("dt: %8.6f\n", dtIMU); printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]); printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]); printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]); diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index a2a0c109c6..0a88d40f39 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -257,7 +257,6 @@ mtd_start(char *partition_names[], unsigned n_partitions) /* Now create MTD FLASH partitions */ - warnx("Creating partitions"); FAR struct mtd_dev_s *part[n_partitions]; char blockname[32]; @@ -266,9 +265,6 @@ mtd_start(char *partition_names[], unsigned n_partitions) for (offset = 0, i = 0; i < n_partitions; offset += nblocks, i++) { - warnx(" Partition %d. Block offset=%lu, size=%lu", - i, (unsigned long)offset, (unsigned long)nblocks); - /* Create the partition */ part[i] = mtd_partition(mtd_dev, offset, nblocks); diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 580fdc62f4..0cbba0a374 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -320,7 +320,7 @@ do_set(const char* name, const char* val) char* end; f = strtod(val,&end); param_set(param, &f); - printf(" -> new: %f\n", f); + printf(" -> new: %4.4f\n", (double)f); }