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 autopilot_check_in_flight(bool motors_on) { if (autopilot_in_flight) { if (autopilot_in_flight_counter > 0) { /* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */ if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) && (fabsf(stateGetSpeedNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_SPEED) && (fabsf(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL)) { autopilot_in_flight_counter--; if (autopilot_in_flight_counter == 0) { autopilot_in_flight = false; } } else { /* thrust, speed or accel not above min threshold, reset counter */ autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME; } } } else { /* currently not in flight */ if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME && motors_on) { /* if thrust above min threshold, assume in_flight. * Don't check for velocity and acceleration above threshold here... */ if (stabilization_cmd[COMMAND_THRUST] > AUTOPILOT_IN_FLIGHT_MIN_THRUST) { autopilot_in_flight_counter++; if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME) { autopilot_in_flight = true; } } else { /* currently not in_flight and thrust below threshold, reset counter */ autopilot_in_flight_counter = 0; } } } }
void ins_update_baro() { #if USE_BAROMETER // TODO update kalman filter with baro struct if (baro.status == BS_RUNNING) { if (!ins_baro_initialised) { ins_qfe = baro.absolute; ins_baro_initialised = TRUE; } if (ins.vf_realign) { ins.vf_realign = FALSE; ins_qfe = baro.absolute; } else { /* not realigning, so normal update with baro measurement */ /* altitude decreases with increasing baro.absolute pressure */ ins_baro_alt = ground_alt - (baro.absolute - ins_qfe) * INS_BARO_SENS; /* run the filter */ EstimatorSetAlt(ins_baro_alt); /* set new altitude, just copy old horizontal position */ struct UtmCoor_f utm; UTM_COPY(utm, *stateGetPositionUtm_f()); utm.alt = ins_alt; stateSetPositionUtm_f(&utm); struct NedCoor_f ned_vel; memcpy(&ned_vel, stateGetSpeedNed_f(), sizeof(struct NedCoor_f)); ned_vel.z = -ins_alt_dot; stateSetSpeedNed_f(&ned_vel); } } #endif }
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); }
static void mavlink_send_global_position_int(struct transport_tx *trans, struct link_device *dev) { float heading = DegOfRad(stateGetNedToBodyEulers_f()->psi); if (heading < 0.) { heading += 360; } uint16_t compass_heading = heading * 100; int32_t relative_alt = stateGetPositionLla_i()->alt - state.ned_origin_f.hmsl; /// TODO: check/ask what coordinate system vel is supposed to be in, not clear from docs mavlink_msg_global_position_int_send(MAVLINK_COMM_0, get_sys_time_msec(), stateGetPositionLla_i()->lat, stateGetPositionLla_i()->lon, stateGetPositionLla_i()->alt, relative_alt, stateGetSpeedNed_f()->x * 100, stateGetSpeedNed_f()->y * 100, stateGetSpeedNed_f()->z * 100, compass_heading); MAVLinkSendMessage(); }
/** * Send Metrics typically displayed on a HUD for fixed wing aircraft. */ static void mavlink_send_vfr_hud(struct transport_tx *trans, struct link_device *dev) { /* Current heading in degrees, in compass units (0..360, 0=north) */ int16_t heading = DegOfRad(stateGetNedToBodyEulers_f()->psi); /* Current throttle setting in integer percent, 0 to 100 */ // is a 16bit unsigned int but supposed to be from 0 to 100?? uint16_t throttle; #ifdef COMMAND_THRUST throttle = commands[COMMAND_THRUST] / (MAX_PPRZ / 100); #elif defined COMMAND_THROTTLE throttle = commands[COMMAND_THROTTLE] / (MAX_PPRZ / 100); #endif mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, stateGetAirspeed_f(), stateGetHorizontalSpeedNorm_f(), // groundspeed heading, throttle, stateGetPositionLla_f()->alt, // altitude, FIXME: should be MSL stateGetSpeedNed_f()->z); // climb rate MAVLinkSendMessage(); }
/** * Update the controls based on a vision result * @param[in] *result The opticflow calculation result used for control */ void OA_update() { float v_x = 0; float v_y = 0; if (opti_speed_flag == 1) { //rotation_vector.psi = stateGetNedToBodyEulers_f()->psi; //opti_speed = stateGetSpeedNed_f(); //Transform to body frame. //float_rmat_of_eulers_312(&T, &rotation_vector)Attractforce_goal_send; //MAT33_VECT3_MUL(*opti_speed, T, *opti_speed); // Calculate the speed in body frame struct FloatVect2 speed_cur; float psi = stateGetNedToBodyEulers_f()->psi; float s_psi = sin(psi); float c_psi = cos(psi); speed_cur.x = c_psi * stateGetSpeedNed_f()->x + s_psi * stateGetSpeedNed_f()->y; speed_cur.y = -s_psi * stateGetSpeedNed_f()->x + c_psi * stateGetSpeedNed_f()->y; opti_speed_read.x = speed_cur.x * 100; opti_speed_read.y = speed_cur.y * 100; //set result_vel v_x = speed_cur.y * 100; v_y = speed_cur.x * 100; } else { } if (OA_method_flag == NO_OBSTACLE_AVOIDANCE) { /* Calculate the error if we have enough flow */ opticflow_stab.desired_vx = 0; opticflow_stab.desired_vy = 0; err_vx = opticflow_stab.desired_vx - v_x; err_vy = opticflow_stab.desired_vy - v_y; /* Calculate the integrated errors (TODO: bound??) */ opticflow_stab.err_vx_int += err_vx / 100; opticflow_stab.err_vy_int += err_vy / 100; /* Calculate the commands */ opticflow_stab.cmd.phi = opticflow_stab.phi_pgain * err_vx / 100 + opticflow_stab.phi_igain * opticflow_stab.err_vx_int; opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100 + opticflow_stab.theta_igain * opticflow_stab.err_vy_int); /* Bound the roll and pitch commands */ BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT); BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT); } if (OA_method_flag == PINGPONG) { opticflow_stab.cmd.phi = ANGLE_BFP_OF_REAL(ref_roll); opticflow_stab.cmd.theta = ANGLE_BFP_OF_REAL(ref_pitch); } if (OA_method_flag == 2) { Total_Kan_x = r_dot_new; Total_Kan_y = speed_pot; alpha_fil = 0.1; yaw_rate = (int32_t)(alpha_fil * ANGLE_BFP_OF_REAL(r_dot_new)); opticflow_stab.cmd.psi = stateGetNedToBodyEulers_i()->psi + yaw_rate; INT32_ANGLE_NORMALIZE(opticflow_stab.cmd.psi); opticflow_stab.desired_vx = 0; opticflow_stab.desired_vy = speed_pot; /* Calculate the error if we have enough flow */ err_vx = opticflow_stab.desired_vx - v_x; err_vy = opticflow_stab.desired_vy - v_y; /* Calculate the integrated errors (TODO: bound??) */ opticflow_stab.err_vx_int += err_vx / 100; opticflow_stab.err_vy_int += err_vy / 100; /* Calculate the commands */ opticflow_stab.cmd.phi = opticflow_stab.phi_pgain * err_vx / 100 + opticflow_stab.phi_igain * opticflow_stab.err_vx_int; opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100 + opticflow_stab.theta_igain * opticflow_stab.err_vy_int); /* Bound the roll and pitch commands */ BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT); BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT); } if (OA_method_flag == POT_HEADING) { new_heading = ref_pitch; opticflow_stab.desired_vx = sin(new_heading) * speed_pot * 100; opticflow_stab.desired_vy = cos(new_heading) * speed_pot * 100; /* Calculate the error if we have enough flow */ err_vx = opticflow_stab.desired_vx - v_x; err_vy = opticflow_stab.desired_vy - v_y; /* Calculate the integrated errors (TODO: bound??) */ opticflow_stab.err_vx_int += err_vx / 100; opticflow_stab.err_vy_int += err_vy / 100; /* Calculate the commands */ opticflow_stab.cmd.phi = opticflow_stab.phi_pgain * err_vx / 100 + opticflow_stab.phi_igain * opticflow_stab.err_vx_int; opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100 + opticflow_stab.theta_igain * opticflow_stab.err_vy_int); /* Bound the roll and pitch commands */ BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT); BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT) }