mirror of https://github.com/ArduPilot/ardupilot
parent
37a165f5bf
commit
4ae9c0cf49
|
@ -107,7 +107,7 @@ void Tracker::one_second_loop()
|
||||||
one_second_counter++;
|
one_second_counter++;
|
||||||
|
|
||||||
if (one_second_counter >= 60) {
|
if (one_second_counter >= 60) {
|
||||||
if(g.compass_enabled) {
|
if (g.compass_enabled) {
|
||||||
compass.save_offsets();
|
compass.save_offsets();
|
||||||
}
|
}
|
||||||
one_second_counter = 0;
|
one_second_counter = 0;
|
||||||
|
|
|
@ -82,7 +82,7 @@ void Tracker::accel_cal_update() {
|
||||||
}
|
}
|
||||||
ins.acal_update();
|
ins.acal_update();
|
||||||
float trim_roll, trim_pitch;
|
float trim_roll, trim_pitch;
|
||||||
if(ins.get_new_trim(trim_roll, trim_pitch)) {
|
if (ins.get_new_trim(trim_roll, trim_pitch)) {
|
||||||
ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
|
ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -100,7 +100,7 @@ void Tracker::update_GPS(void)
|
||||||
gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
|
gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
|
||||||
last_gps_msg_ms = gps.last_message_time_ms();
|
last_gps_msg_ms = gps.last_message_time_ms();
|
||||||
|
|
||||||
if(ground_start_count > 1) {
|
if (ground_start_count > 1) {
|
||||||
ground_start_count--;
|
ground_start_count--;
|
||||||
} else if (ground_start_count == 1) {
|
} else if (ground_start_count == 1) {
|
||||||
// We countdown N number of good GPS fixes
|
// We countdown N number of good GPS fixes
|
||||||
|
|
|
@ -214,7 +214,7 @@ void Tracker::update_yaw_position_servo()
|
||||||
|
|
||||||
channel_yaw.set_servo_out(new_servo_out);
|
channel_yaw.set_servo_out(new_servo_out);
|
||||||
|
|
||||||
if(yaw_servo_out_filt_init){
|
if (yaw_servo_out_filt_init) {
|
||||||
yaw_servo_out_filt.apply(new_servo_out, G_Dt);
|
yaw_servo_out_filt.apply(new_servo_out, G_Dt);
|
||||||
} else {
|
} else {
|
||||||
yaw_servo_out_filt.reset(new_servo_out);
|
yaw_servo_out_filt.reset(new_servo_out);
|
||||||
|
|
|
@ -187,7 +187,7 @@ void Tracker::prepare_servos()
|
||||||
|
|
||||||
void Tracker::set_mode(enum ControlMode mode)
|
void Tracker::set_mode(enum ControlMode mode)
|
||||||
{
|
{
|
||||||
if(control_mode == mode) {
|
if (control_mode == mode) {
|
||||||
// don't switch modes if we are already in the correct mode.
|
// don't switch modes if we are already in the correct mode.
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue