mirror of https://github.com/ArduPilot/ardupilot
Copter: add set guided mode angle for scripting
This commit is contained in:
parent
4aa10a6c30
commit
e797685771
|
@ -309,6 +309,21 @@ bool Copter::set_target_velocity_NED(const Vector3f& vel_ned)
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool Copter::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs)
|
||||||
|
{
|
||||||
|
// exit if vehicle is not in Guided mode or Auto-Guided mode
|
||||||
|
if (!flightmode->in_guided_mode()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
Quaternion q;
|
||||||
|
q.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg));
|
||||||
|
|
||||||
|
mode_guided.set_angle(q,climb_rate_ms*100,use_yaw_rate,radians(yaw_rate_degs));
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
// rc_loops - reads user input from transmitter/receiver
|
// rc_loops - reads user input from transmitter/receiver
|
||||||
// called at 100hz
|
// called at 100hz
|
||||||
void Copter::rc_loop()
|
void Copter::rc_loop()
|
||||||
|
|
|
@ -643,6 +643,7 @@ private:
|
||||||
bool start_takeoff(float alt) override;
|
bool start_takeoff(float alt) override;
|
||||||
bool set_target_location(const Location& target_loc) override;
|
bool set_target_location(const Location& target_loc) override;
|
||||||
bool set_target_velocity_NED(const Vector3f& vel_ned) override;
|
bool set_target_velocity_NED(const Vector3f& vel_ned) override;
|
||||||
|
bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) override;
|
||||||
void rc_loop();
|
void rc_loop();
|
||||||
void throttle_loop();
|
void throttle_loop();
|
||||||
void update_batt_compass(void);
|
void update_batt_compass(void);
|
||||||
|
|
Loading…
Reference in New Issue