Skip to content

Commit 4586000

Browse files
author
chenzhjie
committed
style: code format
1 parent b3d85b9 commit 4586000

File tree

3 files changed

+100
-115
lines changed

3 files changed

+100
-115
lines changed

‎src/flight_control.cpp‎

Lines changed: 96 additions & 111 deletions
Original file line numberDiff line numberDiff line change
@@ -340,7 +340,7 @@ void loop_400Hz(void) {
340340
if (rc_isconnected() == 0) Mode = AUTO_LANDING_MODE;
341341
// if (Range0flag == 20) Mode = AUTO_LANDING_MODE;
342342
if (OverG_flag == 1) Mode = PARKING_MODE;
343-
if(Mode != OldMode)ahrs_reset();
343+
if (Mode != OldMode) ahrs_reset();
344344

345345
// Get command
346346
get_command();
@@ -594,119 +594,104 @@ void control_init(void) {
594594
}
595595
///////////////////////////////////////////////////////////////////
596596

597-
float get_trim_duty(float voltage)
598-
{
599-
return -0.2448f * voltage + 1.5892f;
597+
float get_trim_duty(float voltage) {
598+
return -0.2448f * voltage + 1.5892f;
600599
}
601600

602-
float get_rate_ref(float x)
603-
{
604-
float ref;
605-
float h;
606-
if (x < -1.0f) x = -1.0f;
607-
if (x > 1.0f) x = 1.0f;
608-
if (x >= 0.0f)
609-
{
610-
h = x * (x * x * x * x * x * RATE_EXPO + x * (1 - RATE_EXPO));
611-
ref = PI/180.0f * ((RATE_MAX - RATE_RATE)* h + RATE_RATE * x);
612-
}
613-
else
614-
{
615-
x = -x;
616-
h = x * (x * x * x * x * x * RATE_EXPO + x * (1 - RATE_EXPO));
617-
ref = - PI/180.0f * ((RATE_MAX - RATE_RATE)* h + RATE_RATE * x);
618-
}
619-
return ref;
601+
float get_rate_ref(float x) {
602+
float ref;
603+
float h;
604+
if (x < -1.0f) x = -1.0f;
605+
if (x > 1.0f) x = 1.0f;
606+
if (x >= 0.0f) {
607+
h = x * (x * x * x * x * x * RATE_EXPO + x * (1 - RATE_EXPO));
608+
ref = PI / 180.0f * ((RATE_MAX - RATE_RATE) * h + RATE_RATE * x);
609+
} else {
610+
x = -x;
611+
h = x * (x * x * x * x * x * RATE_EXPO + x * (1 - RATE_EXPO));
612+
ref = -PI / 180.0f * ((RATE_MAX - RATE_RATE) * h + RATE_RATE * x);
613+
}
614+
return ref;
620615
}
621616

622-
void get_command(void)
623-
{
624-
static uint16_t stick_count=0;
625-
static float auto_throttle = 0.0f;
626-
static float old_alt = 0.0;
627-
float th,thlo;
628-
float throttle_limit = 0.7;
629-
float thrust_max;
630-
631-
Control_mode = Stick[CONTROLMODE];
632-
if ( (uint8_t)Stick[ALTCONTROLMODE] == AUTO_ALT ) Throttle_control_mode = 1;
633-
else if( (uint8_t)Stick[ALTCONTROLMODE] == MANUAL_ALT) Throttle_control_mode = 0;
634-
else Throttle_control_mode = 0;
635-
636-
//Thrust control
637-
thlo = Stick[THROTTLE];
638-
//thlo = thlo/throttle_limit;
639-
640-
if (Throttle_control_mode == 0)
641-
{
642-
//Manual Throttle
643-
if(thlo<0.0)thlo = 0.0;
644-
if (thlo>1.0f) thlo = 1.0f;
645-
if ( (-0.2 < thlo) && (thlo < 0.2) )thlo = 0.0f ;//不感帯
646-
th = (get_trim_duty(Voltage)+(thlo-0.4))*BATTERY_VOLTAGE;
647-
if(th<0)th=0.0f;
648-
Thrust_command = Thrust_filtered.update(th, Interval_time);
649-
}
650-
else if (Throttle_control_mode == 1)
651-
{
652-
//Auto Throttle Altitude Control
653-
Alt_flag = 1;
654-
if(Auto_takeoff_counter < 500)
655-
{
656-
Thrust0 = (float)Auto_takeoff_counter/1000.0;
657-
if (Thrust0 > get_trim_duty(3.8)) Thrust0 = get_trim_duty(3.8);
658-
Auto_takeoff_counter ++;
617+
void get_command(void) {
618+
static uint16_t stick_count = 0;
619+
static float auto_throttle = 0.0f;
620+
static float old_alt = 0.0;
621+
float th, thlo;
622+
float throttle_limit = 0.7;
623+
float thrust_max;
624+
625+
Control_mode = Stick[CONTROLMODE];
626+
if ((uint8_t)Stick[ALTCONTROLMODE] == AUTO_ALT)
627+
Throttle_control_mode = 1;
628+
else if ((uint8_t)Stick[ALTCONTROLMODE] == MANUAL_ALT)
629+
Throttle_control_mode = 0;
630+
else
631+
Throttle_control_mode = 0;
632+
633+
// Thrust control
634+
thlo = Stick[THROTTLE];
635+
// thlo = thlo/throttle_limit;
636+
637+
if (Throttle_control_mode == 0) {
638+
// Manual Throttle
639+
if (thlo < 0.0) thlo = 0.0;
640+
if (thlo > 1.0f) thlo = 1.0f;
641+
if ((-0.2 < thlo) && (thlo < 0.2)) thlo = 0.0f; // 不感帯
642+
th = (get_trim_duty(Voltage) + (thlo - 0.4)) * BATTERY_VOLTAGE;
643+
if (th < 0) th = 0.0f;
644+
Thrust_command = Thrust_filtered.update(th, Interval_time);
645+
} else if (Throttle_control_mode == 1) {
646+
// Auto Throttle Altitude Control
647+
Alt_flag = 1;
648+
if (Auto_takeoff_counter < 500) {
649+
Thrust0 = (float)Auto_takeoff_counter / 1000.0;
650+
if (Thrust0 > get_trim_duty(3.8)) Thrust0 = get_trim_duty(3.8);
651+
Auto_takeoff_counter++;
652+
} else if (Auto_takeoff_counter < 1000) {
653+
Thrust0 = (float)Auto_takeoff_counter / 1000.0;
654+
if (Thrust0 > get_trim_duty(Voltage)) Thrust0 = get_trim_duty(Voltage);
655+
Auto_takeoff_counter++;
656+
} else
657+
Thrust0 = get_trim_duty(Voltage);
658+
659+
// Get Altitude ref
660+
if ((-0.2 < thlo) && (thlo < 0.2)) thlo = 0.0f; // 不感帯
661+
Alt_ref = Alt_ref + thlo * 0.001;
662+
if (Alt_ref > ALT_REF_MAX) Alt_ref = ALT_REF_MAX;
663+
if (Alt_ref < ALT_REF_MIN) Alt_ref = ALT_REF_MIN;
664+
if ((Range0flag > OladRange0flag) || (Range0flag == RNAGE0FLAG_MAX)) {
665+
Thrust0 = Thrust0 - 0.02;
666+
OladRange0flag = Range0flag;
667+
}
668+
Thrust_command = Thrust0 * BATTERY_VOLTAGE;
659669
}
660-
else if(Auto_takeoff_counter < 1000)
661-
{
662-
Thrust0 = (float)Auto_takeoff_counter/1000.0;
663-
if (Thrust0 > get_trim_duty(Voltage)) Thrust0 = get_trim_duty(Voltage);
664-
Auto_takeoff_counter ++;
670+
671+
if (Control_mode == ANGLECONTROL) {
672+
Roll_angle_command = 0.4 * Stick[AILERON];
673+
if (Roll_angle_command < -1.0f) Roll_angle_command = -1.0f;
674+
if (Roll_angle_command > 1.0f) Roll_angle_command = 1.0f;
675+
Pitch_angle_command = 0.4 * Stick[ELEVATOR];
676+
if (Pitch_angle_command < -1.0f) Pitch_angle_command = -1.0f;
677+
if (Pitch_angle_command > 1.0f) Pitch_angle_command = 1.0f;
678+
} else if (Control_mode == RATECONTROL) {
679+
Roll_rate_reference = get_rate_ref(Stick[AILERON]);
680+
Pitch_rate_reference = get_rate_ref(Stick[ELEVATOR]);
681+
// USBSerial.printf("%9.6f\n\r", Pitch_rate_reference*180.0f/PI);
665682
}
666-
else Thrust0 = get_trim_duty(Voltage);
667-
668-
//Get Altitude ref
669-
if ( (-0.2 < thlo) && (thlo < 0.2) )thlo = 0.0f ;//不感帯
670-
Alt_ref = Alt_ref + thlo*0.001;
671-
if(Alt_ref > ALT_REF_MAX ) Alt_ref = ALT_REF_MAX;
672-
if(Alt_ref < ALT_REF_MIN ) Alt_ref = ALT_REF_MIN;
673-
if( (Range0flag > OladRange0flag) || (Range0flag == RNAGE0FLAG_MAX))
674-
{
675-
Thrust0 = Thrust0 - 0.02;
676-
OladRange0flag =Range0flag;
683+
684+
Yaw_angle_command = Stick[RUDDER];
685+
if (Yaw_angle_command < -1.0f) Yaw_angle_command = -1.0f;
686+
if (Yaw_angle_command > 1.0f) Yaw_angle_command = 1.0f;
687+
// Yaw control
688+
Yaw_rate_reference = 2.0f * PI * (Yaw_angle_command - Rudder_center);
689+
690+
// flip button check
691+
if (Flip_flag == 0 /*&& Throttle_control_mode == 0*/) {
692+
Flip_flag = get_flip_button();
693+
if (Flip_flag == 1) Mode = FLIP_MODE;
677694
}
678-
Thrust_command = Thrust0 * BATTERY_VOLTAGE;
679-
}
680-
681-
if (Control_mode == ANGLECONTROL)
682-
{
683-
Roll_angle_command = 0.4*Stick[AILERON];
684-
if (Roll_angle_command<-1.0f)Roll_angle_command = -1.0f;
685-
if (Roll_angle_command> 1.0f)Roll_angle_command = 1.0f;
686-
Pitch_angle_command = 0.4*Stick[ELEVATOR];
687-
if (Pitch_angle_command<-1.0f)Pitch_angle_command = -1.0f;
688-
if (Pitch_angle_command> 1.0f)Pitch_angle_command = 1.0f;
689-
}
690-
else if (Control_mode == RATECONTROL)
691-
{
692-
Roll_rate_reference = get_rate_ref(Stick[AILERON]);
693-
Pitch_rate_reference = get_rate_ref(Stick[ELEVATOR]);
694-
//USBSerial.printf("%9.6f\n\r", Pitch_rate_reference*180.0f/PI);
695-
}
696-
697-
Yaw_angle_command = Stick[RUDDER];
698-
if (Yaw_angle_command<-1.0f)Yaw_angle_command = -1.0f;
699-
if (Yaw_angle_command> 1.0f)Yaw_angle_command = 1.0f;
700-
//Yaw control
701-
Yaw_rate_reference = 2.0f * PI * (Yaw_angle_command - Rudder_center);
702-
703-
704-
// flip button check
705-
if (Flip_flag == 0 /*&& Throttle_control_mode == 0*/)
706-
{
707-
Flip_flag = get_flip_button();
708-
if(Flip_flag == 1)Mode = FLIP_MODE;
709-
}
710695
}
711696

712697
#if 0
@@ -843,7 +828,7 @@ uint8_t auto_landing(void) {
843828
Yaw_rate_reference = 2.0f * PI * (Yaw_angle_command - Rudder_center);
844829

845830
if (Control_mode == RATECONTROL) {
846-
Roll_rate_reference = get_rate_ref(Stick[AILERON]);
831+
Roll_rate_reference = get_rate_ref(Stick[AILERON]);
847832
Pitch_rate_reference = get_rate_ref(Stick[ELEVATOR]);
848833
}
849834

@@ -1010,9 +995,9 @@ void angle_control(void) {
1010995
reset_angle_control();
1011996
} else {
1012997
// Altitude Control PID
1013-
alt_err = Alt_ref - Altitude2;
998+
alt_err = Alt_ref - Altitude2;
1014999
if (Alt_flag >= 1) Z_dot_ref = alt_pid.update(alt_err, Interval_time);
1015-
1000+
10161001
if (Control_mode == ANGLECONTROL) {
10171002
// Angle Control
10181003
// Led_color = RED;
@@ -1028,7 +1013,7 @@ void angle_control(void) {
10281013
phi_err = Roll_angle_reference - (Roll_angle - Roll_angle_offset);
10291014
theta_err = Pitch_angle_reference - (Pitch_angle - Pitch_angle_offset);
10301015

1031-
//Angle Control PID
1016+
// Angle Control PID
10321017
Roll_rate_reference = phi_pid.update(phi_err, Interval_time);
10331018
Pitch_rate_reference = theta_pid.update(theta_err, Interval_time);
10341019
}

‎src/flight_control.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,7 @@
6060
#define RNAGE0FLAG_MAX (20)
6161

6262
#define RATE_RATE (70.0f)
63-
#define RATE_MAX (1600.0f)
63+
#define RATE_MAX (1600.0f)
6464
#define RATE_EXPO (0.5f)
6565

6666
// グローバル関数の宣言

‎src/rc.cpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -118,10 +118,10 @@ void OnDataRecv(const uint8_t *mac_addr, const uint8_t *recv_data, int data_len)
118118
d_int[3] = recv_data[18];
119119
Stick[ELEVATOR] = d_float;
120120

121-
Stick[BUTTON_ARM] = recv_data[19];//auto_up_down_status
121+
Stick[BUTTON_ARM] = recv_data[19]; // auto_up_down_status
122122
Stick[BUTTON_FLIP] = recv_data[20];
123-
Stick[CONTROLMODE] = recv_data[21];//Mode:rate or angle control
124-
Stick[ALTCONTROLMODE] = recv_data[22];//高度制御
123+
Stick[CONTROLMODE] = recv_data[21]; // Mode:rate or angle control
124+
Stick[ALTCONTROLMODE] = recv_data[22]; // 高度制御
125125

126126
ahrs_reset_flag = recv_data[23];
127127

0 commit comments

Comments
 (0)