CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/Blimp/Loiter.cpp
Views: 1798
1
#include "Blimp.h"

2


3
#include <AC_AttitudeControl/AC_PosControl.h>

4


5
#define MA 0.99

6
#define MO (1-MA)

7


8
void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled)

9
{

10
    const float dt = blimp.scheduler.get_last_loop_time_s();

11


12
    float scaler_xz_n;

13
    float xz_out = fabsf(blimp.motors->front_out) + fabsf(blimp.motors->down_out);

14
    if (xz_out > 1) {

15
        scaler_xz_n = 1 / xz_out;

16
    } else {

17
        scaler_xz_n = 1;

18
    }

19
    scaler_xz = scaler_xz*MA + scaler_xz_n*MO;

20


21
    float scaler_yyaw_n;

22
    float yyaw_out = fabsf(blimp.motors->right_out) + fabsf(blimp.motors->yaw_out);

23
    if (yyaw_out > 1) {

24
        scaler_yyaw_n = 1 / yyaw_out;

25
    } else {

26
        scaler_yyaw_n = 1;

27
    }

28
    scaler_yyaw = scaler_yyaw*MA + scaler_yyaw_n*MO;

29


30
#if HAL_LOGGING_ENABLED

31
    AP::logger().WriteStreaming("BSC", "TimeUS,xz,yyaw,xzn,yyawn",

32
                                "Qffff",

33
                                AP_HAL::micros64(),

34
                                scaler_xz, scaler_yyaw, scaler_xz_n, scaler_yyaw_n);

35
#endif

36


37
    float yaw_ef = blimp.ahrs.get_yaw();

38
    Vector3f err_xyz = target_pos - blimp.pos_ned;

39
    float err_yaw = wrap_PI(target_yaw - yaw_ef);

40


41
    Vector4b zero;

42
    if ((fabsf(err_xyz.x) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(2-1)))) {

43
        zero.x = true;

44
    }

45
    if ((fabsf(err_xyz.y) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(1-1)))) {

46
        zero.y = true;

47
    }

48
    if ((fabsf(err_xyz.z) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(3-1)))) {

49
        zero.z = true;

50
    }

51
    if ((fabsf(err_yaw)   < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(4-1)))) {

52
        zero.yaw = true;

53
    }

54


55
    //Disabled means "don't update PIDs or output anything at all". Zero means actually output zero thrust. I term is limited in either case."

56
    Vector4b limit = zero || axes_disabled;

57


58
    Vector3f target_vel_ef;

59
    if (!axes_disabled.x && !axes_disabled.y) target_vel_ef = {blimp.pid_pos_xy.update_all(target_pos, blimp.pos_ned, dt, {(float)limit.x, (float)limit.y, (float)limit.z}), 0};

60
    if (!axes_disabled.z) {

61
        target_vel_ef.z = blimp.pid_pos_z.update_all(target_pos.z, blimp.pos_ned.z, dt, limit.z);

62
    }

63


64
    float target_vel_yaw = 0;

65
    if (!axes_disabled.yaw) {

66
        target_vel_yaw = blimp.pid_pos_yaw.update_error(wrap_PI(target_yaw - yaw_ef), dt, limit.yaw);

67
        blimp.pid_pos_yaw.set_target_rate(target_yaw);

68
        blimp.pid_pos_yaw.set_actual_rate(yaw_ef);

69
    }

70


71
    Vector3f target_vel_ef_c{constrain_float(target_vel_ef.x, -blimp.g.max_vel_xy, blimp.g.max_vel_xy),

72
                             constrain_float(target_vel_ef.y, -blimp.g.max_vel_xy, blimp.g.max_vel_xy),

73
                             constrain_float(target_vel_ef.z, -blimp.g.max_vel_z, blimp.g.max_vel_z)};

74
    float target_vel_yaw_c = constrain_float(target_vel_yaw, -blimp.g.max_vel_yaw, blimp.g.max_vel_yaw);

75


76
    Vector2f target_vel_ef_c_scaled_xy = {target_vel_ef_c.x * scaler_xz, target_vel_ef_c.y * scaler_yyaw};

77
    Vector2f vel_ned_filtd_scaled_xy = {blimp.vel_ned_filtd.x * scaler_xz, blimp.vel_ned_filtd.y * scaler_yyaw};

78


79
    Vector2f actuator;

80
    if (!axes_disabled.x && !axes_disabled.y) actuator = blimp.pid_vel_xy.update_all(target_vel_ef_c_scaled_xy, vel_ned_filtd_scaled_xy, dt, {(float)limit.x, (float)limit.y});

81
    float act_down = 0;

82
    if (!axes_disabled.z) {

83
        act_down = blimp.pid_vel_z.update_all(target_vel_ef_c.z * scaler_xz, blimp.vel_ned_filtd.z * scaler_xz, dt, limit.z);

84
    }

85
    blimp.rotate_NE_to_BF(actuator);

86
    float act_yaw = 0;

87
    if (!axes_disabled.yaw) {

88
        act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c * scaler_yyaw, blimp.vel_yaw_filtd * scaler_yyaw, dt, limit.yaw);

89
    }

90


