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

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
Rustom Jehangir 2016-03-13 11:16:44 -07:00
commit 0607107fc2
45 changed files with 414 additions and 245 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.gitignore vendored
View File

@ -29,6 +29,8 @@
.settings/
.autotools
.vagrant
.tags
.tags_sorted_by_file
/.lock-waf*
/.waf*
/tmp/*
@ -97,3 +99,6 @@ status.txt
tags
test.ArduCopter/*
Thumbs.db
cscope.in.out
cscope.out
cscope.po.out

1
APMrover2/GCS_Mavlink.cpp
View File

@ -868,7 +868,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
uint8_t result = MAV_RESULT_UNSUPPORTED;
// do command
send_text(MAV_SEVERITY_INFO,"Command received: ");
switch(packet.command) {

2
APMrover2/Log.cpp
View File

@ -375,7 +375,7 @@ void Rover::Log_Write_Home_And_Origin()
#if AP_AHRS_NAVEKF_AVAILABLE
// log ekf origin if set
Location ekf_orig;
if (ahrs.get_NavEKF_const().getOriginLLH(ekf_orig)) {
if (ahrs.get_origin(ekf_orig)) {
DataFlash.Log_Write_Origin(LogOriginType::ekf_origin, ekf_orig);
}
#endif

2
ArduCopter/Log.cpp
View File

@ -638,7 +638,7 @@ void Copter::Log_Write_Home_And_Origin()
{
// log ekf origin if set
Location ekf_orig;
if (ahrs.get_NavEKF_const().getOriginLLH(ekf_orig)) {
if (ahrs.get_origin(ekf_orig)) {
DataFlash.Log_Write_Origin(LogOriginType::ekf_origin, ekf_orig);
}

17
ArduCopter/Parameters.cpp
View File

@ -298,7 +298,7 @@ const AP_Param::Info Copter::var_info[] = {
// @User: Standard
// @Range: 300 700
// @Units: Percent*10
// @Increment: 1
// @Increment: 10
GSCALAR(throttle_mid, "THR_MID", THR_MID_DEFAULT),
// @Param: THR_DZ
@ -404,42 +404,42 @@ const AP_Param::Info Copter::var_info[] = {
// @Param: CH7_OPT
// @DisplayName: Channel 7 option
// @Description: Select which function if performed when CH7 is above 1800 pwm
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
// @User: Standard
GSCALAR(ch7_option, "CH7_OPT", AUXSW_DO_NOTHING),
// @Param: CH8_OPT
// @DisplayName: Channel 8 option
// @Description: Select which function if performed when CH8 is above 1800 pwm
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
// @User: Standard
GSCALAR(ch8_option, "CH8_OPT", AUXSW_DO_NOTHING),
// @Param: CH9_OPT
// @DisplayName: Channel 9 option
// @Description: Select which function if performed when CH9 is above 1800 pwm
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
// @User: Standard
GSCALAR(ch9_option, "CH9_OPT", AUXSW_DO_NOTHING),
// @Param: CH10_OPT
// @DisplayName: Channel 10 option
// @Description: Select which function if performed when CH10 is above 1800 pwm
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
// @User: Standard
GSCALAR(ch10_option, "CH10_OPT", AUXSW_DO_NOTHING),
// @Param: CH11_OPT
// @DisplayName: Channel 11 option
// @Description: Select which function if performed when CH11 is above 1800 pwm
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
// @User: Standard
GSCALAR(ch11_option, "CH11_OPT", AUXSW_DO_NOTHING),
// @Param: CH12_OPT
// @DisplayName: Channel 12 option
// @Description: Select which function if performed when CH12 is above 1800 pwm
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
// @User: Standard
GSCALAR(ch12_option, "CH12_OPT", AUXSW_DO_NOTHING),
@ -471,7 +471,7 @@ const AP_Param::Info Copter::var_info[] = {
// @DisplayName: RC Feel Roll/Pitch
// @Description: RC feel for roll/pitch which controls vehicle response to user input with 0 being extremely soft and 100 being crisp
// @Range: 0 100
// @Increment: 1
// @Increment: 10
// @User: Standard
// @Values: 0:Very Soft, 25:Soft, 50:Medium, 75:Crisp, 100:Very Crisp
GSCALAR(rc_feel_rp, "RC_FEEL_RP", RC_FEEL_RP_MEDIUM),
@ -782,6 +782,7 @@ const AP_Param::Info Copter::var_info[] = {
// @DisplayName: Throttle acceleration controller P gain
// @Description: Throttle acceleration controller P gain. Converts the difference between desired vertical acceleration and actual acceleration into a motor output
// @Range: 0.500 1.500
// @Increment: 0.05
// @User: Standard
// @Param: ACCEL_Z_I

3
ArduCopter/crash_check.cpp
View File

@ -144,6 +144,9 @@ void Copter::parachute_release()
// release parachute
parachute.release();
// deploy landing gear
landinggear.set_cmd_mode(LandingGear_Deploy);
}
// parachute_manual_release - trigger the release of the parachute, after performing some checks for pilot error

3
ArduCopter/defines.h
View File

@ -68,7 +68,8 @@ enum aux_sw_func {
AUXSW_BRAKE = 33, // Brake flight mode
AUXSW_RELAY2 = 34, // Relay2 pin on/off (in Mission planner set CH8_OPT = 34)
AUXSW_RELAY3 = 35, // Relay3 pin on/off (in Mission planner set CH9_OPT = 35)
AUXSW_RELAY4 = 36 // Relay4 pin on/off (in Mission planner set CH10_OPT = 36)
AUXSW_RELAY4 = 36, // Relay4 pin on/off (in Mission planner set CH10_OPT = 36)
AUXSW_THROW = 37 // change to THROW flight mode
};
// Frame types

12
ArduCopter/switches.cpp
View File

@ -589,6 +589,18 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
}
}
break;
case AUXSW_THROW:
// throw flight mode
if (ch_flag == AUX_SWITCH_HIGH) {
set_mode(THROW);
} else {
// return to flight mode switch's flight mode if we are currently in throw mode
if (control_mode == THROW) {
reset_control_switch();
}
}
break;
}
}

13
ArduPlane/ArduPlane.cpp
View File

@ -544,10 +544,7 @@ void Plane::update_flight_mode(void)
}
// ensure we are fly-forward
if (effective_mode == QSTABILIZE ||
effective_mode == QHOVER ||
effective_mode == QLOITER ||
quadplane.in_vtol_auto()) {
if (quadplane.in_vtol_mode()) {
ahrs.set_fly_forward(false);
} else {
ahrs.set_fly_forward(true);
@ -709,7 +706,8 @@ void Plane::update_flight_mode(void)
case QSTABILIZE:
case QHOVER:
case QLOITER: {
case QLOITER:
case QLAND: {
// set nav_roll and nav_pitch using sticks
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
@ -789,6 +787,7 @@ void Plane::update_navigation()
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
// nothing to do
break;
}
@ -936,9 +935,7 @@ void Plane::update_flight_stage(void)
} else {
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
} else if (control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
} else if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL);
} else {

10
ArduPlane/Attitude.cpp
View File

@ -143,6 +143,7 @@ void Plane::stabilize_stick_mixing_direct()
control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
control_mode == QLAND ||
control_mode == TRAINING) {
return;
}
@ -165,6 +166,7 @@ void Plane::stabilize_stick_mixing_fbw()
control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
control_mode == QLAND ||
control_mode == TRAINING ||
(control_mode == AUTO && g.auto_fbw_steer)) {
return;
@ -364,7 +366,8 @@ void Plane::stabilize()
stabilize_acro(speed_scaler);
} else if (control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER) {
control_mode == QLOITER ||
control_mode == QLAND) {
quadplane.control_run();
} else {
if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) {
@ -994,10 +997,7 @@ void Plane::set_servos(void)
guided_throttle_passthru) {
// manual pass through of throttle while in GUIDED
channel_throttle->radio_out = channel_throttle->radio_in;
} else if (control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
quadplane.in_vtol_auto()) {
} else if (quadplane.in_vtol_mode()) {
// no forward throttle for now
channel_throttle->servo_out = 0;
channel_throttle->calc_pwm();

3
ArduPlane/GCS_Mavlink.cpp
View File

@ -42,6 +42,7 @@ void Plane::send_heartbeat(mavlink_channel_t chan)
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
case CRUISE:
base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
break;
@ -178,6 +179,7 @@ void Plane::send_extended_status1(mavlink_channel_t chan)
case AUTOTUNE:
case QSTABILIZE:
case QHOVER:
case QLAND:
case QLOITER:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
@ -1153,7 +1155,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
uint8_t result = MAV_RESULT_UNSUPPORTED;
// do command
send_text(MAV_SEVERITY_INFO,"Command received: ");
switch(packet.command) {

2
ArduPlane/Log.cpp
View File

@ -471,7 +471,7 @@ void Plane::Log_Write_Home_And_Origin()
#if AP_AHRS_NAVEKF_AVAILABLE
// log ekf origin if set
Location ekf_orig;
if (ahrs.get_NavEKF_const().getOriginLLH(ekf_orig)) {
if (ahrs.get_origin(ekf_orig)) {
DataFlash.Log_Write_Origin(LogOriginType::ekf_origin, ekf_orig);
}
#endif

1
ArduPlane/Plane.h
View File

@ -204,6 +204,7 @@ private:
float initial_correction;
uint32_t last_correction_time_ms;
uint8_t in_range_count;
float height_estimate;
} rangefinder_state;
#endif

9
ArduPlane/altitude.cpp
View File

@ -565,7 +565,7 @@ void Plane::rangefinder_height_update(void)
}
// correct the range for attitude (multiply by DCM.c.z, which
// is cos(roll)*cos(pitch))
height_estimate = distance * ahrs.get_rotation_body_to_ned().c.z;
rangefinder_state.height_estimate = distance * ahrs.get_rotation_body_to_ned().c.z;
// we consider ourselves to be fully in range when we have 10
// good samples (0.2s) that are different by 5% of the maximum
@ -580,11 +580,12 @@ void Plane::rangefinder_height_update(void)
rangefinder_state.in_range = true;
if (!rangefinder_state.in_use &&
(flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) &&
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL ||
control_mode == QLAND) &&
g.rangefinder_landing) {
rangefinder_state.in_use = true;
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)height_estimate);
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)rangefinder_state.height_estimate);
}
}
} else {

6
ArduPlane/commands_logic.cpp
View File

@ -619,7 +619,7 @@ bool Plane::verify_loiter_time()
update_loiter(0);
if (loiter.start_time_ms == 0) {
if (nav_controller->reached_loiter_target()) {
if (nav_controller->reached_loiter_target() && loiter.sum_cd > 1) {
// we've reached the target, start the timer
loiter.start_time_ms = millis();
}
@ -649,7 +649,7 @@ bool Plane::verify_loiter_turns()
if (condition_value != 0) {
// primary goal, loiter time
if (loiter.sum_cd > loiter.total_cd) {
if (loiter.sum_cd > loiter.total_cd && loiter.sum_cd > 1) {
// primary goal completed, initialize secondary heading goal
condition_value = 0;
result = verify_loiter_heading(true);
@ -678,7 +678,7 @@ bool Plane::verify_loiter_to_alt()
//has target altitude been reached?
if (condition_value != 0) {
// primary goal, loiter alt
if (labs(condition_value - current_loc.alt) < 500) {
if (labs(condition_value - current_loc.alt) < 500 && loiter.sum_cd > 1) {
// primary goal completed, initialize secondary heading goal
condition_value = 0;
result = verify_loiter_heading(true);

3
ArduPlane/defines.h
View File

@ -66,7 +66,8 @@ enum FlightMode {
INITIALISING = 16,
QSTABILIZE = 17,
QHOVER = 18,
QLOITER = 19
QLOITER = 19,
QLAND = 20
};
// type of stick mixing enabled

10
ArduPlane/events.cpp
View File

@ -29,9 +29,10 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
case QSTABILIZE:
case QLOITER:
case QHOVER:
failsafe.saved_mode = control_mode;
failsafe.saved_mode_set = 1;
set_mode(QHOVER);
set_mode(QLAND);
break;
case AUTO:
@ -50,7 +51,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
case CIRCLE:
case RTL:
case QHOVER:
case QLAND:
default:
break;
}
@ -87,8 +88,9 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
break;
case QSTABILIZE:
case QHOVER:
case QLOITER:
set_mode(QHOVER);
set_mode(QLAND);
break;
case AUTO:
@ -106,7 +108,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
break;
case RTL:
case QHOVER:
case QLAND:
default:
break;
}

10
ArduPlane/navigation.cpp
View File

@ -6,6 +6,11 @@
void Plane::set_nav_controller(void)
{
switch ((AP_Navigation::ControllerType)g.nav_controller.get()) {
default:
case AP_Navigation::CONTROLLER_DEFAULT:
// fall through to L1 as default controller
case AP_Navigation::CONTROLLER_L1:
nav_controller = &L1_controller;
break;
@ -29,7 +34,10 @@ void Plane::loiter_angle_update(void)
{
int32_t target_bearing_cd = nav_controller->target_bearing_cd();
int32_t loiter_delta_cd;
if (loiter.sum_cd == 0) {
if (loiter.sum_cd == 0 && !nav_controller->reached_loiter_target()) {
// we don't start summing until we are doing the real loiter
loiter_delta_cd = 0;
} else if (loiter.sum_cd == 0) {
// use 1 cd for initial delta
loiter_delta_cd = 1;
} else {

61
ArduPlane/quadplane.cpp
View File

@ -320,7 +320,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
// @User: Standard
// @Range: 300 700
// @Units: Percent*10
// @Increment: 1
// @Increment: 10
AP_GROUPINFO("THR_MID", 28, QuadPlane, throttle_mid, 500),
// @Param: TRAN_PIT_MAX
@ -569,6 +569,14 @@ void QuadPlane::init_loiter(void)
init_throttle_wait();
}
void QuadPlane::init_land(void)
{
init_loiter();
throttle_wait = false;
land_state = QLAND_DESCEND;
motors_lower_limit_start_ms = 0;
}
// helper for is_flying()
bool QuadPlane::is_flying(void)
@ -644,8 +652,23 @@ void QuadPlane::control_loiter()
plane.nav_roll_cd = wp_nav->get_roll();
plane.nav_pitch_cd = wp_nav->get_pitch();
// update altitude target and call position controller
pos_control->set_alt_target_from_climb_rate_ff(get_pilot_desired_climb_rate_cms(), plane.G_Dt, false);
if (plane.control_mode == QLAND) {
if (land_state == QLAND_DESCEND) {
if (plane.g.rangefinder_landing && plane.rangefinder_state.in_range) {
if (plane.rangefinder_state.height_estimate < land_final_alt) {
land_state = QLAND_FINAL;
}
} else if (plane.adjusted_relative_altitude_cm() < land_final_alt*100) {
land_state = QLAND_FINAL;
}
}
float descent_rate = (land_state == QLAND_FINAL)?land_speed_cms:wp_nav->get_speed_down();
pos_control->set_alt_target_from_climb_rate(-descent_rate, plane.G_Dt, true);
check_land_complete();
} else {
// update altitude target and call position controller
pos_control->set_alt_target_from_climb_rate_ff(get_pilot_desired_climb_rate_cms(), plane.G_Dt, false);
}
pos_control->update_z_controller();
}
@ -848,12 +871,7 @@ void QuadPlane::update(void)
return;
}
bool quad_mode = (plane.control_mode == QSTABILIZE ||
plane.control_mode == QHOVER ||
plane.control_mode == QLOITER ||
in_vtol_auto());
if (!quad_mode) {
if (!in_vtol_mode()) {
update_transition();
} else {
assisted_flight = false;
@ -898,8 +916,8 @@ void QuadPlane::control_run(void)
control_hover();
break;
case QLOITER:
case QLAND:
control_loiter();
break;
default:
break;
}
@ -931,6 +949,9 @@ bool QuadPlane::init_mode(void)
case QLOITER:
init_loiter();
break;
case QLAND:
init_land();
break;
default:
break;
}
@ -997,6 +1018,7 @@ bool QuadPlane::in_vtol_mode(void)
return (plane.control_mode == QSTABILIZE ||
plane.control_mode == QHOVER ||
plane.control_mode == QLOITER ||
plane.control_mode == QLAND ||
in_vtol_auto());
}
@ -1220,6 +1242,17 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd)
return true;
}
void QuadPlane::check_land_complete(void)
{
if (land_state == QLAND_FINAL &&
(motors_lower_limit_start_ms != 0 &&
millis() - motors_lower_limit_start_ms > 5000)) {
plane.disarm_motors();
land_state = QLAND_COMPLETE;
plane.gcs_send_text(MAV_SEVERITY_INFO,"Land complete");
}
}
/*
check if a VTOL landing has completed
*/
@ -1247,12 +1280,6 @@ bool QuadPlane::verify_vtol_land(const AP_Mission::Mission_Command &cmd)
plane.gcs_send_text(MAV_SEVERITY_INFO,"Land final started");
}
if (land_state == QLAND_FINAL &&
(motors_lower_limit_start_ms != 0 &&
millis() - motors_lower_limit_start_ms > 5000)) {
plane.disarm_motors();
land_state = QLAND_COMPLETE;
plane.gcs_send_text(MAV_SEVERITY_INFO,"Land complete");
}
check_land_complete();
return false;
}

2
ArduPlane/quadplane.h
View File

@ -118,7 +118,9 @@ private:
void control_hover(void);
void init_loiter(void);
void init_land(void);
void control_loiter(void);
void check_land_complete(void);
float assist_climb_rate_cms(void);

14
ArduPlane/system.cpp
View File

@ -461,6 +461,7 @@ void Plane::set_mode(enum FlightMode mode)
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
if (!quadplane.init_mode()) {
control_mode = previous_mode;
} else {
@ -505,6 +506,7 @@ bool Plane::mavlink_set_mode(uint8_t mode)
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
set_mode((enum FlightMode)mode);
return true;
}
@ -695,6 +697,18 @@ void Plane::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
case GUIDED:
port->print("Guided");
break;
case QSTABILIZE:
port->print("QStabilize");
break;
case QHOVER:
port->print("QHover");
break;
case QLOITER:
port->print("QLoiter");
break;
case QLAND:
port->print("QLand");
break;
default:
port->printf("Mode(%u)", (unsigned)mode);
break;

14
Tools/ardupilotwaf/boards.py
View File

@ -1,6 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
from collections import OrderedDict
import sys
import waflib
@ -26,10 +27,15 @@ class Board:
# Dictionaries (like 'DEFINES') are converted to lists to
# conform to waf conventions.
if isinstance(val, dict):
for item in val.items():
cfg.env.prepend_value(k, '%s=%s' % item)
else:
keys = list(val.keys())
if not isinstance(val, OrderedDict):
keys.sort()
val = ['%s=%s' % (vk, val[vk]) for vk in keys]
if k in cfg.env and isinstance(cfg.env, list):
cfg.env.prepend_value(k, val)
else:
cfg.env[k] = val
def configure_env(self, env):
# Use a dictionary instead of the convetional list for definitions to
@ -207,7 +213,7 @@ class bebop(linux):
env.DEFINES.update(
CONFIG_HAL_BOARD_SUBTYPE = 'HAL_BOARD_SUBTYPE_LINUX_BEBOP',
)
env.STATIC_LINKING = [True]
env.STATIC_LINKING = True
class raspilot(linux):
def configure_env(self, env):

35
Tools/scripts/build_autotest.sh
View File

@ -87,9 +87,11 @@ report_pull_failure() {
oldhash=$(cd APM && git rev-parse HEAD)
echo "Updating APM"
pushd APM
git checkout -f master
git fetch origin
git submodule update
git reset --hard origin/master
git pull || report_pull_failure
git clean -f -f -x -d -d
@ -99,39 +101,6 @@ popd
rsync -a APM/Tools/autotest/web-firmware/ buildlogs/binaries/
pushd PX4Firmware
git fetch origin
git reset --hard origin/master
for v in ArduPlane ArduCopter APMrover2; do
git tag -d $v-beta || true
git tag -d $v-stable || true
done
git fetch origin --tags
git show
popd
pushd PX4NuttX
git fetch origin
git reset --hard origin/master
for v in ArduPlane ArduCopter APMrover2; do
git tag -d $v-beta || true
git tag -d $v-stable || true
done
git fetch origin --tags
git show
popd
pushd uavcan
git fetch origin
git reset --hard origin/master
for v in ArduPlane ArduCopter APMrover2; do
git tag -d $v-beta || true
git tag -d $v-stable || true
done
git fetch origin --tags
git show
popd
echo "Updating pymavlink"
pushd mavlink/pymavlink
git fetch origin

12
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -34,7 +34,7 @@ AP_AHRS_NavEKF::AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gp
AP_AHRS_DCM(ins, baro, gps),
EKF1(_EKF1),
EKF2(_EKF2),
_flags(flags)
_ekf_flags(flags)
{
_dcm_matrix.identity();
}
@ -713,9 +713,15 @@ AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const
#endif
}
/*
fixed wing and rover when in fly_forward mode will fall back to
DCM if the EKF doesn't have GPS. This is the safest option as
DCM is very robust
*/
if (ret != EKF_TYPE_NONE &&
(_vehicle_class == AHRS_VEHICLE_FIXED_WING ||
_vehicle_class == AHRS_VEHICLE_GROUND)) {
(_vehicle_class == AHRS_VEHICLE_FIXED_WING ||
_vehicle_class == AHRS_VEHICLE_GROUND) &&
_flags.fly_forward) {
nav_filter_status filt_state;
if (ret == EKF_TYPE1) {
EKF1.getFilterStatus(filt_state);

4
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -217,7 +217,7 @@ private:
EKF_TYPE active_EKF_type(void) const;
bool always_use_EKF() const {
return _flags & FLAG_ALWAYS_USE_EKF;
return _ekf_flags & FLAG_ALWAYS_USE_EKF;
}
NavEKF &EKF1;
@ -232,7 +232,7 @@ private:
Vector3f _accel_ef_ekf_blended;
const uint16_t startup_delay_ms = 1000;
uint32_t start_time_ms = 0;
Flags _flags;
Flags _ekf_flags;
uint8_t ekf_type(void) const;
void update_DCM(void);

75
libraries/AP_BoardConfig/AP_BoardConfig.cpp
View File

@ -71,10 +71,10 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = {
AP_GROUPINFO("SAFETYENABLE", 3, AP_BoardConfig, _safety_enable, 1),
// @Param: SBUS_OUT
// @DisplayName: Enable use of SBUS output
// @Description: Enabling this option on a Pixhawk enables SBUS servo output from the SBUS output connector
// @Values: 0:Disabled,1:Enabled
AP_GROUPINFO("SBUS_OUT", 4, AP_BoardConfig, _sbus_out_enable, 0),
// @DisplayName: SBUS output rate
// @Description: This sets the SBUS output frame rate in Hz
// @Values: 0:Disabled,1:50Hz,2:75Hz,3:100Hz,4:150Hz,5:200Hz,6:250Hz,7:300Hz
AP_GROUPINFO("SBUS_OUT", 4, AP_BoardConfig, _sbus_out_rate, 0),
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#endif
@ -99,6 +99,12 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = {
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 && !defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
extern "C" int uavcan_main(int argc, const char *argv[]);
#define _UAVCAN_IOCBASE (0x4000) // IOCTL base for module UAVCAN
#define _UAVCAN_IOC(_n) (_IOC(_UAVCAN_IOCBASE, _n))
#define UAVCAN_IOCG_NODEID_INPROGRESS _UAVCAN_IOC(1) // query if node identification is in progress
#endif
void AP_BoardConfig::init()
@ -131,18 +137,44 @@ void AP_BoardConfig::init()
hal.rcout->force_safety_off();
}
if (_sbus_out_enable.get() == 1) {
if (_sbus_out_rate.get() >= 1) {
fd = open("/dev/px4io", 0);
if (fd == -1 || ioctl(fd, SBUS_SET_PROTO_VERSION, 1) != 0) {
hal.console->printf("SBUS: Unable to setup SBUS output\n");
}
if (fd != -1) {
if (fd == -1) {
hal.console->printf("SBUS: Unable to open px4io for sbus\n");
} else {
static const struct {
uint8_t value;
uint16_t rate;
} rates[] = {
{ 1, 50 },
{ 2, 75 },
{ 3, 100 },
{ 4, 150 },
{ 5, 200 },
{ 6, 250 },
{ 7, 300 }
};
uint16_t rate = 300;
for (uint8_t i=0; i<ARRAY_SIZE(rates); i++) {
if (rates[i].value == _sbus_out_rate) {
rate = rates[i].rate;
}
}
for (uint8_t tries=0; tries<10; tries++) {
if (ioctl(fd, SBUS_SET_PROTO_VERSION, 1) != 0) {
continue;
}
if (ioctl(fd, PWM_SERVO_SET_SBUS_RATE, rate) != 0) {
continue;
}
break;
}
close(fd);
}
}
}
#if !defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
if (_can_enable == 1) {
if (_can_enable >= 1) {
const char *args[] = { "uavcan", "start", NULL };
int ret = uavcan_main(3, args);
if (ret != 0) {
@ -150,9 +182,28 @@ void AP_BoardConfig::init()
} else {
hal.console->printf("UAVCAN: started\n");
// give some time for CAN bus initialisation
hal.scheduler->delay(500);
hal.scheduler->delay(1500);
}
}
if (_can_enable >= 2) {
const char *args[] = { "uavcan", "start", "fw", NULL };
int ret = uavcan_main(4, args);
if (ret != 0) {
hal.console->printf("UAVCAN: failed to start servers\n");
} else {
fd = open("/dev/uavcan/esc", 0); // design flaw of uavcan driver, this should be /dev/uavcan/node one day
if (fd == -1) {
AP_HAL::panic("Configuration invalid - unable to open /dev/uavcan/esc");
}
// delay startup, UAVCAN still discovering nodes
while (ioctl(fd, UAVCAN_IOCG_NODEID_INPROGRESS,0) == OK) {
hal.scheduler->delay(500);
}
hal.console->printf("UAVCAN: node discovery complete\n");
close(fd);
}
}
#endif
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN

2
libraries/AP_BoardConfig/AP_BoardConfig.h
View File

@ -28,7 +28,7 @@ private:
AP_Int8 _ser1_rtscts;
AP_Int8 _ser2_rtscts;
AP_Int8 _safety_enable;
AP_Int8 _sbus_out_enable;
AP_Int8 _sbus_out_rate;
#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1
AP_Int8 _can_enable;
#endif

85
libraries/AP_Compass/AP_Compass_HMC5843.h
View File

@ -15,43 +15,7 @@ class AP_HMC5843_SerialBus;
class AP_Compass_HMC5843 : public AP_Compass_Backend
{
private:
static AP_Compass_Backend *_detect(Compass &compass,
AP_HMC5843_SerialBus *bus);
AP_HMC5843_SerialBus *_bus;
AP_HAL::Semaphore *_bus_sem = nullptr;
float _scaling[3] = {0};
bool _initialised;
bool read_raw(void);
uint8_t _base_config;
bool re_initialise(void);
bool read_register(uint8_t address, uint8_t *value);
bool write_register(uint8_t address, uint8_t value);
bool _calibrate(uint8_t calibration_gain,
uint16_t expected_x,
uint16_t expected_yz);
bool _detect_version();
uint32_t _retry_time; // when unhealthy the millis() value to retry at
int16_t _mag_x;
int16_t _mag_y;
int16_t _mag_z;
int16_t _mag_x_accum;
int16_t _mag_y_accum;
int16_t _mag_z_accum;
uint8_t _accum_count;
uint32_t _last_accum_time;
uint8_t _compass_instance;
uint8_t _product_id;
float _gain_multiple;
public:
// detect the sensor
static AP_Compass_Backend *detect_i2c(Compass &compass,
AP_HAL::I2CDriver *i2c);
static AP_Compass_Backend *detect_mpu6000(Compass &compass);
@ -59,9 +23,46 @@ public:
AP_Compass_HMC5843(Compass &compass, AP_HMC5843_SerialBus *bus);
~AP_Compass_HMC5843();
bool init(void);
void read(void);
void accumulate(void);
bool init();
void read();
void accumulate();
private:
static AP_Compass_Backend *_detect(Compass &compass,
AP_HMC5843_SerialBus *bus);
bool read_raw();
bool re_initialise();
bool read_register(uint8_t address, uint8_t *value);
bool write_register(uint8_t address, uint8_t value);
bool _calibrate(uint8_t calibration_gain, uint16_t expected_x,
uint16_t expected_yz);
bool _detect_version();
AP_HMC5843_SerialBus *_bus;
AP_HAL::Semaphore *_bus_sem;
float _scaling[3];
float _gain_multiple;
uint32_t _last_accum_time;
// when unhealthy the millis() value to retry at
uint32_t _retry_time;
int16_t _mag_x;
int16_t _mag_y;
int16_t _mag_z;
int16_t _mag_x_accum;
int16_t _mag_y_accum;
int16_t _mag_z_accum;
uint8_t _accum_count;
uint8_t _base_config;
uint8_t _compass_instance;
uint8_t _product_id;
bool _initialised;
};
class AP_HMC5843_SerialBus
@ -115,7 +116,7 @@ public:
bool start_measurements() override;
private:
AuxiliaryBus *_bus = nullptr;
AuxiliaryBusSlave *_slave = nullptr;
bool _started = false;
AuxiliaryBus *_bus;
AuxiliaryBusSlave *_slave;
bool _started;
};

18
libraries/AP_Compass/Compass.cpp
View File

@ -346,7 +346,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("CAL_FIT", 30, Compass, _calibration_threshold, 8.0f),
AP_GROUPEND
};
@ -473,22 +473,22 @@ void Compass::_detect_backends(void)
}
}
void
void
Compass::accumulate(void)
{
{
for (uint8_t i=0; i< _backend_count; i++) {
// call accumulate on each of the backend
_backends[i]->accumulate();
}
}
bool
bool
Compass::read(void)
{
for (uint8_t i=0; i< _backend_count; i++) {
// call read on each of the backend. This call updates field[i]
_backends[i]->read();
}
}
for (uint8_t i=0; i < COMPASS_MAX_INSTANCES; i++) {
_state[i].healthy = (AP_HAL::millis() - _state[i].last_update_ms < 500);
}
@ -623,12 +623,12 @@ Compass::get_declination() const
calculate a compass heading given the attitude from DCM and the mag vector
*/
float
Compass::calculate_heading(const Matrix3f &dcm_matrix) const
Compass::calculate_heading(const Matrix3f &dcm_matrix, uint8_t i) const
{
float cos_pitch_sq = 1.0f-(dcm_matrix.c.x*dcm_matrix.c.x);
// Tilt compensated magnetic field Y component:
const Vector3f &field = get_field();
const Vector3f &field = get_field(i);
float headY = field.y * dcm_matrix.c.z - field.z * dcm_matrix.c.y;
@ -741,7 +741,7 @@ void Compass::setHIL(uint8_t instance, const Vector3f &mag)
_state[instance].last_update_usec = AP_HAL::micros();
}
const Vector3f& Compass::getHIL(uint8_t instance) const
const Vector3f& Compass::getHIL(uint8_t instance) const
{
return _hil.field[instance];
}
@ -751,7 +751,7 @@ void Compass::_setup_earth_field(void)
{
// assume a earth field strength of 400
_hil.Bearth(400, 0, 0);
// rotate _Bearth for inclination and declination. -66 degrees
// is the inclination in Canberra, Australia
Matrix3f R;

5
libraries/AP_Compass/Compass.h
View File

@ -82,7 +82,10 @@ public:
///
/// @returns heading in radians
///
float calculate_heading(const Matrix3f &dcm_matrix) const;
float calculate_heading(const Matrix3f &dcm_matrix) const {
return calculate_heading(dcm_matrix, get_primary());
}
float calculate_heading(const Matrix3f &dcm_matrix, uint8_t i) const;
/// Sets offset x/y/z values.
///

114
libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.cpp
View File

@ -16,13 +16,14 @@ void setup() {
hal.console->println("Compass library test");
if (!compass.init()) {
hal.console->println("compass initialisation failed!");
while (1) ;
AP_HAL::panic("compass initialisation failed!");
}
hal.console->printf("init done - %u compasses detected\n", compass.get_count());
compass.set_and_save_offsets(0,0,0,0); // set offsets to account for surrounding interference
compass.set_declination(ToRad(0.0f)); // set local difference between magnetic north and true north
// set offsets to account for surrounding interference
compass.set_and_save_offsets(0, 0, 0, 0);
// set local difference between magnetic north and true north
compass.set_declination(ToRad(0.0f));
hal.scheduler->delay(1000);
timer = AP_HAL::micros();
@ -30,66 +31,69 @@ void setup() {
void loop()
{
static float min[3], max[3], offset[3];
static const uint8_t compass_count = compass.get_count();
static float min[COMPASS_MAX_INSTANCES][3];
static float max[COMPASS_MAX_INSTANCES][3];
static float offset[COMPASS_MAX_INSTANCES][3];
compass.accumulate();
if((AP_HAL::micros()- timer) > 100000L)
{
if ((AP_HAL::micros() - timer) > 100000L) {
timer = AP_HAL::micros();
compass.read();
unsigned long read_time = AP_HAL::micros() - timer;
float heading;
if (!compass.healthy()) {
hal.console->println("not healthy");
return;
for (uint8_t i = 0; i < compass_count; i++) {
float heading;
hal.console->printf("Compass #%u: ", i);
if (!compass.healthy()) {
hal.console->println("not healthy");
continue;
}
Matrix3f dcm_matrix;
// use roll = 0, pitch = 0 for this example
dcm_matrix.from_euler(0, 0, 0);
heading = compass.calculate_heading(dcm_matrix, i);
compass.learn_offsets();
const Vector3f &mag = compass.get_field(i);
// capture min
min[i][0] = MIN(mag.x, min[i][0]);
min[i][1] = MIN(mag.y, min[i][1]);
min[i][2] = MIN(mag.z, min[i][2]);
// capture max
max[i][0] = MAX(mag.x, max[i][0]);
max[i][1] = MAX(mag.y, max[i][1]);
max[i][2] = MAX(mag.z, max[i][2]);
// calculate offsets
offset[i][0] = -(max[i][0] + min[i][0]) / 2;
offset[i][1] = -(max[i][1] + min[i][1]) / 2;
offset[i][2] = -(max[i][2] + min[i][2]) / 2;
// display all to user
hal.console->printf("Heading: %.2f (%3d,%3d,%3d) i2c error: %u",
ToDeg(heading),
(int)mag.x,
(int)mag.y,
(int)mag.z,
(unsigned)hal.i2c->lockup_count());
// display offsets
hal.console->printf(" offsets(%.2f, %.2f, %.2f)",
offset[i][0], offset[i][1], offset[i][2]);
hal.console->printf(" t=%u", (unsigned)read_time);
hal.console->println();
}
Matrix3f dcm_matrix;
// use roll = 0, pitch = 0 for this example
dcm_matrix.from_euler(0, 0, 0);
heading = compass.calculate_heading(dcm_matrix);
compass.learn_offsets();
// capture min
const Vector3f &mag = compass.get_field();
if( mag.x < min[0] )
min[0] = mag.x;
if( mag.y < min[1] )
min[1] = mag.y;
if( mag.z < min[2] )
min[2] = mag.z;
// capture max
if( mag.x > max[0] )
max[0] = mag.x;
if( mag.y > max[1] )
max[1] = mag.y;
if( mag.z > max[2] )
max[2] = mag.z;
// calculate offsets
offset[0] = -(max[0]+min[0])/2;
offset[1] = -(max[1]+min[1])/2;
offset[2] = -(max[2]+min[2])/2;
// display all to user
hal.console->printf("Heading: %.2f (%3d,%3d,%3d) i2c error: %u",
ToDeg(heading),
(int)mag.x,
(int)mag.y,
(int)mag.z,
(unsigned)hal.i2c->lockup_count());
// display offsets
hal.console->printf(" offsets(%.2f, %.2f, %.2f)",
offset[0], offset[1], offset[2]);
hal.console->printf(" t=%u", (unsigned)read_time);
hal.console->println();
} else {
hal.scheduler->delay(1);
hal.scheduler->delay(1);
}
}

1
libraries/AP_HAL/AP_HAL_Boards.h
View File

@ -355,6 +355,7 @@
#define HAL_BARO_MS5611_I2C_ADDR 0x77
#define HAL_BARO_MS5611_USE_TIMER true
#define HAL_INS_DEFAULT HAL_INS_BH
#define HAL_INS_MPU9250_NAME "mpu9250"
#define HAL_INS_MPU60x0_I2C_BUS 1
#define HAL_INS_MPU60x0_I2C_ADDR 0x69
#define HAL_COMPASS_DEFAULT HAL_COMPASS_BH

8
libraries/AP_HAL/RCOutput.h
View File

@ -67,10 +67,16 @@ public:
virtual void push() { }
/* Read back current output state, as either single channel or
* array of channels. */
* array of channels. On boards that have a separate IO controller,
* this returns the latest output value that the IO controller has
* reported */
virtual uint16_t read(uint8_t ch) = 0;
virtual void read(uint16_t* period_us, uint8_t len) = 0;
/* Read the current input state. This returns the last value that was written. */
virtual uint16_t read_last_sent(uint8_t ch) { return read(ch); }
virtual void read_last_sent(uint16_t* period_us, uint8_t len) { read(period_us, len); };
/*
set PWM to send to a set of channels when the safety switch is
in the safe state

16
libraries/AP_HAL_PX4/RCOutput.cpp
View File

@ -285,6 +285,22 @@ void PX4RCOutput::read(uint16_t* period_us, uint8_t len)
}
}
uint16_t PX4RCOutput::read_last_sent(uint8_t ch)
{
if (ch >= PX4_NUM_OUTPUT_CHANNELS) {
return 0;
}
return _period[ch];
}
void PX4RCOutput::read_last_sent(uint16_t* period_us, uint8_t len)
{
for (uint8_t i=0; i<len; i++) {
period_us[i] = read_last_sent(i);
}
}
/*
update actuator armed state
*/

2
libraries/AP_HAL_PX4/RCOutput.h
View File

@ -20,6 +20,8 @@ public:
void write(uint8_t ch, uint16_t period_us) override;
uint16_t read(uint8_t ch) override;
void read(uint16_t* period_us, uint8_t len) override;
uint16_t read_last_sent(uint8_t ch) override;
void read_last_sent(uint16_t* period_us, uint8_t len) override;
void set_safety_pwm(uint32_t chmask, uint16_t period_us) override;
void set_failsafe_pwm(uint32_t chmask, uint16_t period_us) override;
bool force_safety_on(void) override;

3
libraries/AP_Navigation/AP_Navigation.h
View File

@ -106,7 +106,8 @@ public:
// select which navigation controller to use by setting the
// NAV_CONTROLLER parameter
enum ControllerType {
CONTROLLER_L1 = 1
CONTROLLER_DEFAULT = 0,
CONTROLLER_L1 = 1
};
};

3
libraries/AP_Notify/AP_Notify.cpp
View File

@ -91,7 +91,8 @@ struct AP_Notify::notify_events_type AP_Notify::events;
ToneAlarm_Linux tonealarm;
NotifyDevice *AP_Notify::_devices[] = {&toshibaled, &tonealarm};
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
NotifyDevice *AP_Notify::_devices[0];
RCOutputRGBLedOff led(15, 13, 14, 255);
NotifyDevice *AP_Notify::_devices[] = { &led };
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI
AP_BoardLED boardled;

18
libraries/AP_Notify/RCOutputRGBLed.h
View File

@ -10,10 +10,26 @@ public:
RCOutputRGBLed(uint8_t red_channel, uint8_t green_channel,
uint8_t blue_channel);
bool hw_init();
bool hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue);
virtual bool hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue);
private:
uint8_t _red_channel;
uint8_t _green_channel;
uint8_t _blue_channel;
};
class RCOutputRGBLedOff : public RCOutputRGBLed {
public:
RCOutputRGBLedOff(uint8_t red_channel, uint8_t green_channel,
uint8_t blue_channel, uint8_t led_off)
: RCOutputRGBLed(red_channel, green_channel, blue_channel,
led_off, led_off, led_off, led_off)
{ }
/* Override the hw_set_rgb method to turn leds off regardless of the
* values passed */
bool hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) override
{
return RCOutputRGBLed::hw_set_rgb(_led_off, _led_off, _led_off);
}
};

8
libraries/AP_OpticalFlow/AP_OpticalFlow_Onboard.cpp
View File

@ -22,7 +22,11 @@
(CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP ||\
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE ||\
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI)
//#define FLOWONBOARD_DEBUG 1
#ifndef OPTICALFLOW_ONBOARD_DEBUG
#define OPTICALFLOW_ONBOARD_DEBUG 0
#endif
#define OPTICALFLOW_ONBOARD_ID 1
extern const AP_HAL::HAL& hal;
@ -83,7 +87,7 @@ void AP_OpticalFlow_Onboard::update()
// copy results to front end
_update_frontend(state);
#if FLOWONBOARD_DEBUG
#if OPTICALFLOW_ONBOARD_DEBUG
hal.console->printf("FLOW_ONBOARD qual:%u FlowRateX:%4.2f Y:%4.2f"
"BodyRateX:%4.2f Y:%4.2f, delta_time = %u\n",
(unsigned)state.surface_quality,

6
libraries/AP_TECS/AP_TECS.cpp
View File

@ -626,10 +626,12 @@ void AP_TECS::_update_throttle(void)
// Constrain throttle demand
_throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf);
float THRminf_clipped_to_zero = constrain_float(_THRminf, 0, _THRmaxf);
// Rate limit PD + FF throttle
// Calculate the throttle increment from the specified slew time
if (aparm.throttle_slewrate != 0) {
float thrRateIncr = _DT * (_THRmaxf - _THRminf) * aparm.throttle_slewrate * 0.01f;
float thrRateIncr = _DT * (_THRmaxf - THRminf_clipped_to_zero) * aparm.throttle_slewrate * 0.01f;
_throttle_dem = constrain_float(_throttle_dem,
_last_throttle_dem - thrRateIncr,
@ -640,7 +642,7 @@ void AP_TECS::_update_throttle(void)
// Calculate integrator state upper and lower limits
// Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand
// Additionally constrain the integrator state amplitude so that the integrator comes off limits faster.
float maxAmp = 0.5f*(_THRmaxf - _THRminf);
float maxAmp = 0.5f*(_THRmaxf - THRminf_clipped_to_zero);
float integ_max = constrain_float((_THRmaxf - _throttle_dem + 0.1f),-maxAmp,maxAmp);
float integ_min = constrain_float((_THRminf - _throttle_dem - 0.1f),-maxAmp,maxAmp);

33
mk/check_modules.sh
View File

@ -2,7 +2,7 @@
echo "Checking modules"
MODULE_LIST="PX4Firmware PX4NuttX uavcan mavlink"
MODULE_LIST="PX4Firmware PX4NuttX mavlink uavcan uavcan/dsdl uavcan/libuavcan/dsdl_compiler/pyuavcan"
NEED_INIT=0
@ -25,24 +25,29 @@ done
set -x
git submodule init || {
echo "git submodule init failed"
git submodule status
git submodule status --recursive
exit 1
}
git submodule update || {
(cd modules/uavcan && git submodule init) || {
echo "init of uavcan failed"
git submodule status --recursive
exit 1
}
git submodule update --recursive || {
echo "git submodule update failed"
git submodule status
git submodule status --recursive
exit 1
}
cat <<EOF
==============================
git submodules are initialised
Please see http://dev.ardupilot.com/wiki/git-submodules/
Please restart the build
==============================
EOF
exit 1
for m in $MODULE_LIST; do
[ -d modules/$m ] || {
echo "modules/$m missing - failed module init"
exit 1
}
[ -f modules/$m/.git ] || {
echo "modules/$m/.git missing - failed module init"
exit 1
}
done
}
for m in $MODULE_LIST; do

2
modules/PX4Firmware

@ -1 +1 @@
Subproject commit 62d935eb1665a74ca9a40ea1438121a594027327
Subproject commit db3f70da0437152ff6bb479292da2cd80f69394b

2
modules/PX4NuttX

@ -1 +1 @@
Subproject commit d48fa3072b2396c489966c4ab10183ac7cf3dea9
Subproject commit a5146c6805057dd51386b64439c8872c68630429

2
modules/uavcan

@ -1 +1 @@
Subproject commit 6dd432c9742c22e1dd1638c7f91cf937e4bdb2f1
Subproject commit 7ce96d6c1e5aeb82fa90ade26a49552c122a984b

2
wscript
View File

@ -114,7 +114,7 @@ def configure(cfg):
])
if cfg.options.submodule_update:
cfg.env.SUBMODULE_UPDATE = [True]
cfg.env.SUBMODULE_UPDATE = True
def collect_dirs_to_recurse(bld, globs, **kw):
dirs = []