Copter: run main loop at 400hz for pixhawk

ArduCopter/ArduCopter.pde

@@ -212,7 +212,11 @@ static DataFlash_Empty DataFlash;
 ////////////////////////////////////////////////////////////////////////////////
 // the rate we run the main loop at
 ////////////////////////////////////////////////////////////////////////////////
+#if MAIN_LOOP_RATE == 400
+static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_400HZ;
+#else
 static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_100HZ;
+#endif
 
 ////////////////////////////////////////////////////////////////////////////////
 // Sensors
@@ -835,6 +839,72 @@ static void pre_arm_checks(bool display_failure);
 // setup the var_info table
 AP_Param param_loader(var_info, WP_START_BYTE);
 
+#if MAIN_LOOP_RATE == 400
+/*
+  scheduler table for fast CPUs - all regular tasks apart from the fast_loop()
+  should be listed here, along with how often they should be called
+  (in 2.5ms units) and the maximum time they are expected to take (in
+  microseconds)
+  1    = 400hz
+  2    = 200hz
+  4    = 100hz
+  8    = 50hz
+  20   = 20hz
+  40   = 10hz
+  133  = 3hz
+  400  = 1hz
+  4000 = 0.1hz
+  
+ */
+static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
+    { rc_loop,               4,     10 },
+    { throttle_loop,         8,     45 },
+    { update_GPS,            8,     90 },
+    { update_nav_mode,       4,     40 },
+    { update_batt_compass,  40,     72 },
+    { read_aux_switches,    40,      5 },
+    { arm_motors_check,     40,      1 },
+    { auto_trim,            40,     14 },
+    { update_altitude,      40,    100 },
+    { run_nav_updates,      40,     80 },
+    { three_hz_loop,       133,      9 },
+    { compass_accumulate,    8,     42 },
+    { barometer_accumulate,  8,     25 },
+#if FRAME_CONFIG == HELI_FRAME
+    { check_dynamic_flight,  8,     10 },
+#endif
+    { update_notify,         8,     10 },
+    { one_hz_loop,         400,     42 },
+    { crash_check,          40,      2 },
+    { gcs_check_input,	     8,    550 },
+    { gcs_send_heartbeat,  400,    150 },
+    { gcs_send_deferred,     8,    720 },
+    { gcs_data_stream_send,  8,    950 },
+#if COPTER_LEDS == ENABLED
+    { update_copter_leds,   40,      5 },
+#endif
+    { update_mount,          8,     45 },
+    { ten_hz_logging_loop,  40,     30 },
+    { fifty_hz_logging_loop, 2,     22 },
+    { perf_update,        4000,     20 },
+    { read_receiver_rssi,   40,      5 },
+#ifdef USERHOOK_FASTLOOP
+    { userhook_FastLoop,     4,     10 },
+#endif
+#ifdef USERHOOK_50HZLOOP
+    { userhook_50Hz,         8,     10 },
+#endif
+#ifdef USERHOOK_MEDIUMLOOP
+    { userhook_MediumLoop,  40,     10 },
+#endif
+#ifdef USERHOOK_SLOWLOOP
+    { userhook_SlowLoop,    120,    10 },
+#endif
+#ifdef USERHOOK_SUPERSLOWLOOP
+    { userhook_SuperSlowLoop,400,   10 },
+#endif
+};
+#else
 /*
   scheduler table - all regular tasks apart from the fast_loop()
   should be listed here, along with how often they should be called
@@ -842,6 +912,7 @@ AP_Param param_loader(var_info, WP_START_BYTE);
   microseconds)
  */
 static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
+    { rc_loop,               1,     100 },
     { throttle_loop,         2,     450 },
     { update_GPS,            2,     900 },
     { update_nav_mode,       1,     400 },
@@ -885,6 +956,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
     { userhook_SuperSlowLoop,100,   100 },
 #endif
 };
+#endif
 
 
 void setup() 
@@ -963,8 +1035,8 @@ void loop()
     // in multiples of the main loop tick. So if they don't run on
     // the first call to the scheduler they won't run on a later
     // call until scheduler.tick() is called again
-    uint32_t time_available = (timer + 10000) - micros();
-    scheduler.run(time_available - 300);
+    uint32_t time_available = (timer + MAIN_LOOP_MICROS) - micros();
+    scheduler.run(time_available);
 }
 
 
@@ -1007,6 +1079,12 @@ static void fast_loop()
     }
 #endif  // OPTFLOW == ENABLED
 
+}
+
+// rc_loops - reads user input from transmitter/receiver
+// called at 100hz
+static void rc_loop()
+{
     // Read radio and 3-position switch on radio
     // -----------------------------------------
     read_radio();

ArduCopter/config.h

@@ -59,6 +59,7 @@
  # define CONFIG_IMU_TYPE   CONFIG_IMU_MPU6000
  # define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
  # define MAGNETOMETER ENABLED
+ # define MAIN_LOOP_RATE    100
  # ifdef APM2_BETA_HARDWARE
   #  define CONFIG_BARO     AP_BARO_BMP085
  # else // APM2 Production Hardware (default)
@@ -70,12 +71,14 @@
  # define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
  # define MAGNETOMETER ENABLED
  # define OPTFLOW DISABLED
+ # define MAIN_LOOP_RATE    100
 #elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
  # define CONFIG_IMU_TYPE   CONFIG_IMU_PX4
  # define CONFIG_BARO       AP_BARO_PX4
  # define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
  # define MAGNETOMETER ENABLED
  # define OPTFLOW DISABLED
+ # define MAIN_LOOP_RATE    400
 #elif CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
  # define CONFIG_IMU_TYPE CONFIG_IMU_FLYMAPLE
  # define CONFIG_BARO AP_BARO_BMP085
@@ -84,6 +87,7 @@
  # define MAGNETOMETER ENABLED
  # define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
  # define OPTFLOW DISABLED
+ # define MAIN_LOOP_RATE    400
 #elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
  # define CONFIG_IMU_TYPE CONFIG_IMU_L3G4200D
  # define CONFIG_BARO AP_BARO_BMP085
@@ -92,8 +96,16 @@
  # define MAGNETOMETER ENABLED
  # define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
  # define OPTFLOW DISABLED
+ # define MAIN_LOOP_RATE    400
 #endif
 
+#if MAIN_LOOP_RATE == 400
+ # define MAIN_LOOP_SECONDS 0.0025
+ # define MAIN_LOOP_MICROS  2500
+#else
+ # define MAIN_LOOP_SECONDS 0.01
+ # define MAIN_LOOP_MICROS  10000
+#endif
 //////////////////////////////////////////////////////////////////////////////
 // FRAME_CONFIG
 //

ArduCopter/system.pde

@@ -232,6 +232,9 @@ static void init_ardupilot()
     if(g.compass_enabled)
         init_compass();
 
+    // initialise attitude controller
+    attitude_control.set_dt(MAIN_LOOP_SECONDS);
+
     // init the optical flow sensor
     if(g.optflow_enabled) {
         init_optflow();