aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: send flight mode string to Notify

Browse Source
This commit is contained in:
Randy Mackay 2017-01-20 19:15:51 +09:00 committed by Lucas De Marchi
parent 26919b4ab6
commit fad2ba608f
2 changed files with 78 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/Plane.h
View File

@ -1064,6 +1064,7 @@ private:
bool start_command_callback(const AP_Mission::Mission_Command &cmd);
bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
void notify_flight_mode(enum FlightMode mode);
void run_cli(AP_HAL::UARTDriver *port);
void log_init();
void init_capabilities(void);

78
ArduPlane/system.cpp
View File

@ -324,7 +324,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
// don't switch modes if we are already in the correct mode.
return;
}
AP_Notify::flags.flight_mode = mode;
if(g.auto_trim > 0 && control_mode == MANUAL)
trim_control_surfaces();
@ -497,6 +497,9 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
if (should_log(MASK_LOG_MODE))
DataFlash.Log_Write_Mode(control_mode);
// update notify with flight mode change
notify_flight_mode(control_mode);
// reset steering integrator on mode change
steerController.reset_I();
}
@ -737,6 +740,79 @@ void Plane::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
}
}
// sets notify object flight mode information
void Plane::notify_flight_mode(enum FlightMode mode)
{
AP_Notify::flags.flight_mode = mode;
// set flight mode string
switch (mode) {
case MANUAL:
notify.set_flight_mode_str("MANU");
break;
case CIRCLE:
notify.set_flight_mode_str("CIRC");
break;
case STABILIZE:
notify.set_flight_mode_str("STAB");
break;
case TRAINING:
notify.set_flight_mode_str("TRAN");
break;
case ACRO:
notify.set_flight_mode_str("ACRO");
break;
case FLY_BY_WIRE_A:
notify.set_flight_mode_str("FBWA");
break;
case FLY_BY_WIRE_B:
notify.set_flight_mode_str("FBWB");
break;
case CRUISE:
notify.set_flight_mode_str("CRUS");
break;
case AUTOTUNE:
notify.set_flight_mode_str("ATUN");
break;
case AUTO:
notify.set_flight_mode_str("AUTO");
break;
case RTL:
notify.set_flight_mode_str("RTL ");
break;
case LOITER:
notify.set_flight_mode_str("LOITER");
break;
case AVOID_ADSB:
notify.set_flight_mode_str("AVOI");
break;
case GUIDED:
notify.set_flight_mode_str("GUID");
break;
case INITIALISING:
notify.set_flight_mode_str("INIT");
break;
case QSTABILIZE:
notify.set_flight_mode_str("QSTB");
break;
case QHOVER:
notify.set_flight_mode_str("QHOV");
break;
case QLOITER:
notify.set_flight_mode_str("QLOT");
break;
case QLAND:
notify.set_flight_mode_str("QLND");
break;
case QRTL:
notify.set_flight_mode_str("QRTL");
break;
default:
notify.set_flight_mode_str("----");
break;
}
}
#if CLI_ENABLED == ENABLED
void Plane::print_comma(void)
{