Skip to content

Commit

Permalink
Sub: Update to match upstream, part 1
Browse files Browse the repository at this point in the history
  • Loading branch information
@rjehangir @tridge
rjehangir authored and tridge committed Feb 21, 2017
1 parent e0d3eba commit 684bc24
Show file tree
Hide file tree
Showing 27 changed files with 441 additions and 521 deletions.
12 changes: 6 additions & 6 deletions ArduSub/ArduSub.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -388,9 +388,9 @@ void Sub::ten_hz_logging_loop()
Log_Write_Attitude();
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() );
DataFlash.Log_Write_PID(LOG_PIDY_MSG, g.pid_rate_yaw.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDY_MSG, gattitude_control.get_rate_yaw_pid().get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() );
}
}
Expand Down Expand Up @@ -431,9 +431,9 @@ void Sub::fifty_hz_logging_loop()
Log_Write_Attitude();
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() );
DataFlash.Log_Write_PID(LOG_PIDY_MSG, g.pid_rate_yaw.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() );
}
}
Expand Down
38 changes: 19 additions & 19 deletions ArduSub/Attitude.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -100,7 +100,7 @@ void Sub::update_thr_average()
{
// ensure throttle_average has been initialised
if( is_zero(throttle_average) ) {
throttle_average = g.throttle_mid;
throttle_average = 0.5f;
// update position controller
pos_control.set_throttle_hover(throttle_average);
}
Expand All @@ -114,7 +114,7 @@ void Sub::update_thr_average()
float throttle = motors.get_throttle();

// calc average throttle if we are in a level hover
if (throttle > g.throttle_min && abs(climb_rate) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) {
if (throttle > 0.0f && abs(climb_rate) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) {
throttle_average = throttle_average * 0.99f + throttle * 0.01f;
// update position controller
pos_control.set_throttle_hover(throttle_average);
Expand All @@ -134,33 +134,34 @@ void Sub::set_throttle_takeoff()
// get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick
// used only for manual throttle modes
// returns throttle output 0 to 1000
int16_t Sub::get_pilot_desired_throttle(int16_t throttle_control)
float Sub::get_pilot_desired_throttle(int16_t throttle_control)
{
int16_t throttle_out;
float throttle_out;

int16_t mid_stick = channel_throttle->get_control_mid();

// ensure reasonable throttle values
throttle_control = constrain_int16(throttle_control,0,1000);
// ensure mid throttle is set within a reasonable range
g.throttle_mid = constrain_int16(g.throttle_mid,g.throttle_min+50,700);
float thr_mid = MAX(0,g.throttle_mid-g.throttle_min) / (float)(1000-g.throttle_min);

// check throttle is above, below or in the deadband
if (throttle_control < mid_stick) {
// below the deadband
throttle_out = g.throttle_min + ((float)(throttle_control-g.throttle_min))*((float)(g.throttle_mid - g.throttle_min))/((float)(mid_stick-g.throttle_min));
throttle_out = ((float)throttle_control)*thr_mid/(float)mid_stick;
}else if(throttle_control > mid_stick) {
// above the deadband
throttle_out = g.throttle_mid + ((float)(throttle_control-mid_stick)) * (float)(1000-g.throttle_mid) / (float)(1000-mid_stick);
throttle_out = (thr_mid) + ((float)(throttle_control-mid_stick)) * (1.0f - thr_mid) / (float)(1000-mid_stick);
}else{
// must be in the deadband
throttle_out = g.throttle_mid;
throttle_out = thr_mid;
}

return throttle_out;
}

// get_pilot_desired_climb_rate - transform pilot's throttle input to
// climb rate in cm/s. we use radio_in instead of control_in to get the full range
// get_pilot_desired_climb_rate - transform pilot's throttle input to climb rate in cm/s
// without any deadzone at the bottom
float Sub::get_pilot_desired_climb_rate(float throttle_control)
{
Expand All @@ -175,15 +176,15 @@ float Sub::get_pilot_desired_climb_rate(float throttle_control)
float deadband_bottom = mid_stick - g.throttle_deadzone;

// ensure a reasonable throttle value
throttle_control = constrain_float(throttle_control,g.throttle_min,1000.0f);
throttle_control = constrain_float(throttle_control,0.0f,1000.0f);

// ensure a reasonable deadzone
g.throttle_deadzone = constrain_int16(g.throttle_deadzone, 0, 400);

// check throttle is above, below or in the deadband
if (throttle_control < deadband_bottom) {
// below the deadband
desired_rate = g.pilot_velocity_z_max * (throttle_control-deadband_bottom) / (deadband_bottom-g.throttle_min);
desired_rate = g.pilot_velocity_z_max * (throttle_control-deadband_bottom) / deadband_bottom;
}else if (throttle_control > deadband_top) {
// above the deadband
desired_rate = g.pilot_velocity_z_max * (throttle_control-deadband_top) / (1000.0f-deadband_top);
Expand All @@ -201,32 +202,31 @@ float Sub::get_pilot_desired_climb_rate(float throttle_control)
// get_non_takeoff_throttle - a throttle somewhere between min and mid throttle which should not lead to a takeoff
float Sub::get_non_takeoff_throttle()
{
return (g.throttle_mid / 2.0f);
return (((float)g.throttle_mid/1000.0f)/2.0f);
}

float Sub::get_takeoff_trigger_throttle()
{
return channel_throttle->get_control_mid() + g.takeoff_trigger_dz;
}

// get_throttle_pre_takeoff - convert pilot's input throttle to a throttle output before take-off
// get_throttle_pre_takeoff - convert pilot's input throttle to a throttle output (in the range 0 to 1) before take-off
// used only for althold, loiter, hybrid flight modes
// returns throttle output 0 to 1000
float Sub::get_throttle_pre_takeoff(float input_thr)
{
// exit immediately if input_thr is zero
if (input_thr <= 0.0f) {
return 0.0f;
}

// TODO: does this parameter sanity check really belong here?
g.throttle_mid = constrain_int16(g.throttle_mid,g.throttle_min+50,700);
// ensure reasonable throttle values
input_thr = constrain_float(input_thr,0.0f,1000.0f);

float in_min = g.throttle_min;
float in_min = 0.0f;
float in_max = get_takeoff_trigger_throttle();

// multicopters will output between spin-when-armed and 1/2 of mid throttle
float out_min = motors.get_throttle_warn();
float out_min = 0.0f;
float out_max = get_non_takeoff_throttle();

if ((g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0) {
Expand Down Expand Up @@ -283,7 +283,7 @@ float Sub::get_surface_tracking_climb_rate(int16_t target_rate, float current_al
void Sub::set_accel_throttle_I_from_pilot_throttle(int16_t pilot_throttle)
{
// shift difference between pilot's throttle and hover throttle into accelerometer I
g.pid_accel_z.set_integrator(pilot_throttle-throttle_average);
g.pid_accel_z.set_integrator((pilot_throttle-throttle_average) * 1000.0f);
}

// updates position controller's maximum altitude using fence and EKF limits
Expand Down
8 changes: 4 additions & 4 deletions ArduSub/GCS_Mavlink.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -389,7 +389,7 @@ void NOINLINE Sub::send_vfr_hud(mavlink_channel_t chan)
gps.ground_speed(),
gps.ground_speed(),
(ahrs.yaw_sensor / 100) % 360,
(int16_t)(motors.get_throttle())/10,
(int16_t)(motors.get_throttle() * 100),
current_loc.alt / 100.0f,
climb_rate / 100.0f);
}
Expand Down Expand Up @@ -434,7 +434,7 @@ void Sub::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 = g.pid_rate_roll.get_pid_info();
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,
pid_info.desired*0.01f,
degrees(gyro.x),
Expand All @@ -447,7 +447,7 @@ void Sub::send_pid_tuning(mavlink_channel_t chan)
}
}
if (g.gcs_pid_mask & 2) {
const DataFlash_Class::PID_Info &pid_info = g.pid_rate_pitch.get_pid_info();
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,
pid_info.desired*0.01f,
degrees(gyro.y),
Expand All @@ -460,7 +460,7 @@ void Sub::send_pid_tuning(mavlink_channel_t chan)
}
}
if (g.gcs_pid_mask & 4) {
const DataFlash_Class::PID_Info &pid_info = g.pid_rate_yaw.get_pid_info();
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,
pid_info.desired*0.01f,
degrees(gyro.z),
Expand Down
14 changes: 7 additions & 7 deletions ArduSub/Log.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -298,8 +298,8 @@ void Sub::Log_Write_Nav_Tuning()
struct PACKED log_Control_Tuning {
LOG_PACKET_HEADER;
uint64_t time_us;
int16_t throttle_in;
int16_t angle_boost;
float throttle_in;
float angle_boost;
float throttle_out;
float desired_alt;
float inav_alt;
Expand All @@ -316,7 +316,7 @@ void Sub::Log_Write_Control_Tuning()
struct log_Control_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
time_us : AP_HAL::micros64(),
throttle_in : channel_throttle->control_in,
throttle_in : attitude_control.get_throttle_in(),
angle_boost : attitude_control.angle_boost(),
throttle_out : motors.get_throttle(),
desired_alt : pos_control.get_alt_target() / 100.0f,
Expand Down Expand Up @@ -624,8 +624,8 @@ void Sub::Log_Sensor_Health()
struct PACKED log_Heli {
LOG_PACKET_HEADER;
uint64_t time_us;
int16_t desired_rotor_speed;
int16_t main_rotor_speed;
float desired_rotor_speed;
float main_rotor_speed;
};

// precision landing logging
Expand Down Expand Up @@ -683,7 +683,7 @@ const struct LogStructure Sub::log_structure[] = {
{ LOG_NAV_TUNING_MSG, sizeof(log_Nav_Tuning),
"NTUN", "Qffffffffff", "TimeUS,DPosX,DPosY,PosX,PosY,DVelX,DVelY,VelX,VelY,DAccX,DAccY" },
{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
"CTUN", "Qhhfffecchh", "TimeUS,ThrIn,AngBst,ThrOut,DAlt,Alt,BarAlt,DSAlt,SAlt,DCRt,CRt" },
"CTUN", "Qfffffecchh", "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_MOTBATT_MSG, sizeof(log_MotBatt),
Expand All @@ -705,7 +705,7 @@ const struct LogStructure Sub::log_structure[] = {
{ LOG_ERROR_MSG, sizeof(log_Error),
"ERR", "QBB", "TimeUS,Subsys,ECode" },
{ LOG_HELI_MSG, sizeof(log_Heli),
"HELI", "Qhh", "TimeUS,DRRPM,ERRPM" },
"HELI", "Qff", "TimeUS,DRRPM,ERRPM" },
{ LOG_PRECLAND_MSG, sizeof(log_Precland),
"PL", "QBffffff", "TimeUS,Heal,bX,bY,eX,eY,pX,pY" },
};
Expand Down
Loading

0 comments on commit 684bc24

Please sign in to comment.