Copter: revised and uniformization of severity messages

This commit is contained in:
lvale 2015-11-04 17:32:01 +00:00 committed by Randy Mackay
parent 420b1ca715
commit 549695181c
13 changed files with 52 additions and 52 deletions

View File

@ -206,7 +206,7 @@ void Copter::perf_update(void)
if (should_log(MASK_LOG_PM)) if (should_log(MASK_LOG_PM))
Log_Write_Performance(); Log_Write_Performance();
if (scheduler.debug()) { if (scheduler.debug()) {
gcs_send_text_fmt("PERF: %u/%u %lu %lu\n", gcs_send_text_fmt(MAV_SEVERITY_WARNING, "PERF: %u/%u %lu %lu\n",
(unsigned)perf_info_get_num_long_running(), (unsigned)perf_info_get_num_long_running(),
(unsigned)perf_info_get_num_loops(), (unsigned)perf_info_get_num_loops(),
(unsigned long)perf_info_get_max_time(), (unsigned long)perf_info_get_max_time(),

View File

@ -933,7 +933,7 @@ private:
void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate); void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate);
void print_hit_enter(); void print_hit_enter();
void tuning(); void tuning();
void gcs_send_text_fmt(const char *fmt, ...); void gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...);
bool start_command(const AP_Mission::Mission_Command& cmd); bool start_command(const AP_Mission::Mission_Command& cmd);
bool verify_command(const AP_Mission::Mission_Command& cmd); bool verify_command(const AP_Mission::Mission_Command& cmd);
bool verify_command_callback(const AP_Mission::Mission_Command& cmd); bool verify_command_callback(const AP_Mission::Mission_Command& cmd);

View File

@ -1022,12 +1022,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: // MAV ID: 21 case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: // MAV ID: 21
{ {
// mark the firmware version in the tlog // mark the firmware version in the tlog
send_text(MAV_SEVERITY_WARNING, FIRMWARE_STRING); send_text(MAV_SEVERITY_INFO, FIRMWARE_STRING);
#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION) #if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
send_text(MAV_SEVERITY_WARNING, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION); send_text(MAV_SEVERITY_INFO, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION);
#endif #endif
send_text(MAV_SEVERITY_WARNING, "Frame: " FRAME_CONFIG_STRING); send_text(MAV_SEVERITY_INFO, "Frame: " FRAME_CONFIG_STRING);
handle_param_request_list(msg); handle_param_request_list(msg);
break; break;
} }
@ -1518,18 +1518,18 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_CMD_DO_SEND_BANNER: { case MAV_CMD_DO_SEND_BANNER: {
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
send_text(MAV_SEVERITY_WARNING, FIRMWARE_STRING); send_text(MAV_SEVERITY_INFO, FIRMWARE_STRING);
#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION) #if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
send_text(MAV_SEVERITY_WARNING, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION); send_text(MAV_SEVERITY_INFO, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION);
#endif #endif
send_text(MAV_SEVERITY_WARNING, "Frame: " FRAME_CONFIG_STRING); send_text(MAV_SEVERITY_INFO, "Frame: " FRAME_CONFIG_STRING);
// send system ID if we can // send system ID if we can
char sysid[40]; char sysid[40];
if (hal.util->get_system_id(sysid)) { if (hal.util->get_system_id(sysid)) {
send_text(MAV_SEVERITY_WARNING, sysid); send_text(MAV_SEVERITY_INFO, sysid);
} }
break; break;
@ -1841,12 +1841,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
if (packet.idx >= copter.rally.get_rally_total() || if (packet.idx >= copter.rally.get_rally_total() ||
packet.idx >= copter.rally.get_rally_max()) { packet.idx >= copter.rally.get_rally_max()) {
send_text(MAV_SEVERITY_WARNING,"bad rally point message ID"); send_text(MAV_SEVERITY_NOTICE,"bad rally point message ID");
break; break;
} }
if (packet.count != copter.rally.get_rally_total()) { if (packet.count != copter.rally.get_rally_total()) {
send_text(MAV_SEVERITY_WARNING,"bad rally point message count"); send_text(MAV_SEVERITY_NOTICE,"bad rally point message count");
break; break;
} }
@ -1867,27 +1867,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
//send a rally point to the GCS //send a rally point to the GCS
case MAVLINK_MSG_ID_RALLY_FETCH_POINT: { case MAVLINK_MSG_ID_RALLY_FETCH_POINT: {
//send_text(MAV_SEVERITY_CRITICAL, "## getting rally point in GCS_Mavlink.cpp 1"); // #### TEMP //send_text(MAV_SEVERITY_INFO, "## getting rally point in GCS_Mavlink.cpp 1"); // #### TEMP
mavlink_rally_fetch_point_t packet; mavlink_rally_fetch_point_t packet;
mavlink_msg_rally_fetch_point_decode(msg, &packet); mavlink_msg_rally_fetch_point_decode(msg, &packet);
//send_text(MAV_SEVERITY_CRITICAL, "## getting rally point in GCS_Mavlink.cpp 2"); // #### TEMP //send_text(MAV_SEVERITY_INFO, "## getting rally point in GCS_Mavlink.cpp 2"); // #### TEMP
if (packet.idx > copter.rally.get_rally_total()) { if (packet.idx > copter.rally.get_rally_total()) {
send_text(MAV_SEVERITY_WARNING, "bad rally point index"); send_text(MAV_SEVERITY_NOTICE, "bad rally point index");
break; break;
} }
//send_text(MAV_SEVERITY_CRITICAL, "## getting rally point in GCS_Mavlink.cpp 3"); // #### TEMP //send_text(MAV_SEVERITY_INFO, "## getting rally point in GCS_Mavlink.cpp 3"); // #### TEMP
RallyLocation rally_point; RallyLocation rally_point;
if (!copter.rally.get_rally_point_with_index(packet.idx, rally_point)) { if (!copter.rally.get_rally_point_with_index(packet.idx, rally_point)) {
send_text(MAV_SEVERITY_WARNING, "failed to set rally point"); send_text(MAV_SEVERITY_NOTICE, "failed to set rally point");
break; break;
} }
//send_text(MAV_SEVERITY_CRITICAL, "## getting rally point in GCS_Mavlink.cpp 4"); // #### TEMP //send_text(MAV_SEVERITY_INFO, "## getting rally point in GCS_Mavlink.cpp 4"); // #### TEMP
mavlink_msg_rally_point_send_buf(msg, mavlink_msg_rally_point_send_buf(msg,
chan, msg->sysid, msg->compid, packet.idx, chan, msg->sysid, msg->compid, packet.idx,
@ -1895,7 +1895,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
rally_point.alt, rally_point.break_alt, rally_point.land_dir, rally_point.alt, rally_point.break_alt, rally_point.land_dir,
rally_point.flags); rally_point.flags);
//send_text(MAV_SEVERITY_CRITICAL, "## getting rally point in GCS_Mavlink.cpp 5"); // #### TEMP //send_text(MAV_SEVERITY_INFO, "## getting rally point in GCS_Mavlink.cpp 5"); // #### TEMP
break; break;
} }
@ -1965,7 +1965,7 @@ void Copter::mavlink_delay_cb()
} }
if (tnow - last_5s > 5000) { if (tnow - last_5s > 5000) {
last_5s = tnow; last_5s = tnow;
gcs_send_text(MAV_SEVERITY_WARNING, "Initialising APM..."); gcs_send_text(MAV_SEVERITY_INFO, "Initialising APM...");
} }
check_usb_mux(); check_usb_mux();
@ -2039,10 +2039,10 @@ void Copter::gcs_send_text(MAV_SEVERITY severity, const char *str)
* only one fits in the queue, so if you send more than one before the * only one fits in the queue, so if you send more than one before the
* last one gets into the serial buffer then the old one will be lost * last one gets into the serial buffer then the old one will be lost
*/ */
void Copter::gcs_send_text_fmt(const char *fmt, ...) void Copter::gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...)
{ {
va_list arg_list; va_list arg_list;
gcs[0].pending_status.severity = (uint8_t)MAV_SEVERITY_WARNING; gcs[0].pending_status.severity = (uint8_t)severity;
va_start(arg_list, fmt); va_start(arg_list, fmt);
hal.util->vsnprintf((char *)gcs[0].pending_status.text, hal.util->vsnprintf((char *)gcs[0].pending_status.text,
sizeof(gcs[0].pending_status.text), fmt, arg_list); sizeof(gcs[0].pending_status.text), fmt, arg_list);

View File

@ -147,9 +147,9 @@ int8_t Copter::process_logs(uint8_t argc, const Menu::arg *argv)
void Copter::do_erase_logs(void) void Copter::do_erase_logs(void)
{ {
gcs_send_text(MAV_SEVERITY_CRITICAL, "Erasing logs\n"); gcs_send_text(MAV_SEVERITY_INFO, "Erasing logs\n");
DataFlash.EraseAll(); DataFlash.EraseAll();
gcs_send_text(MAV_SEVERITY_CRITICAL, "Log erase complete\n"); gcs_send_text(MAV_SEVERITY_INFO, "Log erase complete\n");
} }
#if AUTOTUNE_ENABLED == ENABLED #if AUTOTUNE_ENABLED == ENABLED
@ -807,12 +807,12 @@ void Copter::log_init(void)
{ {
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure)); DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
if (!DataFlash.CardInserted()) { if (!DataFlash.CardInserted()) {
gcs_send_text(MAV_SEVERITY_CRITICAL, "No dataflash inserted"); gcs_send_text(MAV_SEVERITY_NOTICE, "No dataflash inserted");
g.log_bitmask.set(0); g.log_bitmask.set(0);
} else if (DataFlash.NeedPrep()) { } else if (DataFlash.NeedPrep()) {
gcs_send_text(MAV_SEVERITY_CRITICAL, "Preparing log system"); gcs_send_text(MAV_SEVERITY_INFO, "Preparing log system");
DataFlash.Prep(); DataFlash.Prep();
gcs_send_text(MAV_SEVERITY_CRITICAL, "Prepared log system"); gcs_send_text(MAV_SEVERITY_INFO, "Prepared log system");
for (uint8_t i=0; i<num_gcs; i++) { for (uint8_t i=0; i<num_gcs; i++) {
gcs[i].reset_cli_timeout(); gcs[i].reset_cli_timeout();
} }

View File

@ -610,7 +610,7 @@ bool Copter::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
// check if timer has run out // check if timer has run out
if (((millis() - loiter_time) / 1000) >= loiter_time_max) { if (((millis() - loiter_time) / 1000) >= loiter_time_max) {
gcs_send_text_fmt("Reached Command #%i",cmd.index); gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached Command #%i",cmd.index);
return true; return true;
}else{ }else{
return false; return false;
@ -692,7 +692,7 @@ bool Copter::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
// check if timer has run out // check if timer has run out
if (((millis() - loiter_time) / 1000) >= loiter_time_max) { if (((millis() - loiter_time) / 1000) >= loiter_time_max) {
gcs_send_text_fmt("Reached Command #%i",cmd.index); gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached Command #%i",cmd.index);
return true; return true;
}else{ }else{
return false; return false;

View File

@ -95,13 +95,13 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan)
AP_Notify::flags.esc_calibration = true; AP_Notify::flags.esc_calibration = true;
// warn user we are starting calibration // warn user we are starting calibration
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "STARTING CALIBRATION"); gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "STARTING CALIBRATION");
// inform what type of compensation we are attempting // inform what type of compensation we are attempting
if (comp_type == AP_COMPASS_MOT_COMP_CURRENT) { if (comp_type == AP_COMPASS_MOT_COMP_CURRENT) {
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "CURRENT"); gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "CURRENT");
} else{ } else{
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "THROTTLE"); gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "THROTTLE");
} }
// disable throttle and battery failsafe // disable throttle and battery failsafe
@ -245,10 +245,10 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan)
} }
compass.save_motor_compensation(); compass.save_motor_compensation();
// display success message // display success message
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Calibration Successful!"); gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Calibration Successful!");
} else { } else {
// compensation vector never updated, report failure // compensation vector never updated, report failure
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Failed!"); gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_NOTICE, "Failed!");
compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED); compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED);
} }

