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)
}