Merge remote-tracking branch 'upstream/master'

.gitignore

@@ -1,105 +1,85 @@
-*~
+/.lock-waf*
+/.waf*
+/Tools/autotest/aircraft/Rascal/reset.xml
+/Tools/autotest/ch7_mission.txt
+/Tools/autotest/jsb_sim/fgout.xml
+/Tools/autotest/jsb_sim/rascal_test.xml
+/Tools/autotest/jsbsim_fgout_0.xml
+/Tools/autotest/jsbsim_start_0.xml
+/tmp/*
 *.bin
 *.d
+*.dfu
 *.dll
 *.elf
-*.hex
-*.dfu
 *.exe
+*.generated.h
+*.hex
 *.lst
 *.o
 *.obj
 *.px4
-*.vrx
 *.pyc
 *.tlog
 *.tlog.raw
 *.vbrain
+*.vrx
 *.zip
+*~
 .*.swo
 .*.swp
+.DS_Store
+.autotools
 .built
 .context
 .cproject
 .depend
 .directory
-.DS_Store
 .metadata/
 .project
 .settings/
-.autotools
-.vagrant
 .tags
 .tags_sorted_by_file
-/.lock-waf*
-/.waf*
-/tmp/*
-/Tools/ArdupilotMegaPlanner/3DRRadio/bin
-/Tools/ArdupilotMegaPlanner/3DRRadio/obj
-/Tools/ArdupilotMegaPlanner/APMPlannerXplanes/APMPlannerXplanes/Debug
-/Tools/ArdupilotMegaPlanner/bin
-/Tools/ArdupilotMegaPlanner/bin/APM Planner.app
-/Tools/ArdupilotMegaPlanner/bin/Debug
-/Tools/ArdupilotMegaPlanner/bin/Release/gmapcache
-/Tools/ArdupilotMegaPlanner/bin/Release/logs/
-/Tools/ArdupilotMegaPlanner/CustomImages
-/Tools/ArdupilotMegaPlanner/Msi/upload.bat
-/Tools/ArdupilotMegaPlanner/obj
-/Tools/ArdupilotMegaPlanner/resedit/bin
-/Tools/ArdupilotMegaPlanner/resedit/obj
-/Tools/ArdupilotMegaPlanner/Updater/bin
-/Tools/ArdupilotMegaPlanner/Updater/obj
-/Tools/ArdupilotMegaPlanner/wix/bin
-/Tools/ArdupilotMegaPlanner/wix/obj
-/Tools/autotest/ch7_mission.txt
-/Tools/autotest/aircraft/Rascal/reset.xml
-/Tools/autotest/jsbsim_fgout_0.xml
-/Tools/autotest/jsbsim_start_0.xml
-/Tools/autotest/jsb_sim/fgout.xml
-/Tools/autotest/jsb_sim/rascal_test.xml
+.vagrant
+APMrover2/test.APMrover2/
+APMrover2/way.txt
 ArduCopter/Debug/
-ArduCopter/logs/
-ArduCopter/terrain/*.DAT
-ArduCopter/test.ArduCopter/
-ArduCopter/test/*
 ArduCopter/fence.txt
 ArduCopter/ral.txt
+ArduCopter/test.ArduCopter/
+ArduCopter/test/*
 ArduCopter/way.txt
-ArduPlane/logs/
-ArduPlane/test/*
-ArduPlane/terrain/*.DAT
 ArduPlane/test.ArduPlane/
+ArduPlane/test/*
 ArduPlane/way.txt
-APMrover2/way.txt
-APMrover2/logs/
-APMrover2/test.APMrover2/
-mk/PX4/ROMFS/default.parm
-autotest.lck
-build
-Build.ArduCopter/*
-Build.ArduPlane/*
 Build.APMrover2/*
 Build.AntennaTracker/*
 Build.ArduSub/*
-cmake_install.cmake
+Build.ArduCopter/*
+Build.ArduPlane/*
 CMakeCache.txt
 CMakeFiles
+LASTLOG.TXT
+Make.dep
+Thumbs.db
+autotest.lck
+build
+cmake_install.cmake
 config.mk
+cscope.in.out
+cscope.out
+cscope.po.out
 dataflash.bin
 eeprom.bin
 index.html
-LASTLOG.TXT
-Make.dep
+logs/
 mav.log
 mav.log.raw
 mav.parm
+mk/PX4/ROMFS/default.parm
 module.mk
 serialsent.raw
 status.txt
 tags
+terrain/
 test.ArduCopter/*
-Thumbs.db
-cscope.in.out
-cscope.out
-cscope.po.out
-*.generated.h

APMrover2/APMrover2.cpp

@@ -411,14 +411,12 @@ void Rover::update_current_mode(void)
     switch (control_mode){
     case AUTO:
     case RTL:
-        set_reverse(false);
         calc_lateral_acceleration();
         calc_nav_steer();
         calc_throttle(g.speed_cruise);
         break;
 
     case GUIDED:
-        set_reverse(false);
         if (rtl_complete || verify_RTL()) {
             // we have reached destination so stop where we are
             if (channel_throttle->get_servo_out() != g.throttle_min.get()) {
@@ -483,7 +481,6 @@ void Rover::update_current_mode(void)
         // hold position - stop motors and center steering
         channel_throttle->set_servo_out(0);
         channel_steer->set_servo_out(0);
-        set_reverse(false);
         break;
 
     case INITIALISING:

APMrover2/GCS_Mavlink.cpp

@@ -1263,6 +1263,12 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             break;
         }
 
+    case MAVLINK_MSG_ID_GPS_INPUT:
+        {
+            rover.gps.handle_msg(msg);
+            break;
+        }
+
 #if HIL_MODE != HIL_MODE_DISABLED
 	case MAVLINK_MSG_ID_HIL_STATE:
 		{

APMrover2/Rover.h

@@ -502,13 +502,16 @@ class Rover : public AP_HAL::HAL::Callbacks {
     bool verify_command(const AP_Mission::Mission_Command& cmd);
     bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
     void do_nav_wp(const AP_Mission::Mission_Command& cmd);
+    void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
     bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
+    bool verify_loiter_unlim();
     void do_wait_delay(const AP_Mission::Mission_Command& cmd);
     void do_within_distance(const AP_Mission::Mission_Command& cmd);
     void do_change_speed(const AP_Mission::Mission_Command& cmd);
     void do_set_home(const AP_Mission::Mission_Command& cmd);
     void do_digicam_configure(const AP_Mission::Mission_Command& cmd);
     void do_digicam_control(const AP_Mission::Mission_Command& cmd);
+    void do_set_reverse(const AP_Mission::Mission_Command& cmd);
     void init_capabilities(void);
     void rudder_arm_disarm_check();
     void change_arm_state(void);

APMrover2/Steering.cpp

@@ -203,7 +203,9 @@ void Rover::calc_nav_steer() {
     }
 
     // add in obstacle avoidance
-    lateral_acceleration += (obstacle.turn_angle/45.0f) * g.turn_max_g;
+    if (!in_reverse) {
+        lateral_acceleration += (obstacle.turn_angle/45.0f) * g.turn_max_g;
+    }
 
     // constrain to max G force
     lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g*GRAVITY_MSS, g.turn_max_g*GRAVITY_MSS);

APMrover2/commands_logic.cpp

@@ -31,6 +31,10 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
 			do_RTL();
 			break;
 
+        case MAV_CMD_NAV_LOITER_UNLIM:              // Loiter indefinitely
+            do_loiter_unlimited(cmd);
+            break;
+
         // Conditional commands
 		case MAV_CMD_CONDITION_DELAY:
 			do_wait_delay(cmd);
@@ -103,6 +107,10 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
             break;
 #endif
 
+        case MAV_CMD_DO_SET_REVERSE:
+            do_set_reverse(cmd);
+            break;
+
 		default:
 		    // return false for unhandled commands
 		    return false;
@@ -153,6 +161,9 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
 		case MAV_CMD_NAV_RETURN_TO_LAUNCH:
 			return verify_RTL();
 
+        case MAV_CMD_NAV_LOITER_UNLIM:
+            return verify_loiter_unlim();
+
         case MAV_CMD_CONDITION_DELAY:
             return verify_wait_delay();
 
@@ -194,6 +205,15 @@ void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 	set_next_WP(cmd.content.location);
 }
 
+void Rover::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
+{
+    Location cmdloc = cmd.content.location;
+    location_sanitize(current_loc, cmdloc);
+    set_next_WP(cmdloc);
+    loiter_time_max = 100; // an arbitrary large loiter time
+    distance_past_wp = 0;
+}
+
 /********************************************************************************/
 //  Verify Nav (Must) commands
 /********************************************************************************/