View File

@ -1007,19 +1007,19 @@ void Copter::autotune_update_gcs(uint8_t message_id)
{ {
switch (message_id) { switch (message_id) {
case AUTOTUNE_MESSAGE_STARTED: case AUTOTUNE_MESSAGE_STARTED:
gcs_send_text(MAV_SEVERITY_CRITICAL,"AutoTune: Started"); gcs_send_text(MAV_SEVERITY_INFO,"AutoTune: Started");
break; break;
case AUTOTUNE_MESSAGE_STOPPED: case AUTOTUNE_MESSAGE_STOPPED:
gcs_send_text(MAV_SEVERITY_CRITICAL,"AutoTune: Stopped"); gcs_send_text(MAV_SEVERITY_INFO,"AutoTune: Stopped");
break; break;
case AUTOTUNE_MESSAGE_SUCCESS: case AUTOTUNE_MESSAGE_SUCCESS:
gcs_send_text(MAV_SEVERITY_CRITICAL,"AutoTune: Success"); gcs_send_text(MAV_SEVERITY_INFO,"AutoTune: Success");
break; break;
case AUTOTUNE_MESSAGE_FAILED: case AUTOTUNE_MESSAGE_FAILED:
gcs_send_text(MAV_SEVERITY_CRITICAL,"AutoTune: Failed"); gcs_send_text(MAV_SEVERITY_NOTICE,"AutoTune: Failed");
break; break;
case AUTOTUNE_MESSAGE_SAVED_GAINS: case AUTOTUNE_MESSAGE_SAVED_GAINS:
gcs_send_text(MAV_SEVERITY_CRITICAL,"AutoTune: Saved Gains"); gcs_send_text(MAV_SEVERITY_INFO,"AutoTune: Saved Gains");
break; break;
} }
} }

