static inline void autopilot_check_in_flight( void) {
if (autopilot_in_flight) {
if (autopilot_in_flight_counter > 0) {
if (THROTTLE_STICK_DOWN()) {
autopilot_in_flight_counter--;
if (autopilot_in_flight_counter == 0) {
autopilot_in_flight = FALSE;
}
}
else { /* !THROTTLE_STICK_DOWN */
autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME;
}
}
}
else { /* not in flight */
if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME &&
autopilot_motors_on) {
if (!THROTTLE_STICK_DOWN()) {
autopilot_in_flight_counter++;
if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME)
autopilot_in_flight = TRUE;
}
else { /* THROTTLE_STICK_DOWN */
autopilot_in_flight_counter = 0;
}
}
}
}
//Read rc with roll and yaw sitcks switched if the default orientation is vertical but airplane sticks are desired
void stabilization_rate_read_rc_switched_sticks(void)
{
if (ROLL_RATE_DEADBAND_EXCEEDED()) {
stabilization_rate_sp.r = (int32_t) - radio_control.values[RADIO_ROLL] * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ;
} else {
stabilization_rate_sp.r = 0;
}
if (PITCH_RATE_DEADBAND_EXCEEDED()) {
stabilization_rate_sp.q = (int32_t)radio_control.values[RADIO_PITCH] * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ;
} else {
stabilization_rate_sp.q = 0;
}
if (YAW_RATE_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
stabilization_rate_sp.p = (int32_t)radio_control.values[RADIO_YAW] * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ;
} else {
stabilization_rate_sp.p = 0;
}
}
void stabilization_rate_read_rc(void)
{
if (ROLL_RATE_DEADBAND_EXCEEDED()) {
stabilization_rate_sp.p = (int32_t)radio_control.values[RADIO_ROLL] * RATE_BFP_OF_REAL(STABILIZATION_RATE_SP_MAX_P) / MAX_PPRZ;
} else {
stabilization_rate_sp.p = 0;
}
if (PITCH_RATE_DEADBAND_EXCEEDED()) {
stabilization_rate_sp.q = (int32_t)radio_control.values[RADIO_PITCH] * RATE_BFP_OF_REAL(STABILIZATION_RATE_SP_MAX_Q) / MAX_PPRZ;
} else {
stabilization_rate_sp.q = 0;
}
if (YAW_RATE_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
stabilization_rate_sp.r = (int32_t)radio_control.values[RADIO_YAW] * RATE_BFP_OF_REAL(STABILIZATION_RATE_SP_MAX_R) / MAX_PPRZ;
} else {
stabilization_rate_sp.r = 0;
}
}
/**
* State machine to check if motors should be turned ON or OFF
* The motors start/stop when pushing the yaw stick without throttle during a given time
* An intermediate state prevents oscillating between ON and OFF while keeping the stick pushed
* The stick must return to a neutral position before starting/stoping again
*/
static inline void autopilot_check_motors_on( void ) {
switch(autopilot_check_motor_status) {
case STATUS_MOTORS_OFF:
autopilot_motors_on = FALSE;
autopilot_motors_on_counter = 0;
if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) // stick pushed
autopilot_check_motor_status = STATUS_M_OFF_STICK_PUSHED;
break;
case STATUS_M_OFF_STICK_PUSHED:
autopilot_motors_on = FALSE;
autopilot_motors_on_counter++;
if (autopilot_motors_on_counter >= AUTOPILOT_MOTOR_ON_TIME)
autopilot_check_motor_status = STATUS_START_MOTORS;
else if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // stick released too soon
autopilot_check_motor_status = STATUS_MOTORS_OFF;
break;
case STATUS_START_MOTORS:
autopilot_motors_on = TRUE;
autopilot_motors_on_counter = AUTOPILOT_MOTOR_ON_TIME;
if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // wait until stick released
autopilot_check_motor_status = STATUS_MOTORS_ON;
break;
case STATUS_MOTORS_ON:
autopilot_motors_on = TRUE;
autopilot_motors_on_counter = AUTOPILOT_MOTOR_ON_TIME;
if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) // stick pushed
autopilot_check_motor_status = STATUS_M_ON_STICK_PUSHED;
break;
case STATUS_M_ON_STICK_PUSHED:
autopilot_motors_on = TRUE;
autopilot_motors_on_counter--;
if (autopilot_motors_on_counter == 0)
autopilot_check_motor_status = STATUS_STOP_MOTORS;
else if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // stick released too soon
autopilot_check_motor_status = STATUS_MOTORS_ON;
break;
case STATUS_STOP_MOTORS:
autopilot_motors_on = FALSE;
autopilot_motors_on_counter = 0;
if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // wait until stick released
autopilot_check_motor_status = STATUS_MOTORS_OFF;
break;
default:
break;
};
}
void autopilot_periodic(void) {
RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
#if FAILSAFE_GROUND_DETECT
INFO("Using FAILSAFE_GROUND_DETECT")
if (autopilot_mode == AP_MODE_FAILSAFE && autopilot_detect_ground) {
autopilot_set_mode(AP_MODE_KILL);
autopilot_detect_ground = FALSE;
}
#endif
/* set failsafe commands, if in FAILSAFE or KILL mode */
#if !FAILSAFE_GROUND_DETECT
if (autopilot_mode == AP_MODE_KILL ||
autopilot_mode == AP_MODE_FAILSAFE) {
#else
if (autopilot_mode == AP_MODE_KILL) {
#endif
SetCommands(commands_failsafe);
}
else {
guidance_v_run( autopilot_in_flight );
guidance_h_run( autopilot_in_flight );
SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on);
}
}
void autopilot_set_mode(uint8_t new_autopilot_mode) {
/* force kill mode as long as AHRS is not aligned */
if (!ahrs_is_aligned())
new_autopilot_mode = AP_MODE_KILL;
if (new_autopilot_mode != autopilot_mode) {
/* horizontal mode */
switch (new_autopilot_mode) {
case AP_MODE_FAILSAFE:
#ifndef KILL_AS_FAILSAFE
stab_att_sp_euler.phi = 0;
stab_att_sp_euler.theta = 0;
guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
break;
#endif
case AP_MODE_KILL:
autopilot_set_motors_on(FALSE);
autopilot_in_flight = FALSE;
autopilot_in_flight_counter = 0;
guidance_h_mode_changed(GUIDANCE_H_MODE_KILL);
break;
case AP_MODE_RC_DIRECT:
guidance_h_mode_changed(GUIDANCE_H_MODE_RC_DIRECT);
break;
case AP_MODE_RATE_DIRECT:
case AP_MODE_RATE_Z_HOLD:
guidance_h_mode_changed(GUIDANCE_H_MODE_RATE);
break;
case AP_MODE_ATTITUDE_RC_CLIMB:
case AP_MODE_ATTITUDE_DIRECT:
case AP_MODE_ATTITUDE_CLIMB:
case AP_MODE_ATTITUDE_Z_HOLD:
guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
break;
case AP_MODE_FORWARD:
guidance_h_mode_changed(GUIDANCE_H_MODE_FORWARD);
break;
case AP_MODE_CARE_FREE_DIRECT:
guidance_h_mode_changed(GUIDANCE_H_MODE_CARE_FREE);
break;
case AP_MODE_HOVER_DIRECT:
case AP_MODE_HOVER_CLIMB:
case AP_MODE_HOVER_Z_HOLD:
guidance_h_mode_changed(GUIDANCE_H_MODE_HOVER);
break;
case AP_MODE_NAV:
guidance_h_mode_changed(GUIDANCE_H_MODE_NAV);
break;
default:
break;
}
/* vertical mode */
switch (new_autopilot_mode) {
case AP_MODE_FAILSAFE:
#ifndef KILL_AS_FAILSAFE
guidance_v_zd_sp = SPEED_BFP_OF_REAL(0.5);
guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
break;
#endif
case AP_MODE_KILL:
guidance_v_mode_changed(GUIDANCE_V_MODE_KILL);
break;
case AP_MODE_RC_DIRECT:
case AP_MODE_RATE_DIRECT:
case AP_MODE_ATTITUDE_DIRECT:
case AP_MODE_HOVER_DIRECT:
case AP_MODE_CARE_FREE_DIRECT:
case AP_MODE_FORWARD:
guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT);
break;
case AP_MODE_RATE_RC_CLIMB:
case AP_MODE_ATTITUDE_RC_CLIMB:
guidance_v_mode_changed(GUIDANCE_V_MODE_RC_CLIMB);
break;
case AP_MODE_ATTITUDE_CLIMB:
case AP_MODE_HOVER_CLIMB:
guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
break;
case AP_MODE_RATE_Z_HOLD:
case AP_MODE_ATTITUDE_Z_HOLD:
case AP_MODE_HOVER_Z_HOLD:
guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER);
break;
case AP_MODE_NAV:
guidance_v_mode_changed(GUIDANCE_V_MODE_NAV);
break;
default:
break;
}
autopilot_mode = new_autopilot_mode;
}
}
static inline void autopilot_check_in_flight( bool_t motors_on ) {
if (autopilot_in_flight) {
if (autopilot_in_flight_counter > 0) {
if (THROTTLE_STICK_DOWN()) {
autopilot_in_flight_counter--;
if (autopilot_in_flight_counter == 0) {
autopilot_in_flight = FALSE;
}
}
else { /* !THROTTLE_STICK_DOWN */
autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME;
}
}
}
else { /* not in flight */
if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME &&
motors_on) {
if (!THROTTLE_STICK_DOWN()) {
autopilot_in_flight_counter++;
if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME)
autopilot_in_flight = TRUE;
}
else { /* THROTTLE_STICK_DOWN */
autopilot_in_flight_counter = 0;
}
}
}
}
void autopilot_set_motors_on(bool_t motors_on) {
if (ahrs_is_aligned() && motors_on)
autopilot_motors_on = TRUE;
else
autopilot_motors_on = FALSE;
kill_throttle = ! autopilot_motors_on;
autopilot_arming_set(autopilot_motors_on);
}
void autopilot_on_rc_frame(void) {
if (kill_switch_is_on())
autopilot_set_mode(AP_MODE_KILL);
else {
uint8_t new_autopilot_mode = 0;
AP_MODE_OF_PPRZ(radio_control.values[RADIO_MODE], new_autopilot_mode);
/* don't enter NAV mode if GPS is lost (this also prevents mode oscillations) */
if (!(new_autopilot_mode == AP_MODE_NAV
#if USE_GPS
&& GpsIsLost()
#endif
))
autopilot_set_mode(new_autopilot_mode);
}
/* if not in FAILSAFE mode check motor and in_flight status, read RC */
if (autopilot_mode > AP_MODE_FAILSAFE) {
/* if there are some commands that should always be set from RC, do it */
#ifdef SetAutoCommandsFromRC
SetAutoCommandsFromRC(commands, radio_control.values);
#endif
/* if not in NAV_MODE set commands from the rc */
#ifdef SetCommandsFromRC
if (autopilot_mode != AP_MODE_NAV) {
SetCommandsFromRC(commands, radio_control.values);
}
#endif
/* an arming sequence is used to start/stop motors */
autopilot_arming_check_motors_on();
kill_throttle = ! autopilot_motors_on;
autopilot_check_in_flight(autopilot_motors_on);
guidance_v_read_rc();
guidance_h_read_rc(autopilot_in_flight);
}
}
void autopilot_periodic(void) {
RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
#if FAILSAFE_GROUND_DETECT
INFO("Using FAILSAFE_GROUND_DETECT")//使用模式FAILSAFE_GROUND_DETECT失效保护_
if (autopilot_mode == AP_MODE_FAILSAFE && autopilot_detect_ground) {
autopilot_set_mode(AP_MODE_KILL);
autopilot_detect_ground = FALSE;
}
#endif
/* set failsafe commands, if in FAILSAFE or KILL mode */
#if !FAILSAFE_GROUND_DETECT
if (autopilot_mode == AP_MODE_KILL ||
autopilot_mode == AP_MODE_FAILSAFE) {
#else
if (autopilot_mode == AP_MODE_KILL) {
#endif
SetCommands(commands_failsafe);
}
else {
/* 计算向导模式下的两个方向水平和垂直方向的姿态信息*/
guidance_v_run( autopilot_in_flight );
guidance_h_run( autopilot_in_flight );
/*设置旋翼的命令：稳定模式配置，飞行模式，电机打开状态*/
SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on);
}
}
/*飞控模式设置函数*/
void autopilot_set_mode(uint8_t new_autopilot_mode) {
/* force kill mode as long as AHRS is not aligned
强制杀死模式只要ahrs不是均衡的 */
if (!ahrs_is_aligned())
new_autopilot_mode = AP_MODE_KILL;
/* 新的飞行模式*/
if (new_autopilot_mode != autopilot_mode) {
/* horizontal mode 水平模式 */
switch (new_autopilot_mode) {
case AP_MODE_FAILSAFE://失效保护模式
#ifndef KILL_AS_FAILSAFE
stab_att_sp_euler.phi = 0;
stab_att_sp_euler.theta = 0;
guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
break;
#endif
case AP_MODE_KILL://kill模式
autopilot_set_motors_on(FALSE);
autopilot_in_flight = FALSE;
autopilot_in_flight_counter = 0;
guidance_h_mode_changed(GUIDANCE_H_MODE_KILL);
break;
case AP_MODE_RC_DIRECT://RC指挥模式
guidance_h_mode_changed(GUIDANCE_H_MODE_RC_DIRECT);
break;
case AP_MODE_RATE_DIRECT://速度指挥模式
case AP_MODE_RATE_Z_HOLD://Z轴（高度）速度指挥模式
guidance_h_mode_changed(GUIDANCE_H_MODE_RATE);
break;
case AP_MODE_ATTITUDE_RC_CLIMB://RC 姿态爬升模式
case AP_MODE_ATTITUDE_DIRECT://姿态向导模式
case AP_MODE_ATTITUDE_CLIMB://姿态爬升模式
case AP_MODE_ATTITUDE_Z_HOLD://高度保持模式
guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
break;
case AP_MODE_FORWARD://前进模式
guidance_h_mode_changed(GUIDANCE_H_MODE_FORWARD);
break;
case AP_MODE_CARE_FREE_DIRECT://自由模式
guidance_h_mode_changed(GUIDANCE_H_MODE_CARE_FREE);
break;
case AP_MODE_HOVER_DIRECT://盘旋向导模式
case AP_MODE_HOVER_CLIMB://盘旋爬升模式
case AP_MODE_HOVER_Z_HOLD://盘旋高度保持模式
guidance_h_mode_changed(GUIDANCE_H_MODE_HOVER);
break;
case AP_MODE_NAV://导航模式
guidance_h_mode_changed(GUIDANCE_H_MODE_NAV);
break;
default:
break;
}
/* vertical mode 垂直模式*/
switch (new_autopilot_mode) {
case AP_MODE_FAILSAFE:
#ifndef KILL_AS_FAILSAFE
guidance_v_zd_sp = SPEED_BFP_OF_REAL(0.5);
guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
break;
#endif
case AP_MODE_KILL:
guidance_v_mode_changed(GUIDANCE_V_MODE_KILL);
break;
case AP_MODE_RC_DIRECT:
case AP_MODE_RATE_DIRECT:
case AP_MODE_ATTITUDE_DIRECT:
case AP_MODE_HOVER_DIRECT:
case AP_MODE_CARE_FREE_DIRECT:
case AP_MODE_FORWARD:
guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT);
break;
case AP_MODE_RATE_RC_CLIMB:
case AP_MODE_ATTITUDE_RC_CLIMB:
guidance_v_mode_changed(GUIDANCE_V_MODE_RC_CLIMB);
break;
case AP_MODE_ATTITUDE_CLIMB:
case AP_MODE_HOVER_CLIMB:
guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
break;
case AP_MODE_RATE_Z_HOLD:
case AP_MODE_ATTITUDE_Z_HOLD:
case AP_MODE_HOVER_Z_HOLD:
guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER);
break;
case AP_MODE_NAV:
guidance_v_mode_changed(GUIDANCE_V_MODE_NAV);
break;
default:
break;
}
autopilot_mode = new_autopilot_mode;
}
}
static inline void autopilot_check_in_flight( bool_t motors_on ) {
if (autopilot_in_flight) {
if (autopilot_in_flight_counter > 0) {
if (THROTTLE_STICK_DOWN()) {
autopilot_in_flight_counter--;
if (autopilot_in_flight_counter == 0) {
autopilot_in_flight = FALSE;
}
}
else { /* !THROTTLE_STICK_DOWN */
autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME;
}
}
}
else { /* not in flight */
if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME &&
motors_on) {
if (!THROTTLE_STICK_DOWN()) {
autopilot_in_flight_counter++;
if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME)
autopilot_in_flight = TRUE;
}
else { /* THROTTLE_STICK_DOWN */
autopilot_in_flight_counter = 0;
}
}
}
}
void autopilot_set_motors_on(bool_t motors_on) {
if (ahrs_is_aligned() && motors_on)
autopilot_motors_on = TRUE;
else
autopilot_motors_on = FALSE;
kill_throttle = ! autopilot_motors_on;
autopilot_arming_set(autopilot_motors_on);
}
void autopilot_on_rc_frame(void) {
//是否关闭开关
if (kill_switch_is_on())
autopilot_set_mode(AP_MODE_KILL);//关闭自驾模式
else {
uint8_t new_autopilot_mode = 0;
AP_MODE_OF_PPRZ(radio_control.values[RADIO_MODE], new_autopilot_mode);
/* don't enter NAV mode if GPS is lost (this also prevents mode oscillations) */
if (!(new_autopilot_mode == AP_MODE_NAV
#if USE_GPS
&& GpsIsLost()//GPS丢失时不要使用NAV导航模式
#endif
))
autopilot_set_mode(new_autopilot_mode);
}
/* if not in FAILSAFE mode check motor and in_flight status, read RC */
//飞行模式不是：失效保护时，检查电机和飞行状态，读RC
if (autopilot_mode > AP_MODE_FAILSAFE) {
/* if there are some commands that should always be set from RC, do it */
//如果有来自于RC的命令，则去执行命令
#ifdef SetAutoCommandsFromRC
SetAutoCommandsFromRC(commands, radio_control.values);
#endif
/* if not in NAV_MODE set commands from the rc */
//如果不是导航模式，设置来自RC的命令
#ifdef SetCommandsFromRC
if (autopilot_mode != AP_MODE_NAV) {
SetCommandsFromRC(commands, radio_control.values);
}
#endif
/* an arming sequence is used to start/stop motors */
// 一个。。。序列用来启动和关闭电机
autopilot_arming_check_motors_on();
kill_throttle = ! autopilot_motors_on;//关闭电机
autopilot_check_in_flight(autopilot_motors_on);
guidance_v_read_rc();//的垂直方向的信息
guidance_h_read_rc(autopilot_in_flight);//读EC
}
}
void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) {
sp->phi = (radio_control.values[RADIO_ROLL] * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ);
sp->theta = (radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ);
if (in_flight) {
/* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
if (YAW_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
sp->psi += (radio_control.values[RADIO_YAW] * STABILIZATION_ATTITUDE_SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ);
FLOAT_ANGLE_NORMALIZE(sp->psi);
}
if (coordinated_turn) {
//Coordinated turn
//feedforward estimate angular rotation omega = g*tan(phi)/v
//Take v = 9.81/1.3 m/s
float omega;
const float max_phi = RadOfDeg(85.0);
if(abs(sp->phi) < max_phi)
omega = 1.3*tanf(sp->phi);
else //max 60 degrees roll, then take constant omega
omega = 1.3*1.72305* ((sp->phi > 0) - (sp->phi < 0));
sp->psi += omega/RC_UPDATE_FREQ;
}
#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
// Make sure the yaw setpoint does not differ too much from the real yaw
// to prevent a sudden switch at 180 deg
float heading = stabilization_attitude_get_heading_f();
float delta_psi = sp->psi - heading;
FLOAT_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){
sp->psi = heading + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
}
else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){
sp->psi = heading - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
}
FLOAT_ANGLE_NORMALIZE(sp->psi);
#endif
//Care Free mode
if (in_carefree) {
//care_free_heading has been set to current psi when entering care free mode.
float cos_psi;
float sin_psi;
float temp_theta;
float care_free_delta_psi_f = sp->psi - care_free_heading;
FLOAT_ANGLE_NORMALIZE(care_free_delta_psi_f);
sin_psi = sinf(care_free_delta_psi_f);
cos_psi = cosf(care_free_delta_psi_f);
temp_theta = cos_psi*sp->theta - sin_psi*sp->phi;
sp->phi = cos_psi*sp->phi - sin_psi*sp->theta;
sp->theta = temp_theta;
}
}
else { /* if not flying, use current yaw as setpoint */
sp->psi = stateGetNedToBodyEulers_f()->psi;
}
}
/** Read attitude setpoint from RC as euler angles
* @param[in] coordinated_turn true if in horizontal mode forward
* @param[in] in_carefree true if in carefree mode
* @param[in] in_flight true if in flight
* @param[out] sp attitude setpoint as euler angles
*/
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) {
const int32_t max_rc_phi = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI);
const int32_t max_rc_theta = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA);
const int32_t max_rc_r = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R);
sp->phi = (int32_t) ((radio_control.values[RADIO_ROLL] * max_rc_phi) / MAX_PPRZ);
sp->theta = (int32_t) ((radio_control.values[RADIO_PITCH] * max_rc_theta) / MAX_PPRZ);
if (in_flight) {
/* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
if (YAW_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
sp->psi += (int32_t) ((radio_control.values[RADIO_YAW] * max_rc_r) / MAX_PPRZ / RC_UPDATE_FREQ);
INT32_ANGLE_NORMALIZE(sp->psi);
}
if (coordinated_turn) {
//Coordinated turn
//feedforward estimate angular rotation omega = g*tan(phi)/v
//Take v = 9.81/1.3 m/s
int32_t omega;
const int32_t max_phi = ANGLE_BFP_OF_REAL(RadOfDeg(85.0));
if(abs(sp->phi) < max_phi)
omega = ANGLE_BFP_OF_REAL(1.3*tanf(ANGLE_FLOAT_OF_BFP(sp->phi)));
else //max 60 degrees roll, then take constant omega
omega = ANGLE_BFP_OF_REAL(1.3*1.72305* ((sp->phi > 0) - (sp->phi < 0)));
sp->psi += omega/RC_UPDATE_FREQ;
}
#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
// Make sure the yaw setpoint does not differ too much from the real yaw
// to prevent a sudden switch at 180 deg
const int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);
int32_t heading = stabilization_attitude_get_heading_i();
int32_t delta_psi = sp->psi - heading;
INT32_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > delta_limit){
sp->psi = heading + delta_limit;
}
else if (delta_psi < -delta_limit){
sp->psi = heading - delta_limit;
}
INT32_ANGLE_NORMALIZE(sp->psi);
#endif
//Care Free mode
if (in_carefree) {
//care_free_heading has been set to current psi when entering care free mode.
int32_t cos_psi;
int32_t sin_psi;
int32_t temp_theta;
int32_t care_free_delta_psi_i;
care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading);
INT32_ANGLE_NORMALIZE(care_free_delta_psi_i);
PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i);
PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i);
temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi, INT32_ANGLE_FRAC);
sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC);
sp->theta = temp_theta;
}
}
else { /* if not flying, use current yaw as setpoint */
sp->psi = stateGetNedToBodyEulers_i()->psi;
}
}
void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool in_flight, bool in_carefree,
bool coordinated_turn)
{
/* last time this function was called, used to calculate yaw setpoint update */
static float last_ts = 0.f;
sp->phi = get_rc_roll_f();
sp->theta = get_rc_pitch_f();
if (in_flight) {
/* calculate dt for yaw integration */
float dt = get_sys_time_float() - last_ts;
/* make sure nothing drastically weird happens, bound dt to 0.5sec */
Bound(dt, 0, 0.5);
/* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
if (YAW_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
sp->psi += get_rc_yaw_f() * dt;
FLOAT_ANGLE_NORMALIZE(sp->psi);
}
if (coordinated_turn) {
//Coordinated turn
//feedforward estimate angular rotation omega = g*tan(phi)/v
float omega;
const float max_phi = RadOfDeg(60.0);
if (fabsf(sp->phi) < max_phi) {
omega = 9.81 / COORDINATED_TURN_AIRSPEED * tanf(sp->phi);
} else { //max 60 degrees roll
omega = 9.81 / COORDINATED_TURN_AIRSPEED * 1.72305 * ((sp->phi > 0) - (sp->phi < 0));
}
sp->psi += omega * dt;
}
#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
// Make sure the yaw setpoint does not differ too much from the real yaw
// to prevent a sudden switch at 180 deg
float heading = stabilization_attitude_get_heading_f();
float delta_psi = sp->psi - heading;
FLOAT_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) {
sp->psi = heading + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
} else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) {
sp->psi = heading - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
}
FLOAT_ANGLE_NORMALIZE(sp->psi);
#endif
//Care Free mode
if (in_carefree) {
//care_free_heading has been set to current psi when entering care free mode.
float cos_psi;
float sin_psi;
float temp_theta;
float care_free_delta_psi_f = sp->psi - care_free_heading;
FLOAT_ANGLE_NORMALIZE(care_free_delta_psi_f);
sin_psi = sinf(care_free_delta_psi_f);
cos_psi = cosf(care_free_delta_psi_f);
temp_theta = cos_psi * sp->theta - sin_psi * sp->phi;
sp->phi = cos_psi * sp->phi - sin_psi * sp->theta;
sp->theta = temp_theta;
}
} else { /* if not flying, use current yaw as setpoint */
sp->psi = stateGetNedToBodyEulers_f()->psi;
}
/* update timestamp for dt calculation */
last_ts = get_sys_time_float();
}
/** Read attitude setpoint from RC as euler angles
* @param[in] coordinated_turn true if in horizontal mode forward
* @param[in] in_carefree true if in carefree mode
* @param[in] in_flight true if in flight
* @param[out] sp attitude setpoint as euler angles
*/
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool in_flight, bool in_carefree,
bool coordinated_turn)
{
/* last time this function was called, used to calculate yaw setpoint update */
static float last_ts = 0.f;
sp->phi = get_rc_roll();
sp->theta = get_rc_pitch();
if (in_flight) {
/* calculate dt for yaw integration */
float dt = get_sys_time_float() - last_ts;
/* make sure nothing drastically weird happens, bound dt to 0.5sec */
Bound(dt, 0, 0.5);
/* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
if (YAW_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
sp->psi += get_rc_yaw() * dt;
INT32_ANGLE_NORMALIZE(sp->psi);
}
if (coordinated_turn) {
//Coordinated turn
//feedforward estimate angular rotation omega = g*tan(phi)/v
int32_t omega;
const int32_t max_phi = ANGLE_BFP_OF_REAL(RadOfDeg(60.0));
if (abs(sp->phi) < max_phi) {
omega = ANGLE_BFP_OF_REAL(9.81 / COORDINATED_TURN_AIRSPEED * tanf(ANGLE_FLOAT_OF_BFP(sp->phi)));
} else { //max 60 degrees roll
omega = ANGLE_BFP_OF_REAL(9.81 / COORDINATED_TURN_AIRSPEED * 1.72305 * ((sp->phi > 0) - (sp->phi < 0)));
}
sp->psi += omega * dt;
}
#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
// Make sure the yaw setpoint does not differ too much from the real yaw
// to prevent a sudden switch at 180 deg
const int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);
int32_t heading = stabilization_attitude_get_heading_i();
int32_t delta_psi = sp->psi - heading;
INT32_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > delta_limit) {
sp->psi = heading + delta_limit;
} else if (delta_psi < -delta_limit) {
sp->psi = heading - delta_limit;
}
INT32_ANGLE_NORMALIZE(sp->psi);
#endif
//Care Free mode
if (in_carefree) {
//care_free_heading has been set to current psi when entering care free mode.
int32_t cos_psi;
int32_t sin_psi;
int32_t temp_theta;
int32_t care_free_delta_psi_i;
care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading);
INT32_ANGLE_NORMALIZE(care_free_delta_psi_i);
PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i);
PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i);
temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi,
INT32_ANGLE_FRAC);
sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC);
sp->theta = temp_theta;
}
} else { /* if not flying, use current yaw as setpoint */
sp->psi = stateGetNedToBodyEulers_i()->psi;
}
/* update timestamp for dt calculation */
last_ts = get_sys_time_float();
}