@@ -264,6 +284,13 @@ bool Rover::verify_RTL()
     return false;
 }
 
+bool Rover::verify_loiter_unlim()
+{
+    // Continually increase the loiter time so it never finishes
+    loiter_time += loiter_time_max;
+    return false;
+}
+
 /********************************************************************************/
 //  Condition (May) commands
 /********************************************************************************/
@@ -382,3 +409,12 @@ void Rover::log_picture()
         }      
     }
 }
+
+void Rover::do_set_reverse(const AP_Mission::Mission_Command& cmd)
+{
+	if (cmd.p1 == 1) {
+        set_reverse(true);
+	} else {
+        set_reverse(false);
+    }
+}

APMrover2/system.cpp

@@ -250,6 +250,9 @@ void Rover::set_reverse(bool reverse)
         return;
     }
     g.pidSpeedThrottle.reset_I();
+    steerController.reset_I();
+    nav_controller->set_reverse(reverse);
+    steerController.set_reverse(reverse);
     in_reverse = reverse;
 }
 
@@ -269,7 +272,6 @@ void Rover::set_mode(enum mode mode)
     control_mode = mode;
     throttle_last = 0;
     throttle = 500;
-    set_reverse(false);
     g.pidSpeedThrottle.reset_I();
 
     if (control_mode != AUTO) {

ArduCopter/ArduCopter.cpp

@@ -102,7 +102,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     SCHED_TASK(compass_accumulate,   100,    100),
     SCHED_TASK(barometer_accumulate,  50,     90),
 #if PRECISION_LANDING == ENABLED
-    SCHED_TASK(update_precland,       50,     50),
+    SCHED_TASK(update_precland,      400,     50),
 #endif
 #if FRAME_CONFIG == HELI_FRAME
     SCHED_TASK(check_dynamic_flight,  50,     75),
@@ -127,7 +127,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     SCHED_TASK(compass_cal_update,   100,    100),
     SCHED_TASK(accel_cal_update,      10,    100),
 #if ADSB_ENABLED == ENABLED
-    SCHED_TASK(adsb_update,            1,    100),
+    SCHED_TASK(adsb_update,           10,    100),
 #endif
 #if FRSKY_TELEM_ENABLED == ENABLED
     SCHED_TASK(frsky_telemetry_send,   5,     75),
@@ -428,6 +428,11 @@ void Copter::twentyfive_hz_logging()
         DataFlash.Log_Write_IMU(ins);
     }
 #endif
+
+#if PRECISION_LANDING == ENABLED
+    // log output
+    Log_Write_Precland();
+#endif
 }
 
 void Copter::dataflash_periodic(void)
@@ -496,6 +501,8 @@ void Copter::one_hz_loop()
 
     // log terrain data
     terrain_logging();
+
+    adsb.set_is_flying(!ap.land_complete);
 }
 
 // called at 50hz

ArduCopter/Copter.h

@@ -140,6 +140,7 @@ class Copter : public AP_HAL::HAL::Callbacks {
 
     // Global parameters are all contained within the 'g' class.
     Parameters g;
+    ParametersG2 g2;
 
     // main loop scheduler
     AP_Scheduler scheduler;
@@ -271,6 +272,9 @@ class Copter : public AP_HAL::HAL::Callbacks {
 
     uint32_t precland_last_update_ms;
 
+    // altitude below which we do no navigation in auto takeoff
+    float auto_takeoff_no_nav_alt_cm;
+
     RCMapper rcmap;
 
     // board specific config
@@ -568,6 +572,8 @@ class Copter : public AP_HAL::HAL::Callbacks {
         uint8_t dynamic_flight          : 1;    // 0   // true if we are moving at a significant speed (used to turn on/off leaky I terms)
         uint8_t init_targets_on_arming  : 1;    // 1   // true if we have been disarmed, and need to reset rate controller targets when we arm
     } heli_flags;
+
+    int16_t hover_roll_trim_scalar_slew;
 #endif
 
 #if GNDEFFECT_COMPENSATION == ENABLED
@@ -631,6 +637,8 @@ class Copter : public AP_HAL::HAL::Callbacks {
     float get_takeoff_trigger_throttle();
     float get_throttle_pre_takeoff(float input_thr);
     float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt);
+    void auto_takeoff_set_start_alt(void);
+    void auto_takeoff_attitude_run(float target_yaw_rate);
     void set_accel_throttle_I_from_pilot_throttle(float pilot_throttle);
     void update_poscon_alt_max();
     void rotate_body_frame_to_NE(float &x, float &y);
@@ -805,7 +813,8 @@ class Copter : public AP_HAL::HAL::Callbacks {
     void land_run();
     void land_gps_run();
     void land_nogps_run();
-    float get_land_descent_speed();
+    void land_run_vertical_control(bool pause_descent = false);
+    void land_run_horizontal_control();
     void land_do_not_use_GPS();
     void set_mode_land_with_pause(mode_reason_t reason);
     bool landing_with_GPS();

ArduCopter/GCS_Mavlink.cpp

@@ -1857,6 +1857,13 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
         break;
     }
 
+    case MAVLINK_MSG_ID_GPS_INPUT:
+    {
+      result = MAV_RESULT_ACCEPTED;
+      copter.gps.handle_msg(msg);
+      break;
+    }
+
 #if HIL_MODE != HIL_MODE_DISABLED
     case MAVLINK_MSG_ID_HIL_STATE:          // MAV ID: 90
     {