View File

@ -47,7 +47,7 @@ void Copter::crash_check()
// log an error in the dataflash // log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH); Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH);
// send message to gcs // send message to gcs
gcs_send_text(MAV_SEVERITY_CRITICAL,"Crash: Disarming"); gcs_send_text(MAV_SEVERITY_EMERGENCY,"Crash: Disarming");
// disarm motors // disarm motors
init_disarm_motors(); init_disarm_motors();
} }
@ -136,7 +136,7 @@ void Copter::parachute_check()
void Copter::parachute_release() void Copter::parachute_release()
{ {
// send message to gcs and dataflash // send message to gcs and dataflash
gcs_send_text(MAV_SEVERITY_CRITICAL,"Parachute: Released!"); gcs_send_text(MAV_SEVERITY_INFO,"Parachute: Released!");
Log_Write_Event(DATA_PARACHUTE_RELEASED); Log_Write_Event(DATA_PARACHUTE_RELEASED);
// disarm motors // disarm motors
@ -159,7 +159,7 @@ void Copter::parachute_manual_release()
// do not release if we are landed or below the minimum altitude above home // do not release if we are landed or below the minimum altitude above home
if (ap.land_complete) { if (ap.land_complete) {
// warn user of reason for failure // warn user of reason for failure
gcs_send_text(MAV_SEVERITY_CRITICAL,"Parachute: Landed"); gcs_send_text(MAV_SEVERITY_INFO,"Parachute: Landed");
// log an error in the dataflash // log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_PARACHUTE, ERROR_CODE_PARACHUTE_LANDED); Log_Write_Error(ERROR_SUBSYSTEM_PARACHUTE, ERROR_CODE_PARACHUTE_LANDED);
return; return;
@ -168,7 +168,7 @@ void Copter::parachute_manual_release()
// do not release if we are landed or below the minimum altitude above home // do not release if we are landed or below the minimum altitude above home
if ((parachute.alt_min() != 0 && (current_loc.alt < (int32_t)parachute.alt_min() * 100))) { if ((parachute.alt_min() != 0 && (current_loc.alt < (int32_t)parachute.alt_min() * 100))) {
// warn user of reason for failure // warn user of reason for failure
gcs_send_text(MAV_SEVERITY_CRITICAL,"Parachute: Too Low"); gcs_send_text(MAV_SEVERITY_ALERT,"Parachute: Too Low");
// log an error in the dataflash // log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_PARACHUTE, ERROR_CODE_PARACHUTE_TOO_LOW); Log_Write_Error(ERROR_SUBSYSTEM_PARACHUTE, ERROR_CODE_PARACHUTE_TOO_LOW);
return; return;

View File

@ -85,7 +85,7 @@ void Copter::esc_calibration_passthrough()
motors.set_update_rate(50); motors.set_update_rate(50);
// send message to GCS // send message to GCS
gcs_send_text(MAV_SEVERITY_CRITICAL,"ESC Calibration: passing pilot throttle to ESCs"); gcs_send_text(MAV_SEVERITY_INFO,"ESC Calibration: passing pilot throttle to ESCs");
while(1) { while(1) {
// arm motors // arm motors
@ -115,7 +115,7 @@ void Copter::esc_calibration_auto()
motors.set_update_rate(50); motors.set_update_rate(50);
// send message to GCS // send message to GCS
gcs_send_text(MAV_SEVERITY_CRITICAL,"ESC Calibration: auto calibration"); gcs_send_text(MAV_SEVERITY_INFO,"ESC Calibration: auto calibration");
// arm and enable motors // arm and enable motors
motors.armed(true); motors.armed(true);
@ -131,7 +131,7 @@ void Copter::esc_calibration_auto()
// wait for safety switch to be pressed // wait for safety switch to be pressed
while (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { while (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
if (!printed_msg) { if (!printed_msg) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"ESC Calibration: push safety switch"); gcs_send_text(MAV_SEVERITY_INFO,"ESC Calibration: push safety switch");
printed_msg = true; printed_msg = true;
} }
delay(10); delay(10);

View File

@ -159,7 +159,7 @@ void Copter::failsafe_battery_event(void)
set_failsafe_battery(true); set_failsafe_battery(true);
// warn the ground station and log to dataflash // warn the ground station and log to dataflash
gcs_send_text(MAV_SEVERITY_CRITICAL,"Low Battery!"); gcs_send_text(MAV_SEVERITY_WARNING,"Low Battery!");
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED); Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED);
} }

View File

@ -149,7 +149,7 @@ bool Copter::init_arm_motors(bool arming_from_gcs)
} }
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs_send_text(MAV_SEVERITY_CRITICAL, "ARMING MOTORS"); gcs_send_text(MAV_SEVERITY_INFO, "ARMING MOTORS");
#endif #endif
// Remember Orientation // Remember Orientation
@ -866,7 +866,7 @@ void Copter::init_disarm_motors()
} }
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs_send_text(MAV_SEVERITY_CRITICAL, "DISARMING MOTORS"); gcs_send_text(MAV_SEVERITY_INFO, "DISARMING MOTORS");
#endif #endif
// save compass offsets learned by the EKF // save compass offsets learned by the EKF
@ -936,7 +936,7 @@ void Copter::lost_vehicle_check()
if (soundalarm_counter >= LOST_VEHICLE_DELAY) { if (soundalarm_counter >= LOST_VEHICLE_DELAY) {
if (AP_Notify::flags.vehicle_lost == false) { if (AP_Notify::flags.vehicle_lost == false) {
AP_Notify::flags.vehicle_lost = true; AP_Notify::flags.vehicle_lost = true;
gcs_send_text(MAV_SEVERITY_CRITICAL,"Locate Copter Alarm!"); gcs_send_text(MAV_SEVERITY_NOTICE,"Locate Copter Alarm!");
} }
} else { } else {
soundalarm_counter++; soundalarm_counter++;

View File

@ -4,13 +4,13 @@
void Copter::init_barometer(bool full_calibration) void Copter::init_barometer(bool full_calibration)
{ {
gcs_send_text(MAV_SEVERITY_WARNING, "Calibrating barometer"); gcs_send_text(MAV_SEVERITY_NOTICE, "Calibrating barometer");
if (full_calibration) { if (full_calibration) {
barometer.calibrate(); barometer.calibrate();
}else{ }else{
barometer.update_calibration(); barometer.update_calibration();
} }
gcs_send_text(MAV_SEVERITY_WARNING, "barometer calibration complete"); gcs_send_text(MAV_SEVERITY_INFO, "barometer calibration complete");
} }
// return barometric altitude in centimeters // return barometric altitude in centimeters

View File

@ -597,7 +597,7 @@ void Copter::save_trim()
float pitch_trim = ToRad((float)channel_pitch->control_in/100.0f); float pitch_trim = ToRad((float)channel_pitch->control_in/100.0f);
ahrs.add_trim(roll_trim, pitch_trim); ahrs.add_trim(roll_trim, pitch_trim);
Log_Write_Event(DATA_SAVE_TRIM); Log_Write_Event(DATA_SAVE_TRIM);
gcs_send_text(MAV_SEVERITY_CRITICAL, "Trim saved"); gcs_send_text(MAV_SEVERITY_INFO, "Trim saved");
} }
// auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions // auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions