/* Set guided mode setpoint
* Note: Offset position command in NED frame or body frame will only be implemented if
* local reference frame has been initialised.
* Flag definition:
bit 0: x,y as offset coordinates
bit 1: x,y in body coordinates
bit 2: z as offset coordinates
bit 3: yaw as offset coordinates
bit 4: free
bit 5: x,y as vel
bit 6: z as vel
bit 7: yaw as rate
*/
void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw)
{
/* only update setpoints when in guided mode */
if (autopilot_mode != AP_MODE_GUIDED) {
return;
}
// handle x,y
struct FloatVect2 setpoint = {.x = x, .y = y};
if (bit_is_set(flags, 5)) { // velocity setpoint
if (bit_is_set(flags, 1)) { // set velocity in body frame
guidance_h_set_guided_body_vel(setpoint.x, setpoint.y);
} else {
guidance_h_set_guided_vel(setpoint.x, setpoint.y);
}
} else { // position setpoint
if (!bit_is_set(flags, 0) && !bit_is_set(flags, 1)) { // set absolute position setpoint
guidance_h_set_guided_pos(setpoint.x, setpoint.y);
} else {
if (stateIsLocalCoordinateValid()) {
if (bit_is_set(flags, 1)) { // set position as offset in body frame
float psi = stateGetNedToBodyEulers_f()->psi;
setpoint.x = stateGetPositionNed_f()->x + cosf(-psi) * x + sinf(-psi) * y;
setpoint.y = stateGetPositionNed_f()->y - sinf(-psi) * x + cosf(-psi) * y;
} else { // set position as offset in NED frame
setpoint.x += stateGetPositionNed_f()->x;
setpoint.y += stateGetPositionNed_f()->y;
}
guidance_h_set_guided_pos(setpoint.x, setpoint.y);
}
}
}
//handle z
if (bit_is_set(flags, 6)) { // speed set-point
guidance_v_set_guided_vz(z);
} else { // position set-point
if (bit_is_set(flags, 2)) { // set position as offset in NED frame
if (stateIsLocalCoordinateValid()) {
z += stateGetPositionNed_f()->z;
guidance_v_set_guided_z(z);
}
} else {
guidance_v_set_guided_z(z);
}
}
//handle yaw
if (bit_is_set(flags, 7)) { // speed set-point
guidance_h_set_guided_heading_rate(yaw);
} else { // position set-point
if (bit_is_set(flags, 3)) { // set yaw as offset
yaw += stateGetNedToBodyEulers_f()->psi; // will be wrapped to [-pi,pi] later
}
guidance_h_set_guided_heading(yaw);
}
}
bool autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw)
{
if (autopilot_mode == AP_MODE_GUIDED && stateIsLocalCoordinateValid()) {
float x = stateGetPositionNed_f()->x + dx;
float y = stateGetPositionNed_f()->y + dy;
float z = stateGetPositionNed_f()->z + dz;
float heading = stateGetNedToBodyEulers_f()->psi + dyaw;
return autopilot_guided_goto_ned(x, y, z, heading);
}
return false;
}
static void mavlink_send_local_position_ned(struct transport_tx *trans, struct link_device *dev)
{
mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
get_sys_time_msec(),
stateGetPositionNed_f()->x,
stateGetPositionNed_f()->y,
stateGetPositionNed_f()->z,
stateGetSpeedNed_f()->x,
stateGetSpeedNed_f()->y,
stateGetSpeedNed_f()->z);
MAVLinkSendMessage();
}
static inline void mavlink_send_local_position_ned(void)
{
mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
get_sys_time_msec(),
stateGetPositionNed_f()->x,
stateGetPositionNed_f()->y,
stateGetPositionNed_f()->z,
stateGetSpeedNed_f()->x,
stateGetSpeedNed_f()->y,
stateGetSpeedNed_f()->z);
MAVLinkSendMessage();
}
void CN_calculate_target(void)
{
current_pos = *stateGetPositionNed_f();
float psi = stateGetNedToBodyEulers_f()->psi;
float s_psi = sin(psi);
float c_psi = cos(psi);
float s_waypoint_rot = sin(waypoint_rot);
float c_waypoint_rot = cos(waypoint_rot);
float dx_ref_NED;
float dy_ref_NED;
if (OA_method_flag == 2) {
dx_ref_NED = dx_ref;
dy_ref_NED = dy_ref;
} else {
dx_ref_NED = c_psi * dx_ref - s_psi * dy_ref;
dy_ref_NED = s_psi * dx_ref + c_psi * dy_ref;
}
target.x = init_target.x + dx_ref_NED;
target.y = init_target.y + dy_ref_NED;
//apply rotation of waypoint
if (OA_method_flag == 6) {
target.x = c_waypoint_rot * target.x - s_waypoint_rot * target.y;
target.y = s_waypoint_rot * target.x + c_waypoint_rot * target.y;
}
pos_diff.x = target.x - current_pos.x;
pos_diff.y = target.y - current_pos.y;
heading_goal_f = atan2(pos_diff.y, pos_diff.x);
}
void guidance_indi_run(bool in_flight, int32_t heading) {
//filter accel to get rid of noise
//filter attitude to synchronize with accel
guidance_indi_filter_attitude();
guidance_indi_filter_accel();
float pos_x_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.x) - stateGetPositionNed_f()->x; //+-ov.y/ (STABILIZATION_ATTITUDE_SP_MAX_THETA)*3.0;
float pos_y_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.y) - stateGetPositionNed_f()->y; //+ ov.x/ (STABILIZATION_ATTITUDE_SP_MAX_PHI)*3.0;
float speed_sp_x = pos_x_err*guidance_indi_pos_gain;
float speed_sp_y = pos_y_err*guidance_indi_pos_gain;
sp_accel.x = (speed_sp_x - stateGetSpeedNed_f()->x)*guidance_indi_speed_gain;
sp_accel.y = (speed_sp_y - stateGetSpeedNed_f()->y)*guidance_indi_speed_gain;
// sp_accel.x = (radio_control.values[RADIO_PITCH]/9600.0)*8.0;
// sp_accel.y = -(radio_control.values[RADIO_ROLL]/9600.0)*8.0;
// struct FloatMat33 Ga;
guidance_indi_calcG(&Ga);
MAT33_INV(Ga_inv, Ga);
float altitude_sp = POS_FLOAT_OF_BFP(guidance_v_z_sp);
float vertical_velocity_sp = guidance_indi_pos_gain*(altitude_sp - stateGetPositionNed_f()->z);
// float vertical_velocity_rc_euler = -(stabilization_cmd[COMMAND_THRUST]-4500.0)/4500.0*2.0;
float vertical_velocity_err_euler = vertical_velocity_sp - stateGetSpeedNed_f()->z;
sp_accel.z = vertical_velocity_err_euler*guidance_indi_speed_gain;
struct FloatVect3 a_diff = { sp_accel.x - filt_accel_ned.x, sp_accel.y -filt_accel_ned.y, 0.0};
Bound(a_diff.x, -6.0, 6.0);
Bound(a_diff.y, -6.0, 6.0);
Bound(a_diff.z, -9.0, 9.0);
MAT33_VECT3_MUL(euler_cmd, Ga_inv, a_diff);
guidance_euler_cmd.phi = roll_filt + euler_cmd.x;
guidance_euler_cmd.theta = pitch_filt + euler_cmd.y;
guidance_euler_cmd.psi = 0;//stateGetNedToBodyEulers_f()->psi;
//Bound euler angles to prevent flipping and keep upright
Bound(guidance_euler_cmd.phi, -GUIDANCE_H_MAX_BANK, GUIDANCE_H_MAX_BANK);
Bound(guidance_euler_cmd.theta, -GUIDANCE_H_MAX_BANK, GUIDANCE_H_MAX_BANK);
stabilization_attitude_set_setpoint_rp_quat_f(in_flight, heading);
}
