diff --git a/ArduCopter/Makefile b/ArduCopter/Makefile index 712e288804..ff51071f54 100644 --- a/ArduCopter/Makefile +++ b/ArduCopter/Makefile @@ -22,7 +22,7 @@ apm2: make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DCLI_SLIDER_ENABLED=DISABLED" mavlink10: - make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DMAVLINK10 -DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED" + make -f Makefile EXTRAFLAGS="-DMAVLINK10" etags: cd .. && etags -f ArduCopter/TAGS --langmap=C++:.pde.cpp.h $$(git ls-files ArduCopter libraries) diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 2203c2920f..eee368cbf2 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1071,6 +1071,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_ACTION_SET_AUTO: set_mode(AUTO); result=1; + // clearing failsafe should not be needed + // here. Added based on some puzzling results in + // the simulator (tridge) + failsafe = FAILSAFE_NONE; break; case MAV_ACTION_STORAGE_READ: diff --git a/ArduPlane/Makefile b/ArduPlane/Makefile index 77407d4d0a..bd70435fd6 100644 --- a/ArduPlane/Makefile +++ b/ArduPlane/Makefile @@ -22,7 +22,7 @@ apm2: make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2" mavlink10: - make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DMAVLINK10 -DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED" + make -f Makefile EXTRAFLAGS="-DMAVLINK10" etags: cd .. && etags -f ArduPlane/TAGS --langmap=C++:.pde.cpp.h $$(git ls-files ArduPlane libraries) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 56974ea8e1..cdb6569e51 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -286,6 +286,7 @@ def fly_ArduPlane(viewerip=None): print("Failed to start mavlink connection on 127.0.0.1:19550" % msg) raise mav.message_hooks.append(message_hook) + mav.idle_hooks.append(idle_hook) failed = False e = 'None' diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index 57a0fd4c05..c11cb2612c 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -53,6 +53,7 @@ def dump_logs(atype): lognums.append(int(mavproxy.match.group(1))) mavproxy.expect("Log]") for i in range(numlogs): + print("Dumping log %u (i=%u)" % (lognums[i], i)) mavproxy.send("dump %u\n" % lognums[i]) mavproxy.expect("logs enabled:", timeout=400) mavproxy.expect("Log]") @@ -259,12 +260,11 @@ def run_tests(steps): if not passed: print("FAILED %u tests: %s" % (len(failed), failed)) - results.addglob("Google Earth track", '*.kml') + results.addglob("Google Earth track", '*.kmz') results.addfile('Full Logs', 'autotest-output.txt') results.addglob('DataFlash Log', '*.flashlog') results.addglob("MAVLink log", '*.mavlog') results.addglob("GPX track", '*.gpx') - results.addglob("KMZ track", '*.kmz') results.addfile('ArduPlane build log', 'ArduPlane.txt') results.addfile('ArduPlane code size', 'ArduPlane.sizes.txt') results.addfile('ArduPlane stack sizes', 'ArduPlane.framesizes.txt') diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.cpp b/libraries/AP_ADC/AP_ADC_ADS7844.cpp index 1f0c659278..b319b29f7d 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.cpp +++ b/libraries/AP_ADC/AP_ADC_ADS7844.cpp @@ -212,7 +212,7 @@ uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result) // division. That costs us 36 bytes of stack, but I think its // worth it. for (i = 0; i < 6; i++) { - result[i] = sum[i] / count[i]; + result[i] = (sum[i] + (count[i]/2)) / count[i]; } diff --git a/libraries/Desktop/support/sitl.cpp b/libraries/Desktop/support/sitl.cpp index b81fb8e2d8..2ee237a342 100644 --- a/libraries/Desktop/support/sitl.cpp +++ b/libraries/Desktop/support/sitl.cpp @@ -41,7 +41,16 @@ struct ADC_UDR2 UDR2; struct RC_ICR4 ICR4; extern AP_TimerProcess timer_scheduler; extern Arduino_Mega_ISR_Registry isr_registry; -static volatile uint32_t sim_input_count; + +static volatile struct { + double latitude, longitude; // degrees + double altitude; // MSL + double heading; // degrees + double speedN, speedE; // m/s + double roll, pitch, yaw; // degrees + double airspeed; // m/s + uint32_t update_count; +} sim_state; /* @@ -123,14 +132,19 @@ static void sitl_fgear_input(void) return; } - sitl_update_gps(d.fg_pkt.latitude, d.fg_pkt.longitude, - ft2m(d.fg_pkt.altitude), - ft2m(d.fg_pkt.speedN), ft2m(d.fg_pkt.speedE), true); - sitl_update_adc(d.fg_pkt.rollDeg, d.fg_pkt.pitchDeg, d.fg_pkt.heading, kt2mps(d.fg_pkt.airspeed)); - sitl_update_barometer(ft2m(d.fg_pkt.altitude)); - sitl_update_compass(d.fg_pkt.heading, d.fg_pkt.rollDeg, d.fg_pkt.pitchDeg, d.fg_pkt.heading); + sim_state.latitude = d.fg_pkt.latitude; + sim_state.longitude = d.fg_pkt.longitude; + sim_state.altitude = ft2m(d.fg_pkt.altitude); + sim_state.speedN = ft2m(d.fg_pkt.speedN); + sim_state.speedE = ft2m(d.fg_pkt.speedE); + sim_state.roll = d.fg_pkt.rollDeg; + sim_state.pitch = d.fg_pkt.pitchDeg; + sim_state.yaw = d.fg_pkt.yawDeg; + sim_state.heading = d.fg_pkt.heading; + sim_state.airspeed = kt2mps(d.fg_pkt.airspeed); + sim_state.update_count++; + count++; - sim_input_count++; if (millis() - last_report > 1000) { printf("SIM %u FPS\n", count); count = 0; @@ -239,6 +253,8 @@ static void sitl_simulator_output(void) */ static void timer_handler(int signum) { + static uint32_t last_update_count; + /* make sure we die if our parent dies */ if (kill(parent_pid, 0) != 0) { exit(1); @@ -258,9 +274,22 @@ static void timer_handler(int signum) // send RC output to flight sim sitl_simulator_output(); - if (sim_input_count == 0) { + if (sim_state.update_count == 0) { sitl_update_gps(0, 0, 0, 0, 0, false); + return; } + + if (sim_state.update_count == last_update_count) { + return; + } + last_update_count = sim_state.update_count; + + sitl_update_gps(sim_state.latitude, sim_state.longitude, + sim_state.altitude, + sim_state.speedN, sim_state.speedE, true); + sitl_update_adc(sim_state.roll, sim_state.pitch, sim_state.heading, sim_state.airspeed); + sitl_update_barometer(sim_state.altitude); + sitl_update_compass(sim_state.heading, sim_state.roll, sim_state.pitch, sim_state.heading); } diff --git a/libraries/Desktop/support/sitl_adc.cpp b/libraries/Desktop/support/sitl_adc.cpp index e127446aa3..a08cd6759b 100644 --- a/libraries/Desktop/support/sitl_adc.cpp +++ b/libraries/Desktop/support/sitl_adc.cpp @@ -52,16 +52,21 @@ void sitl_update_adc(float roll, float pitch, float yaw, float airspeed) const float _gyro_gain_y = 0.41; const float _gyro_gain_z = 0.41; const float _accel_scale = 9.80665 / 423.8; - uint16_t adc[7]; + float adc[7]; float xAccel, yAccel, zAccel, scale; float rollRate, pitchRate, yawRate; static uint32_t last_update; static float last_roll, last_pitch, last_yaw; unsigned long delta_t; + // 200Hz max + if (micros() - last_update < 5000) { + return; + } + /* map roll/pitch/yaw to values the accelerometer would see */ - xAccel = sin(ToRad(pitch)); - yAccel = -sin(ToRad(roll)); + xAccel = sin(ToRad(pitch)) * cos(ToRad(roll)); + yAccel = -sin(ToRad(roll)) * cos(ToRad(pitch)); zAccel = -cos(ToRad(roll)) * cos(ToRad(pitch)); scale = 9.81 / sqrt((xAccel*xAccel)+(yAccel*yAccel)+(zAccel*zAccel)); xAccel *= scale; @@ -76,12 +81,13 @@ void sitl_update_adc(float roll, float pitch, float yaw, float airspeed) delta_t = micros(); } else { delta_t = micros() - last_update; - rollRate = 1.0e6 * (roll - last_roll) / delta_t; - pitchRate = 1.0e6 * (pitch - last_pitch) / delta_t; - yawRate = 1.0e6 * (yaw - last_yaw) / delta_t; - rollRate = normalise(rollRate, -180, 180); - pitchRate = normalise(pitchRate, -180, 180); - yawRate = normalise(yawRate, -180, 180); + float rollChange, pitchChange, yawChange; + rollChange = normalise180(roll - last_roll); + pitchChange = normalise180(pitch - last_pitch); + yawChange = normalise180(yaw - last_yaw); + rollRate = 1.0e6 * rollChange / delta_t; + pitchRate = 1.0e6 * pitchChange / delta_t; + yawRate = 1.0e6 * yawChange / delta_t; } last_update += delta_t; @@ -100,20 +106,30 @@ void sitl_update_adc(float roll, float pitch, float yaw, float airspeed) /* tell the UDR2 register emulation what values to give to the driver */ for (uint8_t i=0; i<6; i++) { - UDR2.set(sensor_map[i], (adc[i]<<3) & 0x7FFF); + UDR2.set(sensor_map[i], adc[i]); } // set the airspeed sensor input - UDR2.set(7, airspeed_sensor(airspeed)<<3); + UDR2.set(7, airspeed_sensor(airspeed)); /* FIX: rubbish value for temperature for now */ - UDR2.set(3, 2000<<3); + UDR2.set(3, 2000); + +#if 0 + extern AP_DCM_HIL dcm; + dcm.setHil(ToRad(roll), ToRad(pitch), ToRad(yaw), + ToRad(rollRate), ToRad(pitchRate), ToRad(yawRate)); + +#endif #if 0 extern AP_DCM dcm; - printf("roll=%6.1f / %6.1f pitch=%6.1f / %6.1f rollRate=%6.1f pitchRate=%6.1f\n", - roll, dcm.roll_sensor/100.0, pitch, dcm.pitch_sensor/100.0, rollRate, pitchRate); + Vector3f omega = dcm.get_gyro(); + printf("dt=%5u adc[2]=%6.1f roll=%6.1f / %6.1f yaw=%6.1f / %6.1f yawRate=%6.3f / %6.3f\n", + (unsigned)delta_t, + adc[2], + roll, dcm.roll_sensor/100.0, yaw, dcm.yaw_sensor/100.0, yawRate, ToDeg(omega.z)); #endif } diff --git a/libraries/Desktop/support/sitl_adc.h b/libraries/Desktop/support/sitl_adc.h index ac18dd1e34..38a3caba44 100644 --- a/libraries/Desktop/support/sitl_adc.h +++ b/libraries/Desktop/support/sitl_adc.h @@ -6,11 +6,13 @@ #ifndef _SITL_ADC_H #define _SITL_ADC_H +#include + // this implements the UDR2 register struct ADC_UDR2 { uint16_t value, next_value; uint8_t idx; - uint16_t channels[8]; + float channels[8]; ADC_UDR2() { // constructor @@ -26,19 +28,26 @@ struct ADC_UDR2 { to output next */ ADC_UDR2& operator=(uint8_t cmd) { + float next_analog; switch (cmd) { - case 0x87: next_value = channels[0]; break; - case 0xC7: next_value = channels[1]; break; - case 0x97: next_value = channels[2]; break; - case 0xD7: next_value = channels[3]; break; - case 0xA7: next_value = channels[4]; break; - case 0xE7: next_value = channels[5]; break; - case 0xB7: next_value = channels[6]; break; - case 0xF7: next_value = channels[7]; break; + case 0x87: next_analog = channels[0]; break; + case 0xC7: next_analog = channels[1]; break; + case 0x97: next_analog = channels[2]; break; + case 0xD7: next_analog = channels[3]; break; + case 0xA7: next_analog = channels[4]; break; + case 0xE7: next_analog = channels[5]; break; + case 0xB7: next_analog = channels[6]; break; + case 0xF7: next_analog = channels[7]; break; + case 0: + default: return *this; } - if (cmd != 0) { - idx = 1; + idx = 1; + int noise = (((unsigned long)random()) % 3) - 1; + next_value = (unsigned)(next_analog + noise + 0.5); + if (next_value >= 0x1000) { + next_value = 0xFFF; } + next_value = (next_value << 3); return *this; } @@ -60,7 +69,7 @@ struct ADC_UDR2 { /* interface to set a channel value from SITL */ - void set(uint8_t i, uint16_t v) { + void set(uint8_t i, float v) { channels[i] = v; } }; diff --git a/libraries/Desktop/support/util.cpp b/libraries/Desktop/support/util.cpp index 8cf988b2c6..e0f757f767 100644 --- a/libraries/Desktop/support/util.cpp +++ b/libraries/Desktop/support/util.cpp @@ -90,3 +90,8 @@ double normalise(double v, double min, double max) } return v; } + +double normalise180(double v) +{ + return normalise(v, -180, 180); +} diff --git a/libraries/Desktop/support/util.h b/libraries/Desktop/support/util.h index 5f075184f9..7b815c6ae6 100644 --- a/libraries/Desktop/support/util.h +++ b/libraries/Desktop/support/util.h @@ -9,3 +9,4 @@ float swap_float(float f); void swap_floats(float *f, unsigned count); void set_nonblocking(int fd); double normalise(double v, double min, double max); +double normalise180(double v);