diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 2d391fdea2..a089c1be7a 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -271,6 +271,21 @@ void Copter::fast_loop() } } +// start takeoff to given altitude (for use by scripting) +bool Copter::start_takeoff(float alt) +{ + // exit if vehicle is not in Guided mode or Auto-Guided mode + if (!flightmode->in_guided_mode()) { + return false; + } + + if (mode_guided.do_user_takeoff_start(alt * 100.0f)) { + copter.set_auto_armed(true); + return true; + } + return false; +} + // set target location (for use by scripting) bool Copter::set_target_location(const Location& target_loc) { @@ -282,6 +297,19 @@ bool Copter::set_target_location(const Location& target_loc) return mode_guided.set_destination(target_loc); } +bool Copter::set_target_velocity_NED(const Vector3f& vel_ned) +{ + // exit if vehicle is not in Guided mode or Auto-Guided mode + if (!flightmode->in_guided_mode()) { + return false; + } + + // convert vector to neu in cm + const Vector3f vel_neu_cms(vel_ned.x * 100.0f, vel_ned.y * 100.0f, -vel_ned.z * 100.0f); + mode_guided.set_velocity(vel_neu_cms); + return true; +} + // rc_loops - reads user input from transmitter/receiver // called at 100hz void Copter::rc_loop() diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index e44833d401..6d6546327a 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -636,7 +636,9 @@ private: uint8_t &task_count, uint32_t &log_bit) override; void fast_loop() override; + bool start_takeoff(float alt) override; bool set_target_location(const Location& target_loc) override; + bool set_target_velocity_NED(const Vector3f& vel_ned) override; void rc_loop(); void throttle_loop(); void update_batt_compass(void);