diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index dfdc0a61e4..4961aa05af 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -215,7 +215,7 @@ void Copter::send_pid_tuning(mavlink_channel_t chan) const Vector3f &gyro = ahrs.get_gyro(); if (g.gcs_pid_mask & 1) { const DataFlash_Class::PID_Info &pid_info = attitude_control->get_rate_roll_pid().get_pid_info(); - mavlink_msg_pid_tuning_send(chan, PID_TUNING_ROLL, + mavlink_msg_pid_tuning_send(chan, PID_TUNING_ROLL, pid_info.desired*0.01f, degrees(gyro.x), pid_info.FF*0.01f, @@ -228,7 +228,7 @@ void Copter::send_pid_tuning(mavlink_channel_t chan) } if (g.gcs_pid_mask & 2) { const DataFlash_Class::PID_Info &pid_info = attitude_control->get_rate_pitch_pid().get_pid_info(); - mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH, + mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH, pid_info.desired*0.01f, degrees(gyro.y), pid_info.FF*0.01f, @@ -241,7 +241,7 @@ void Copter::send_pid_tuning(mavlink_channel_t chan) } if (g.gcs_pid_mask & 4) { const DataFlash_Class::PID_Info &pid_info = attitude_control->get_rate_yaw_pid().get_pid_info(); - mavlink_msg_pid_tuning_send(chan, PID_TUNING_YAW, + mavlink_msg_pid_tuning_send(chan, PID_TUNING_YAW, pid_info.desired*0.01f, degrees(gyro.z), pid_info.FF*0.01f, @@ -254,7 +254,7 @@ void Copter::send_pid_tuning(mavlink_channel_t chan) } if (g.gcs_pid_mask & 8) { const DataFlash_Class::PID_Info &pid_info = copter.pos_control->get_accel_z_pid().get_pid_info(); - mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ, + mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ, pid_info.desired*0.01f, -(ahrs.get_accel_ef_blended().z + GRAVITY_MSS), pid_info.FF*0.01f, @@ -413,7 +413,7 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id) case MSG_MOUNT_STATUS: #if MOUNT == ENABLED - CHECK_PAYLOAD_SIZE(MOUNT_STATUS); + CHECK_PAYLOAD_SIZE(MOUNT_STATUS); copter.camera_mount.status_msg(chan); #endif // MOUNT == ENABLED break; diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index 5c38e7808c..dae91eb709 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -78,7 +78,7 @@ void Copter::read_control_switch() // check_if_auxsw_mode_used - Check to see if any of the Aux Switches are set to a given mode. bool Copter::check_if_auxsw_mode_used(uint8_t auxsw_mode_check) { - bool ret = g.ch7_option == auxsw_mode_check || g.ch8_option == auxsw_mode_check || g.ch9_option == auxsw_mode_check + bool ret = g.ch7_option == auxsw_mode_check || g.ch8_option == auxsw_mode_check || g.ch9_option == auxsw_mode_check || g.ch10_option == auxsw_mode_check || g.ch11_option == auxsw_mode_check || g.ch12_option == auxsw_mode_check; return ret; @@ -180,7 +180,7 @@ void Copter::init_aux_switches() // init_aux_switch_function - initialize aux functions void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag) -{ +{ // init channel options switch(ch_option) { case AUXSW_SIMPLE_MODE: diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp index 41f14375a1..6d8c3c274f 100644 --- a/ArduCopter/tuning.cpp +++ b/ArduCopter/tuning.cpp @@ -10,7 +10,7 @@ void Copter::tuning() { RC_Channel *rc6 = RC_Channels::rc_channel(CH_6); - // exit immediately if not using tuning function, or when radio failsafe is invoked, so tuning values are not set to zero + // exit immediately if not using tuning function, or when radio failsafe is invoked, so tuning values are not set to zero if ((g.radio_tuning <= 0) || failsafe.radio || failsafe.radio_counter != 0 || rc6->get_radio_in() == 0) { return; }