Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: Match Copter changes

This commit is contained in:
Rustom Jehangir 2016-04-03 10:19:09 -07:00 committed by Andrew Tridgell
parent 802f4f617f
commit e0d3eba5a4
11 changed files with 37 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduSub/ArduSub.cpp
View File

@ -386,7 +386,7 @@ void Sub::ten_hz_logging_loop()
// log attitude data if we're not already logging at the higher rate
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_Attitude();
Log_Write_Rate();
DataFlash.Log_Write_Rate(ahrs, motors, attitude_control, pos_control);
if (should_log(MASK_LOG_PID)) {
DataFlash.Log_Write_PID(LOG_PIDR_MSG, g.pid_rate_roll.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDP_MSG, g.pid_rate_pitch.get_pid_info() );
@ -429,7 +429,7 @@ void Sub::fifty_hz_logging_loop()
#if HIL_MODE == HIL_MODE_DISABLED
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_Attitude();
Log_Write_Rate();
DataFlash.Log_Write_Rate(ahrs, motors, attitude_control, pos_control);
if (should_log(MASK_LOG_PID)) {
DataFlash.Log_Write_PID(LOG_PIDR_MSG, g.pid_rate_roll.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDP_MSG, g.pid_rate_pitch.get_pid_info() );

9
ArduSub/GCS_Mavlink.cpp
View File

@ -1847,10 +1847,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
#if PRECISION_LANDING == ENABLED
case MAVLINK_MSG_ID_LANDING_TARGET:
// configure or release parachute
result = MAV_RESULT_ACCEPTED;
sub.precland.handle_msg(msg);
case MAVLINK_MSG_ID_LANDING_TARGET:
// configure or release parachute
result = MAV_RESULT_ACCEPTED;
copter.precland.handle_msg(msg);
break;
#endif
#if CAMERA == ENABLED

44
ArduSub/Log.cpp
View File

@ -376,47 +376,6 @@ void Sub::Log_Write_Attitude()
DataFlash.Log_Write_POS(ahrs);
}
struct PACKED log_Rate {
LOG_PACKET_HEADER;
uint64_t time_us;
float control_roll;
float roll;
float roll_out;
float control_pitch;
float pitch;
float pitch_out;
float control_yaw;
float yaw;
float yaw_out;
float control_accel;
float accel;
float accel_out;
};
// Write an rate packet
void Sub::Log_Write_Rate()
{
const Vector3f &rate_targets = attitude_control.rate_bf_targets();
const Vector3f &accel_target = pos_control.get_accel_target();
struct log_Rate pkt_rate = {
LOG_PACKET_HEADER_INIT(LOG_RATE_MSG),
time_us : AP_HAL::micros64(),
control_roll : (float)rate_targets.x,
roll : (float)(ahrs.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100),
roll_out : motors.get_roll(),
control_pitch : (float)rate_targets.y,
pitch : (float)(ahrs.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100),
pitch_out : motors.get_pitch(),
control_yaw : (float)rate_targets.z,
yaw : (float)(ahrs.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100),
yaw_out : motors.get_yaw(),
control_accel : (float)accel_target.z,
accel : (float)(-(ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f),
accel_out : motors.get_throttle()
};
DataFlash.WriteBlock(&pkt_rate, sizeof(pkt_rate));
}
struct PACKED log_MotBatt {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -727,8 +686,6 @@ const struct LogStructure Sub::log_structure[] = {
"CTUN", "Qhhfffecchh", "TimeUS,ThrIn,AngBst,ThrOut,DAlt,Alt,BarAlt,DSAlt,SAlt,DCRt,CRt" },
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance),
"PM", "QHHIhBH", "TimeUS,NLon,NLoop,MaxT,PMT,I2CErr,INSErr" },
{ LOG_RATE_MSG, sizeof(log_Rate),
"RATE", "Qffffffffffff", "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut" },
{ LOG_MOTBATT_MSG, sizeof(log_MotBatt),
"MOTB", "Qffff", "TimeUS,LiftMax,BatVolt,BatRes,ThLimit" },
{ LOG_STARTUP_MSG, sizeof(log_Startup),
@ -830,7 +787,6 @@ void Sub::Log_Write_Nav_Tuning() {}
void Sub::Log_Write_Control_Tuning() {}
void Sub::Log_Write_Performance() {}
void Sub::Log_Write_Attitude(void) {}
void Sub::Log_Write_Rate() {}
void Sub::Log_Write_MotBatt() {}
void Sub::Log_Write_Startup() {}
void Sub::Log_Write_Event(uint8_t id) {}

11
ArduSub/Parameters.cpp
View File

@ -249,6 +249,15 @@ const AP_Param::Info Sub::var_info[] = {
// @User: Standard
GSCALAR(land_speed, "LAND_SPEED", LAND_SPEED),
// @Param: LAND_SPEED_HIGH
// @DisplayName: Land speed high
// @Description: The descent speed for the first stage of landing in cm/s. If this is zero then WPNAV_SPEED_DN is used
// @Units: cm/s
// @Range: 0 500
// @Increment: 10
// @User: Standard
GSCALAR(land_speed_high, "LAND_SPEED_HIGH", 0),
// @Param: PILOT_VELZ_MAX
// @DisplayName: Pilot maximum vertical speed
// @Description: The maximum vertical velocity the pilot may request in cm/s
@ -938,7 +947,7 @@ const AP_Param::Info Sub::var_info[] = {
GOBJECT(landinggear, "LGR_", AP_LandingGear),
// @Group: COMPASS_
// @Path: ../libraries/AP_Compass/Compass.cpp
// @Path: ../libraries/AP_Compass/AP_Compass.cpp
GOBJECT(compass, "COMPASS_", Compass),
// @Group: INS_

2
ArduSub/Parameters.h
View File

@ -321,6 +321,7 @@ public:
k_param_land_speed,
k_param_auto_velocity_z_min, // remove
k_param_auto_velocity_z_max, // remove - 219
k_param_land_speed_high,
//
// 220: PI/D Controllers
@ -426,6 +427,7 @@ public:
//
AP_Int32 rtl_loiter_time;
AP_Int16 land_speed;
AP_Int16 land_speed_high;
AP_Int16 pilot_velocity_z_max; // maximum vertical velocity the pilot may request
AP_Int16 pilot_accel_z; // vertical acceleration the pilot may request

1
ArduSub/Sub.h
View File

@ -624,7 +624,6 @@ private:
void Log_Write_Control_Tuning();
void Log_Write_Performance();
void Log_Write_Attitude();
void Log_Write_Rate();
void Log_Write_MotBatt();
void Log_Write_Startup();
void Log_Write_Event(uint8_t id);

2
ArduSub/control_autotune.cpp
View File

@ -516,7 +516,7 @@ void Copter::autotune_attitude_control()
// log this iterations lean angle and rotation rate
Log_Write_AutoTuneDetails(lean_angle, rotation_rate);
Log_Write_Rate();
DataFlash.Log_Write_Rate(ahrs, motors, attitude_control, pos_control);
break;
case AUTOTUNE_STEP_UPDATE_GAINS:

4
ArduSub/control_land.cpp
View File

@ -207,6 +207,10 @@ float Sub::get_land_descent_speed()
#endif
// if we are above 10m and the sonar does not sense anything perform regular alt hold descent
if (pos_control.get_pos_target().z <= pv_alt_above_origin(LAND_START_ALT) && !(sonar_ok && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
if (g.land_speed_high > 0) {
// user has asked for a different landing speed than normal descent rate
return -abs(g.land_speed_high);
}
return pos_control.get_speed_up();
}else{
return abs(g.land_speed);

6
ArduSub/control_throw.cpp
View File

@ -17,11 +17,7 @@ bool Sub::throw_init(bool ignore_checks)
}
// this mode needs a position reference
if (position_ok()) {
return true;
} else {
return false;
}
return true;
}
// clean up when exiting throw mode

1
ArduSub/defines.h
View File

@ -268,7 +268,6 @@ enum ThrowModeState {
#define LOG_DATA_FLOAT_MSG 0x18
#define LOG_AUTOTUNE_MSG 0x19
#define LOG_AUTOTUNEDETAILS_MSG 0x1A
#define LOG_RATE_MSG 0x1D
#define LOG_MOTBATT_MSG 0x1E
#define LOG_PARAMTUNE_MSG 0x1F
#define LOG_HELI_MSG 0x20

15
ArduSub/wscript
View File

@ -8,6 +8,7 @@ def build(bld):
vehicle=vehicle,
libraries=bld.ap_common_vehicle_libraries() + [
'AC_AttitudeControl',
'AC_InputManager',
'AC_Fence',
'AC_PID',
'AC_PrecLand',
@ -33,7 +34,15 @@ def build(bld):
use='mavlink',
)
bld.ap_program(
program_name='ardusub',
use=vehicle + '_libs',
frames = (
'bluerov','vectored','vectored6DOF',
)
for frame in frames:
frame_config = frame.upper().replace('-','_') + '_FRAME'
bld.ap_program(
program_name='ardusub-%s' % frame,
program_groups=['bin', 'sub'],
use=vehicle + '_libs',
defines=['FRAME_CONFIG=%s' % frame_config],
)