Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen
Diff: Autopilot.cpp
- Revision:
- 14:7b6c3f8b85fb
- Parent:
- 13:75636841e43b
- Child:
- 15:28f480b41553
diff -r 75636841e43b -r 7b6c3f8b85fb Autopilot.cpp --- a/Autopilot.cpp Wed Dec 01 09:23:26 2021 +0000 +++ b/Autopilot.cpp Wed Dec 01 11:51:21 2021 +0000 @@ -23,18 +23,28 @@ this->vel_obj = vel; } -void Autopilot::update_val(const Vector3 rpy, const float altitude, const Matrix pos, const Matrix vel) +void Autopilot::set_climb(float path_ang, float vdot) +{ + this->path_obj = path_ang; + this->vdot_obj = vdot; +} + +void Autopilot::update_val(const Vector3 rpy, const float altitude, const Matrix pos, const Matrix vel, const Vector3 v_dot) { this->roll = rpy.x; this->pitch = rpy.y; this->yaw = rpy.z; + this->alt_before = this->alt; this->alt = altitude; + this->pos_before = this->pos_ned; this->pos_ned.x = pos(1, 1); this->pos_ned.y = pos(2, 1); this->pos_ned.z = pos(3, 1); + this->vel_before = this->vel_ned; this->vel_ned.x = vel(1, 1); this->vel_ned.y = vel(2, 1); this->vel_ned.z = vel(3, 1); + this->vdot = v_dot; } void Autopilot::level() @@ -95,6 +105,20 @@ pitch_obj = p_control(Bsp - B, 0.05f); } +void Autopilot::climb() +{ + //TECS使用(https://docs.px4.io/master/en/flight_stack/controller_diagrams.html), p制御のみ + float Edot = path_ang + vdot.Norm() / G; + float Edot_sp = path_obj + vdot_obj / G; + float dT_cruise = 0.0f; + dT_obj = dT_cruise; + dT_obj += p_control(Edot_sp - Edot, 10.0f); + + float Bdot = path_ang - vdot.Norm() / G; + float Bdot_sp = path_obj - vdot_obj / G; + pitch_obj = p_control(Bdot_sp - Bdot, 10.0f); +} + void Autopilot::return_val(float &r_obj, float &p_obj, float &t_obj) { limit_obj(); @@ -147,4 +171,11 @@ float Autopilot::deg2rad(float deg) { return deg * M_PI / 180.0f; +} + +float Autopilot::calc_path() +{ + float alt_diff = alt - alt_before; + float xy_diff = sqrt((pos_ned.x - pos_before.x)*(pos_ned.x - pos_before.x) + (pos_ned.y - pos_before.y)*(pos_ned.y - pos_before.y)); + path_ang = atan2f(alt_diff, xy_diff); } \ No newline at end of file