91
    if (!blimp.motors->armed()) {

92
        blimp.pid_pos_xy.set_integrator(Vector2f(0,0));

93
        blimp.pid_pos_z.set_integrator(0);

94
        blimp.pid_pos_yaw.set_integrator(0);

95
        blimp.pid_vel_xy.set_integrator(Vector2f(0,0));

96
        blimp.pid_vel_z.set_integrator(0);

97
        blimp.pid_vel_yaw.set_integrator(0);

98
        target_pos = blimp.pos_ned;

99
        target_yaw = blimp.ahrs.get_yaw();

100
    }

101


102
    if (zero.x) {

103
        blimp.motors->front_out = 0;

104
    } else if (axes_disabled.x);

105
    else {

106
        blimp.motors->front_out = actuator.x;

107
    }

108
    if (zero.y) {

109
        blimp.motors->right_out = 0;

110
    } else if (axes_disabled.y);

111
    else {

112
        blimp.motors->right_out = actuator.y;

113
    }

114
    if (zero.z) {

115
        blimp.motors->down_out = 0;

116
    } else if (axes_disabled.z);

117
    else {

118
        blimp.motors->down_out = act_down;

119
    }

120
    if (zero.yaw) {

121
        blimp.motors->yaw_out  = 0;

122
    } else if (axes_disabled.yaw);

123
    else {

124
        blimp.motors->yaw_out = act_yaw;

125
    }

126


127
#if HAL_LOGGING_ENABLED

128
    AC_PosControl::Write_PSCN(0.0, target_pos.x * 100.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0);

129
    AC_PosControl::Write_PSCE(0.0, target_pos.y * 100.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0);

130
    AC_PosControl::Write_PSCD(0.0, -target_pos.z * 100.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0);

131
#endif

132
}

133


134
void Loiter::run_vel(Vector3f& target_vel_ef, float& target_vel_yaw, Vector4b axes_disabled)

135
{

136
    const float dt = blimp.scheduler.get_last_loop_time_s();

137


138
    Vector4b zero;

139
    if (!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(2-1)))) {

140
        zero.x = true;

141
    }

142
    if (!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(1-1)))) {

143
        zero.y = true;

144
    }

145
    if (!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(3-1)))) {

146
        zero.z = true;

147
    }

148
    if (!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(4-1)))) {

149
        zero.yaw = true;

150
    }

151
    //Disabled means "don't update PIDs or output anything at all". Zero means actually output zero thrust. I term is limited in either case."

152
    Vector4b limit = zero || axes_disabled;

153


154
    Vector3f target_vel_ef_c{constrain_float(target_vel_ef.x, -blimp.g.max_vel_xy, blimp.g.max_vel_xy),

155
                             constrain_float(target_vel_ef.y, -blimp.g.max_vel_xy, blimp.g.max_vel_xy),

156
                             constrain_float(target_vel_ef.z, -blimp.g.max_vel_z, blimp.g.max_vel_z)};

157
    float target_vel_yaw_c = constrain_float(target_vel_yaw, -blimp.g.max_vel_yaw, blimp.g.max_vel_yaw);

158


159
    Vector2f target_vel_ef_c_scaled_xy = {target_vel_ef_c.x * scaler_xz, target_vel_ef_c.y * scaler_yyaw};

160
    Vector2f vel_ned_filtd_scaled_xy = {blimp.vel_ned_filtd.x * scaler_xz, blimp.vel_ned_filtd.y * scaler_yyaw};

161


162
    Vector2f actuator;

163
    if (!axes_disabled.x && !axes_disabled.y) actuator = blimp.pid_vel_xy.update_all(target_vel_ef_c_scaled_xy, vel_ned_filtd_scaled_xy, dt, {(float)limit.x, (float)limit.y});

164
    float act_down = 0;

165
    if (!axes_disabled.z) {

166
        act_down = blimp.pid_vel_z.update_all(target_vel_ef_c.z * scaler_xz, blimp.vel_ned_filtd.z * scaler_xz, dt, limit.z);

167
    }

168
    blimp.rotate_NE_to_BF(actuator);

169
    float act_yaw = 0;

170
    if (!axes_disabled.yaw) {

171
        act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c * scaler_yyaw, blimp.vel_yaw_filtd * scaler_yyaw, dt, limit.yaw);

172
    }

173


174
    if (!blimp.motors->armed()) {

175
        blimp.pid_vel_xy.set_integrator(Vector2f(0,0));

176
        blimp.pid_vel_z.set_integrator(0);

177
        blimp.pid_vel_yaw.set_integrator(0);

178
    }

179


180
    if (zero.x) {

181
        blimp.motors->front_out = 0;

182
    } else if (axes_disabled.x);

183
    else {

184
        blimp.motors->front_out = actuator.x;

185
    }

186
    if (zero.y) {

187
        blimp.motors->right_out = 0;

188
    } else if (axes_disabled.y);

189
    else {

190
        blimp.motors->right_out = actuator.y;

191
    }

192
    if (zero.z) {

193
        blimp.motors->down_out = 0;

194
    } else if (axes_disabled.z);

195
    else {

196
        blimp.motors->down_out = act_down;

197
    }

198
    if (zero.yaw) {

199
        blimp.motors->yaw_out  = 0;

200
    } else if (axes_disabled.yaw);

201
    else {

202
        blimp.motors->yaw_out = act_yaw;

203
    }

204


205
#if HAL_LOGGING_ENABLED

206
    AC_PosControl::Write_PSCN(0.0, 0.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0);

207
    AC_PosControl::Write_PSCE(0.0, 0.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0);

208
    AC_PosControl::Write_PSCD(0.0, 0.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0);

209
#endif

210
}

211


212