diff --git a/ArduCopter/control_stabilize.pde b/ArduCopter/control_stabilize.pde index 89b654a621..5ff40406f3 100644 --- a/ArduCopter/control_stabilize.pde +++ b/ArduCopter/control_stabilize.pde @@ -1,5 +1,11 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +// acro_init - initialise acro controller +static bool acro_init() +{ + return true; +} + // acro_run - runs the acro controller // should be called at 100hz or more static void acro_run() @@ -32,6 +38,12 @@ static void acro_run() // body-frame rate controller is run directly from 100hz loop } +// stabilize_init - initialise stabilize controller +static bool stabilize_init() +{ + return true; +} + // stabilize_run - runs the main stabilize controller // should be called at 100hz or more static void stabilize_run() @@ -88,12 +100,24 @@ static void stabilize_run() // To-Do: add throttle control for stabilize mode here? } +// althold_init - initialise althold controller +static bool althold_init() +{ + return true; +} + // althold_run - runs the althold controller // should be called at 100hz or more static void althold_run() { } +// auto_init - initialise auto controller +static bool auto_init() +{ + return true; +} + // auto_run - runs the auto controller // should be called at 100hz or more static void auto_run() @@ -127,24 +151,48 @@ static void auto_run() // body-frame rate controller is run directly from 100hz loop } +// circle_init - initialise circle controller +static bool circle_init() +{ + return true; +} + // circle_run - runs the circle controller // should be called at 100hz or more static void circle_run() { } +// loiter_init - initialise loiter controller +static bool loiter_init() +{ + return true; +} + // loiter_run - runs the loiter controller // should be called at 100hz or more static void loiter_run() { } +// guided_init - initialise guided controller +static bool guided_init() +{ + return true; +} + // guided_run - runs the guided controller // should be called at 100hz or more static void guided_run() { } +// land_init - initialise land controller +static bool land_init() +{ + return true; +} + // land_run - runs the land controller // should be called at 100hz or more static void land_run() @@ -152,6 +200,12 @@ static void land_run() verify_land(); } +// rtl_init - initialise rtl controller +static bool rtl_init() +{ + return true; +} + // rtl_run - runs the return-to-launch controller // should be called at 100hz or more static void rtl_run() @@ -159,18 +213,36 @@ static void rtl_run() verify_RTL(); } +// ofloiter_init - initialise ofloiter controller +static bool ofloiter_init() +{ + return true; +} + // ofloiter_run - runs the optical flow loiter controller // should be called at 100hz or more static void ofloiter_run() { } +// drift_init - initialise drift controller +static bool drift_init() +{ + return true; +} + // drift_run - runs the drift controller // should be called at 100hz or more static void drift_run() { } +// sport_init - initialise sport controller +static bool sport_init() +{ + return true; +} + // sport_run - runs the sport controller // should be called at 100hz or more static void sport_run() diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index b83a9456ff..e52c69d494 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -384,7 +384,7 @@ static bool set_mode(uint8_t mode) switch(mode) { case ACRO: - success = true; + success = acro_init(); set_yaw_mode(ACRO_YAW); set_roll_pitch_mode(ACRO_RP); set_throttle_mode(ACRO_THR); @@ -392,7 +392,7 @@ static bool set_mode(uint8_t mode) break; case STABILIZE: - success = true; + success = stabilize_init(); set_yaw_mode(STABILIZE_YAW); set_roll_pitch_mode(STABILIZE_RP); set_throttle_mode(STABILIZE_THR); @@ -400,7 +400,7 @@ static bool set_mode(uint8_t mode) break; case ALT_HOLD: - success = true; + success = althold_init(); set_yaw_mode(ALT_HOLD_YAW); set_roll_pitch_mode(ALT_HOLD_RP); set_throttle_mode(ALT_HOLD_THR); @@ -410,15 +410,15 @@ static bool set_mode(uint8_t mode) case AUTO: // check we have a GPS and at least one mission command (note the home position is always command 0) if ((GPS_ok() && g.command_total > 1) || ignore_checks) { - success = true; + success = auto_init(); // roll-pitch, throttle and yaw modes will all be set by the first nav command init_commands(); // clear the command queues. will be reloaded when "run_autopilot" calls "update_commands" function - } + } break; case CIRCLE: if (GPS_ok() || ignore_checks) { - success = true; + success = circle_init(); set_roll_pitch_mode(CIRCLE_RP); set_throttle_mode(CIRCLE_THR); set_nav_mode(CIRCLE_NAV); @@ -428,7 +428,7 @@ static bool set_mode(uint8_t mode) case LOITER: if (GPS_ok() || ignore_checks) { - success = true; + success = loiter_init(); set_yaw_mode(LOITER_YAW); set_roll_pitch_mode(LOITER_RP); set_throttle_mode(LOITER_THR); @@ -448,7 +448,7 @@ static bool set_mode(uint8_t mode) case GUIDED: if (GPS_ok() || ignore_checks) { - success = true; + success = guided_init(); set_yaw_mode(get_wp_yaw_mode(false)); set_roll_pitch_mode(GUIDED_RP); set_throttle_mode(GUIDED_THR); @@ -457,20 +457,20 @@ static bool set_mode(uint8_t mode) break; case LAND: - success = true; + success = land_init(); do_land(NULL); // land at current location break; case RTL: if (GPS_ok() || ignore_checks) { - success = true; + success = rtl_init(); do_RTL(); } break; case OF_LOITER: if (g.optflow_enabled || ignore_checks) { - success = true; + success = ofloiter_init(); set_yaw_mode(OF_LOITER_YAW); set_roll_pitch_mode(OF_LOITER_RP); set_throttle_mode(OF_LOITER_THR); @@ -479,7 +479,7 @@ static bool set_mode(uint8_t mode) break; case DRIFT: - success = true; + success = drift_init(); set_yaw_mode(YAW_DRIFT); set_roll_pitch_mode(ROLL_PITCH_DRIFT); set_nav_mode(NAV_NONE); @@ -487,7 +487,7 @@ static bool set_mode(uint8_t mode) break; case SPORT: - success = true; + success = sport_init(); set_yaw_mode(SPORT_YAW); set_roll_pitch_mode(SPORT_RP); set_throttle_mode(SPORT_THR);