Browse Source

Copter: add nullptr checks for get_frame_string

fix rebase errors
c415-sdk
Mark Whitehorn 4 years ago committed by Peter Barker
parent
commit
f405454aba
  1. 1
      ArduCopter/Copter.h
  2. 3
      ArduCopter/GCS_Copter.cpp
  3. 7
      ArduCopter/GCS_Mavlink.cpp
  4. 2
      ArduCopter/Log.cpp
  5. 68
      ArduCopter/system.cpp

1
ArduCopter/Copter.h

@ -863,7 +863,6 @@ private: @@ -863,7 +863,6 @@ private:
bool ekf_alt_ok() const;
void update_auto_armed();
bool should_log(uint32_t mask);
MAV_TYPE get_frame_mav_type();
const char* get_frame_string() const;
void allocate_motors(void);
bool is_tradheli() const;

3
ArduCopter/GCS_Copter.cpp

@ -9,6 +9,9 @@ uint8_t GCS_Copter::sysid_this_mav() const @@ -9,6 +9,9 @@ uint8_t GCS_Copter::sysid_this_mav() const
const char* GCS_Copter::frame_string() const
{
if (copter.motors == nullptr) {
return "motors not allocated";
}
return copter.motors->get_frame_string();
}

7
ArduCopter/GCS_Mavlink.cpp

@ -548,7 +548,12 @@ bool GCS_MAVLINK_Copter::params_ready() const @@ -548,7 +548,12 @@ bool GCS_MAVLINK_Copter::params_ready() const
void GCS_MAVLINK_Copter::send_banner()
{
GCS_MAVLINK::send_banner();
send_text(MAV_SEVERITY_INFO, "Frame: %s", copter.motors->get_frame_string());
if (copter.motors == nullptr) {
send_text(MAV_SEVERITY_INFO, "motors not allocated");
return;
}
send_text(MAV_SEVERITY_INFO, "Frame: %s/%s", copter.motors->get_frame_string(),
copter.motors->get_type_string());
}
// a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes

2
ArduCopter/Log.cpp

@ -617,7 +617,7 @@ const struct LogStructure Copter::log_structure[] = { @@ -617,7 +617,7 @@ const struct LogStructure Copter::log_structure[] = {
void Copter::Log_Write_Vehicle_Startup_Messages()
{
// only 200(?) bytes are guaranteed by AP_Logger
logger.Write_MessageF("Frame: %s, Type: %s", motors->get_frame_string(), motors->get_type_string());
logger.Write_MessageF("Frame: %s/%s", motors->get_frame_string(), motors->get_type_string());
logger.Write_Mode((uint8_t)control_mode, control_mode_reason);
ahrs.Log_Write_Home_And_Origin();
gps.Write_AP_Logger_Log_Startup_messages();

68
ArduCopter/system.cpp

@ -448,74 +448,6 @@ bool Copter::should_log(uint32_t mask) @@ -448,74 +448,6 @@ bool Copter::should_log(uint32_t mask)
#endif
}
// return string corresponding to frame_class
const char* Copter::get_frame_string() const
{
switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) {
case AP_Motors::MOTOR_FRAME_QUAD:
return "QUAD";
case AP_Motors::MOTOR_FRAME_HEXA:
return "HEXA";
case AP_Motors::MOTOR_FRAME_Y6:
return "Y6";
case AP_Motors::MOTOR_FRAME_OCTA:
return "OCTA";
case AP_Motors::MOTOR_FRAME_OCTAQUAD:
return "OCTA_QUAD";
case AP_Motors::MOTOR_FRAME_HELI:
return "HELI";
case AP_Motors::MOTOR_FRAME_HELI_DUAL:
return "HELI_DUAL";
case AP_Motors::MOTOR_FRAME_HELI_QUAD:
return "HELI_QUAD";
case AP_Motors::MOTOR_FRAME_TRI:
return "TRI";
case AP_Motors::MOTOR_FRAME_SINGLE:
return "SINGLE";
case AP_Motors::MOTOR_FRAME_COAX:
return "COAX";
case AP_Motors::MOTOR_FRAME_TAILSITTER:
return "TAILSITTER";
case AP_Motors::MOTOR_FRAME_DODECAHEXA:
return "DODECA_HEXA";
case AP_Motors::MOTOR_FRAME_DECA:
return "DECA";
case AP_Motors::MOTOR_FRAME_UNDEFINED:
default:
return "UNKNOWN";
}
// return MAV_TYPE corresponding to frame class
MAV_TYPE Copter::get_frame_mav_type()
{
switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) {
case AP_Motors::MOTOR_FRAME_QUAD:
case AP_Motors::MOTOR_FRAME_UNDEFINED:
return MAV_TYPE_QUADROTOR;
case AP_Motors::MOTOR_FRAME_HEXA:
case AP_Motors::MOTOR_FRAME_Y6:
return MAV_TYPE_HEXAROTOR;
case AP_Motors::MOTOR_FRAME_OCTA:
case AP_Motors::MOTOR_FRAME_OCTAQUAD:
return MAV_TYPE_OCTOROTOR;
case AP_Motors::MOTOR_FRAME_HELI:
case AP_Motors::MOTOR_FRAME_HELI_DUAL:
case AP_Motors::MOTOR_FRAME_HELI_QUAD:
return MAV_TYPE_HELICOPTER;
case AP_Motors::MOTOR_FRAME_TRI:
return MAV_TYPE_TRICOPTER;
case AP_Motors::MOTOR_FRAME_SINGLE:
case AP_Motors::MOTOR_FRAME_COAX:
case AP_Motors::MOTOR_FRAME_TAILSITTER:
return MAV_TYPE_COAXIAL;
case AP_Motors::MOTOR_FRAME_DODECAHEXA:
return MAV_TYPE_DODECAROTOR;
case AP_Motors::MOTOR_FRAME_DECA:
return MAV_TYPE_DECAROTOR;
}
// unknown frame so return generic
return MAV_TYPE_GENERIC;
}
/*
allocate the motors class
*/

Loading…
Cancel
Save